more precise sphere fitting and sorting

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_sqrt(Get_sqrt::sqrt_object(m_traits)),
-    m_squared_distance_3(m_traits.compute_squared_distance_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)) {
 
       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;
+      if (std::isnan(CGAL::to_double(sq_norm))) return false;
-      normal = normal / m_sqrt (sq_norm);
+      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;
+      if (std::isnan(CGAL::to_double(sq_ray))) return false;
-      ray = ray / m_sqrt (sq_ray);
+      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<std::size_t>& region) {
 
       CGAL_precondition(region.size() > 0);
-      auto unary_function = [&](const std::size_t& idx) -> const Point_3& {
+      FT radius; Point_3 center;
-        return get (m_point_map, *(m_input_range.begin() + idx));
+      std::tie(radius, center) = internal::create_sphere(
-      };
+        m_input_range, m_point_map, region, m_traits, false).first;
-
+      if (radius >= FT(0)) {
-      internal::create_sphere(
+        m_radius = radius;
-        make_range(
+        m_center = center;
-          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);
       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

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 FT = typename Traits::FT;
-    // using Compare_scores = internal::Compare_scores<FT>;
+    using Compare_scores = internal::Compare_scores<FT>;
-
-    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<Traits, Local_traits>;
-    using Compare_scores = internal::Compare_scores<Local_FT>;
 
   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())),
+      np, internal_np::geom_traits), GeomTraits())) {
-    m_to_local_converter() {
 
       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<std::size_t> m_order;
-    std::vector<Local_FT> m_scores;
+    std::vector<FT> m_scores;
-    const To_local_converter m_to_local_converter;
 
     void compute_scores() {
+
       std::vector<std::size_t> neighbors;
-      std::vector<Local_point_3> points;
-
-      typename internal::Get_sqrt<Local_traits>::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);
-
+        m_scores[i] = internal::create_sphere(
-        points.clear();
+          m_input_range, m_point_map, neighbors, m_traits, true).second;
-        for (std::size_t j = 0; j < neighbors.size(); ++j) {
+        CGAL_assertion(m_scores[i] >= FT(0));
-          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<double>::max());
-        }
       }
     }
   };

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<FT>(point.x()), static_cast<FT>(point.y()));
+        static_cast<FT>(point.x()),
+        static_cast<FT>(point.y()));
       fitted_radius += sqrt(squared_distance_2(converted, fitted_center));
     }
     fitted_radius /= static_cast<FT>(points.size());
@@ -472,7 +473,8 @@ namespace internal {
       score = FT(0);
       for (const auto& point : points) {
         const Point_2 converted = Point_2(
-          static_cast<FT>(point.x()), static_cast<FT>(point.y()));
+          static_cast<FT>(point.x()),
+          static_cast<FT>(point.y()));
         score += CGAL::abs(sqrt(squared_distance_2(converted, fitted_center)) - fitted_radius);
       }
     }
@@ -482,18 +484,41 @@ namespace internal {
   }
 
   template<
-  typename PointRange,
+  typename Traits,
-  typename Sqrt,
+  typename InputRange,
-  typename Squared_distance_3,
+  typename PointMap>
-  typename Point_3,
+  std::pair<std::pair<typename Traits::FT, typename Traits::Point_3>, typename Traits::FT>
-  typename FT>
+  create_sphere(
-  bool create_sphere(
+    const InputRange& input_range, const PointMap point_map,
-    const PointRange& points,
+    const std::vector<std::size_t>& region, const Traits&, const bool compute_score) {
-    const Sqrt& sqrt,
-    const Squared_distance_3& squared_distance_3,
-    Point_3& center, FT& radius) {
 
-    using Diagonalize_traits = Eigen_diagonalize_traits<FT, 5>;
+    using FT = typename Traits::FT;
+    using Point_3 = typename Traits::Point_3;
+
+    typename Get_sqrt<Traits>::Sqrt sqrt;
+    typename Traits::Compute_squared_distance_3 squared_distance_3;
+
+    using ITraits = CGAL::Exact_predicates_inexact_constructions_kernel;
+    using IConverter = CGAL::Cartesian_converter<Traits, ITraits>;
+
+    using IFT = typename ITraits::FT;
+    using IPoint_3 = typename ITraits::Point_3;
+
+    // Convert to inexact type.
+    std::vector<IPoint_3> 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<IFT, 5>;
     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),
+      IFT(0), IFT(0), IFT(0), IFT(0), IFT(0),
-      FT(0), FT(0), FT(0), FT(0), FT(0),
+      IFT(0), IFT(0), IFT(0), IFT(0), IFT(0),
-      FT(0), FT(0), FT(0), FT(0), FT(0)
+      IFT(0), IFT(0), IFT(0), IFT(0), IFT(0)
     };
 
