From 02edd6d2cbd664d1bab19e49f35460d39240ff0a Mon Sep 17 00:00:00 2001 From: Dmitry Anisimov Date: Mon, 27 Sep 2021 15:10:54 +0200 Subject: [PATCH] more precise sphere fitting and sorting --- .../Least_squares_sphere_fit_region.h | 40 +++--- .../Least_squares_sphere_fit_sorting.h | 50 ++----- .../Region_growing/internal/utils.h | 130 ++++++++++++------ 3 files changed, 114 insertions(+), 106 deletions(-) diff --git a/Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_region.h b/Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_region.h index d9e1b28b191..89a4e163f30 100644 --- a/Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_region.h +++ b/Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_region.h @@ -173,10 +173,8 @@ namespace Point_set { np, internal_np::normal_map), NormalMap())), m_traits(parameters::choose_parameter(parameters::get_parameter( np, internal_np::geom_traits), GeomTraits())), - m_squared_length_3(m_traits.compute_squared_length_3_object()), - m_squared_distance_3(m_traits.compute_squared_distance_3_object()), - m_scalar_product_3(m_traits.compute_scalar_product_3_object()), - m_sqrt(Get_sqrt::sqrt_object(m_traits)) { + m_sqrt(Get_sqrt::sqrt_object(m_traits)), + m_squared_distance_3(m_traits.compute_squared_distance_3_object()) { CGAL_precondition(input_range.size() > 0); const FT max_distance = parameters::choose_parameter( @@ -327,7 +325,7 @@ namespace Point_set { } // TODO: Why do we get so many nan in this class? - if (std::isnan(m_radius)) { + if (std::isnan(CGAL::to_double(m_radius))) { return false; } @@ -341,7 +339,7 @@ namespace Point_set { Vector_3 normal = get(m_normal_map, key); const FT sq_dist = m_squared_distance_3(query_point, m_center); - if (std::isnan(sq_dist)) return false; + if (std::isnan(CGAL::to_double(sq_dist))) return false; const FT distance_to_center = m_sqrt(sq_dist); const FT distance_to_sphere = CGAL::abs(distance_to_center - m_radius); @@ -350,13 +348,13 @@ namespace Point_set { } const FT sq_norm = normal * normal; - if (std::isnan(sq_norm)) return false; - normal = normal / m_sqrt (sq_norm); + if (std::isnan(CGAL::to_double(sq_norm))) return false; + normal = normal / m_sqrt(sq_norm); Vector_3 ray(m_center, query_point); const FT sq_ray = ray * ray; - if (std::isnan(sq_ray)) return false; - ray = ray / m_sqrt (sq_ray); + if (std::isnan(CGAL::to_double(sq_ray))) return false; + ray = ray / m_sqrt(sq_ray); if (CGAL::abs(normal * ray) < m_cos_value_threshold) { return false; @@ -397,15 +395,13 @@ namespace Point_set { bool update(const std::vector& region) { CGAL_precondition(region.size() > 0); - auto unary_function = [&](const std::size_t& idx) -> const Point_3& { - return get (m_point_map, *(m_input_range.begin() + idx)); - }; - - internal::create_sphere( - make_range( - boost::make_transform_iterator(region.begin(), unary_function), - boost::make_transform_iterator(region.end(), unary_function)), - m_sqrt, m_squared_distance_3, m_center, m_radius); + FT radius; Point_3 center; + std::tie(radius, center) = internal::create_sphere( + m_input_range, m_point_map, region, m_traits, false).first; + if (radius >= FT(0)) { + m_radius = radius; + m_center = center; + } return true; } @@ -423,13 +419,11 @@ namespace Point_set { FT m_min_radius; FT m_max_radius; - const Squared_length_3 m_squared_length_3; - const Squared_distance_3 m_squared_distance_3; - const Scalar_product_3 m_scalar_product_3; const Sqrt m_sqrt; + const Squared_distance_3 m_squared_distance_3; - Point_3 m_center; FT m_radius; + Point_3 m_center; }; } // namespace Point_set diff --git a/Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_sorting.h b/Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_sorting.h index ec00fdee3a7..fdcdb073be6 100644 --- a/Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_sorting.h +++ b/Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_sorting.h @@ -74,14 +74,8 @@ namespace Point_set { /// @} private: - // using FT = typename Traits::FT; - // using Compare_scores = internal::Compare_scores; - - using Local_traits = Exact_predicates_inexact_constructions_kernel; - using Local_FT = typename Local_traits::FT; - using Local_point_3 = typename Local_traits::Point_3; - using To_local_converter = Cartesian_converter; - using Compare_scores = internal::Compare_scores; + using FT = typename Traits::FT; + using Compare_scores = internal::Compare_scores; public: /// \name Initialization @@ -128,8 +122,7 @@ namespace Point_set { m_point_map(parameters::choose_parameter(parameters::get_parameter( np, internal_np::point_map), PointMap())), m_traits(parameters::choose_parameter(parameters::get_parameter( - np, internal_np::geom_traits), GeomTraits())), - m_to_local_converter() { + np, internal_np::geom_traits), GeomTraits())) { CGAL_precondition(input_range.size() > 0); m_order.resize(m_input_range.size()); @@ -216,45 +209,18 @@ namespace Point_set { const Point_map m_point_map; const Traits m_traits; std::vector m_order; - std::vector m_scores; - const To_local_converter m_to_local_converter; + std::vector m_scores; void compute_scores() { + std::vector neighbors; - std::vector points; - - typename internal::Get_sqrt::Sqrt sqrt; - typename Local_traits::Compute_squared_distance_3 squared_distance_3; - for (std::size_t i = 0; i < m_input_range.size(); ++i) { - neighbors.clear(); m_neighbor_query(i, neighbors); neighbors.push_back(i); - - points.clear(); - for (std::size_t j = 0; j < neighbors.size(); ++j) { - CGAL_precondition(neighbors[j] < m_input_range.size()); - - const auto& key = *(m_input_range.begin() + neighbors[j]); - points.push_back(m_to_local_converter(get(m_point_map, key))); - } - CGAL_postcondition(points.size() == neighbors.size()); - - Local_point_3 fitted_center; - Local_FT fitted_radius; - - if (internal::create_sphere( - points, sqrt, squared_distance_3, fitted_center, fitted_radius)) { - - // Score is min squared distance to sphere. - m_scores[i] = Local_FT(0); - for (const Local_point_3& p : points) { - m_scores[i] += abs(sqrt(squared_distance_3(p, fitted_center)) - fitted_radius); - } - } else { - m_scores[i] = Local_FT(std::numeric_limits::max()); - } + m_scores[i] = internal::create_sphere( + m_input_range, m_point_map, neighbors, m_traits, true).second; + CGAL_assertion(m_scores[i] >= FT(0)); } } }; diff --git a/Shape_detection/include/CGAL/Shape_detection/Region_growing/internal/utils.h b/Shape_detection/include/CGAL/Shape_detection/Region_growing/internal/utils.h index b1856c12fb5..1c465eb0929 100644 --- a/Shape_detection/include/CGAL/Shape_detection/Region_growing/internal/utils.h +++ b/Shape_detection/include/CGAL/Shape_detection/Region_growing/internal/utils.h @@ -461,7 +461,8 @@ namespace internal { FT fitted_radius = FT(0); for (const auto& point : points) { const Point_2 converted = Point_2( - static_cast(point.x()), static_cast(point.y())); + static_cast(point.x()), + static_cast(point.y())); fitted_radius += sqrt(squared_distance_2(converted, fitted_center)); } fitted_radius /= static_cast(points.size()); @@ -472,7 +473,8 @@ namespace internal { score = FT(0); for (const auto& point : points) { const Point_2 converted = Point_2( - static_cast(point.x()), static_cast(point.y())); + static_cast(point.x()), + static_cast(point.y())); score += CGAL::abs(sqrt(squared_distance_2(converted, fitted_center)) - fitted_radius); } } @@ -482,18 +484,41 @@ namespace internal { } template< - typename PointRange, - typename Sqrt, - typename Squared_distance_3, - typename Point_3, - typename FT> - bool create_sphere( - const PointRange& points, - const Sqrt& sqrt, - const Squared_distance_3& squared_distance_3, - Point_3& center, FT& radius) { + typename Traits, + typename InputRange, + typename PointMap> + std::pair, typename Traits::FT> + create_sphere( + const InputRange& input_range, const PointMap point_map, + const std::vector& region, const Traits&, const bool compute_score) { - using Diagonalize_traits = Eigen_diagonalize_traits; + using FT = typename Traits::FT; + using Point_3 = typename Traits::Point_3; + + typename Get_sqrt::Sqrt sqrt; + typename Traits::Compute_squared_distance_3 squared_distance_3; + + using ITraits = CGAL::Exact_predicates_inexact_constructions_kernel; + using IConverter = CGAL::Cartesian_converter; + + using IFT = typename ITraits::FT; + using IPoint_3 = typename ITraits::Point_3; + + // Convert to inexact type. + std::vector points; + CGAL_precondition(region.size() > 0); + points.reserve(region.size()); + const IConverter iconverter = IConverter(); + + for (const std::size_t item_index : region) { + CGAL_precondition(item_index < input_range.size()); + const auto& key = *(input_range.begin() + item_index); + const auto& point = get(point_map, key); + points.push_back(iconverter(point)); + } + CGAL_precondition(points.size() == region.size()); + + using Diagonalize_traits = Eigen_diagonalize_traits; using Covariance_matrix = typename Diagonalize_traits::Covariance_matrix; using Vector = typename Diagonalize_traits::Vector; @@ -501,23 +526,23 @@ namespace internal { // Use bbox to compute diagonalization with smaller coordinates to // avoid loss of precision when inverting large coordinates. - const Bbox_3 bbox = bbox_3(points.begin(), points.end()); + const auto bbox = CGAL::bbox_3(points.begin(), points.end()); // Sphere least squares fitting. // See create_circle_2() above for more details on computation. Covariance_matrix A = { - FT(0), FT(0), FT(0), FT(0), FT(0), - FT(0), FT(0), FT(0), FT(0), FT(0), - FT(0), FT(0), FT(0), FT(0), FT(0) + IFT(0), IFT(0), IFT(0), IFT(0), IFT(0), + IFT(0), IFT(0), IFT(0), IFT(0), IFT(0), + IFT(0), IFT(0), IFT(0), IFT(0), IFT(0) }; - A[0] = static_cast(points.size()); - for (const Point_3& p : points) { + A[0] = static_cast(points.size()); + for (const auto& point : points) { - const FT x = p.x() - bbox.xmin(); - const FT y = p.y() - bbox.ymin(); - const FT z = p.z() - bbox.zmin(); - const FT r = x * x + y * y + z * z; + const IFT x = point.x() - bbox.xmin(); + const IFT y = point.y() - bbox.ymin(); + const IFT z = point.z() - bbox.zmin(); + const IFT r = x * x + y * y + z * z; A[1] += x; A[2] += y; @@ -536,36 +561,59 @@ namespace internal { } Vector eigenvalues = { - FT(0), FT(0), FT(0), FT(0), FT(0) + IFT(0), IFT(0), IFT(0), IFT(0), IFT(0) }; Matrix eigenvectors = { - FT(0), FT(0), FT(0), FT(0), FT(0), - FT(0), FT(0), FT(0), FT(0), FT(0), - FT(0), FT(0), FT(0), FT(0), FT(0), - FT(0), FT(0), FT(0), FT(0), FT(0), - FT(0), FT(0), FT(0), FT(0), FT(0) + IFT(0), IFT(0), IFT(0), IFT(0), IFT(0), + IFT(0), IFT(0), IFT(0), IFT(0), IFT(0), + IFT(0), IFT(0), IFT(0), IFT(0), IFT(0), + IFT(0), IFT(0), IFT(0), IFT(0), IFT(0), + IFT(0), IFT(0), IFT(0), IFT(0), IFT(0) }; Diagonalize_traits:: diagonalize_selfadjoint_covariance_matrix(A, eigenvalues, eigenvectors); // Perfect plane case, no sphere can be fitted. - if (eigenvectors[4] == 0) { - return false; + if (eigenvectors[4] == IFT(0)) { + return std::make_pair( + std::make_pair(FT(-1), Point_3()), + static_cast(std::numeric_limits::max())); } + CGAL_assertion(eigenvectors[4] != IFT(0)); // Other cases. - const FT half = FT(1) / FT(2); - center = Point_3( - bbox.xmin() - half * (eigenvectors[1] / eigenvectors[4]), - bbox.ymin() - half * (eigenvectors[2] / eigenvectors[4]), - bbox.zmin() - half * (eigenvectors[3] / eigenvectors[4])); + const IFT half = IFT(1) / IFT(2); + const Point_3 fitted_center = Point_3( + static_cast(bbox.xmin() - half * (eigenvectors[1] / eigenvectors[4])), + static_cast(bbox.ymin() - half * (eigenvectors[2] / eigenvectors[4])), + static_cast(bbox.zmin() - half * (eigenvectors[3] / eigenvectors[4])) + ); - radius = FT(0); - for (const Point_3& p : points) { - radius += sqrt(squared_distance_3(p, center)); + FT fitted_radius = FT(0); + for (const auto& point : points) { + const Point_3 converted = Point_3( + static_cast(point.x()), + static_cast(point.y()), + static_cast(point.z())); + fitted_radius += sqrt(squared_distance_3(converted, fitted_center)); } - radius /= static_cast(points.size()); - return true; + fitted_radius /= static_cast(points.size()); + + // Compute score. + FT score = FT(-1); + if (compute_score) { + score = FT(0); + for (const auto& point : points) { + const Point_3 converted = Point_3( + static_cast(point.x()), + static_cast(point.y()), + static_cast(point.z())); + score += CGAL::abs(sqrt(squared_distance_3(converted, fitted_center)) - fitted_radius); + } + } + + return std::make_pair( + std::make_pair(fitted_radius, fitted_center), score); } template<