-    A[0] = static_cast<FT>(points.size());
+    A[0] = static_cast<IFT>(points.size());
-    for (const Point_3& p : points) {
+    for (const auto& point : points) {
 
-      const FT x = p.x() - bbox.xmin();
+      const IFT x = point.x() - bbox.xmin();
-      const FT y = p.y() - bbox.ymin();
+      const IFT y = point.y() - bbox.ymin();
-      const FT z = p.z() - bbox.zmin();
+      const IFT z = point.z() - bbox.zmin();
-      const FT r = x * x + y * y + z * z;
+      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),
+      IFT(0), IFT(0), IFT(0), IFT(0), IFT(0),
-      FT(0), FT(0), FT(0), FT(0), FT(0),
+      IFT(0), IFT(0), IFT(0), IFT(0), IFT(0),
-      FT(0), FT(0), FT(0), FT(0), FT(0),
+      IFT(0), IFT(0), IFT(0), IFT(0), IFT(0),
-      FT(0), FT(0), FT(0), FT(0), FT(0),
+      IFT(0), IFT(0), IFT(0), IFT(0), IFT(0),
-      FT(0), FT(0), FT(0), FT(0), FT(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) {
+    if (eigenvectors[4] == IFT(0)) {
-      return false;
+      return std::make_pair(
+        std::make_pair(FT(-1), Point_3()),
+        static_cast<FT>(std::numeric_limits<double>::max()));
     }
+    CGAL_assertion(eigenvectors[4] != IFT(0));
 
     // Other cases.
-    const FT half = FT(1) / FT(2);
+    const IFT half = IFT(1) / IFT(2);
-    center = Point_3(
+    const Point_3 fitted_center = Point_3(
-      bbox.xmin() - half * (eigenvectors[1] / eigenvectors[4]),
+      static_cast<FT>(bbox.xmin() - half * (eigenvectors[1] / eigenvectors[4])),
-      bbox.ymin() - half * (eigenvectors[2] / eigenvectors[4]),
+      static_cast<FT>(bbox.ymin() - half * (eigenvectors[2] / eigenvectors[4])),
-      bbox.zmin() - half * (eigenvectors[3] / eigenvectors[4]));
+      static_cast<FT>(bbox.zmin() - half * (eigenvectors[3] / eigenvectors[4]))
+    );
 
-    radius = FT(0);
+    FT fitted_radius = FT(0);
-    for (const Point_3& p : points) {
+    for (const auto& point : points) {
-      radius += sqrt(squared_distance_3(p, center));
+      const Point_3 converted = Point_3(
+        static_cast<FT>(point.x()),
+        static_cast<FT>(point.y()),
+        static_cast<FT>(point.z()));
+      fitted_radius += sqrt(squared_distance_3(converted, fitted_center));
     }
-    radius /= static_cast<FT>(points.size());
+    fitted_radius /= static_cast<FT>(points.size());
-    return true;
+
+    // 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<FT>(point.x()),
+          static_cast<FT>(point.y()),
+          static_cast<FT>(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<