Merge remote-tracking branch 'mine/Shape_detection-Region_growing_on_spheres-GF' into Shape_detection-Region_growing_on_spheres-GF

Shape_detection/doc/Shape_detection/PackageDescription.txt

@@ -117,7 +117,10 @@ on a polygon mesh.}
 - `CGAL::Shape_detection::Point_set::K_neighbor_query<GeomTraits, InputRange, PointMap>`
 - `CGAL::Shape_detection::Point_set::Sphere_neighbor_query<GeomTraits, InputRange, PointMap>`
 - `CGAL::Shape_detection::Point_set::Least_squares_line_fit_region<GeomTraits, InputRange, PointMap, NormalMap>`
+- `CGAL::Shape_detection::Point_set::Least_squares_circle_fit_region<GeomTraits, InputRange, PointMap, NormalMap>`
 - `CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region<GeomTraits, InputRange, PointMap, NormalMap>`
+- `CGAL::Shape_detection::Point_set::Least_squares_sphere_fit_region<GeomTraits, InputRange, PointMap, NormalMap>`
+- `CGAL::Shape_detection::Point_set::Least_squares_cylinder_fit_region<GeomTraits, InputRange, PointMap, NormalMap>`
 - `CGAL::Shape_detection::Point_set::Least_squares_line_fit_sorting<GeomTraits, InputRange, NeighborQuery, PointMap>`
 - `CGAL::Shape_detection::Point_set::Least_squares_plane_fit_sorting<GeomTraits, InputRange, NeighborQuery, PointMap>`
 

Shape_detection/doc/Shape_detection/Shape_detection.txt

@@ -236,8 +236,8 @@ Shapes are detected by growing regions from seed items, where each region is cre
 
 Together with the generic algorithm's implementation `CGAL::Shape_detection::Region_growing`, three particular instances of this algorithm are provided:
 
-- Line detection in a \ref Shape_detection_RegionGrowingPoints "2D point set";
+- Line and circle detection in a \ref Shape_detection_RegionGrowingPoints "2D point set";
-- Plane detection in a \ref Shape_detection_RegionGrowingPoints "3D point set";
+- Plane, sphere, and cylinder detection in a \ref Shape_detection_RegionGrowingPoints "3D point set";
 - Plane detection on a \ref Shape_detection_RegionGrowingMesh "polygon mesh".
 
 Other instances can be easily added by the user, as explained below.
@@ -342,10 +342,13 @@ This class finds K (specified by the user) nearest neighbors of the query point
 
 The component also provides
 
-- `CGAL::Shape_detection::Point_set::Least_squares_line_fit_region` - least squares line fit type of region for 2D points;
+- `CGAL::Shape_detection::Point_set::Least_squares_line_fit_region` - least squares line fit type of region for 2D points
-- `CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region` - least squares plane fit type of region for 3D points.
+- `CGAL::Shape_detection::Point_set::Least_squares_circle_fit_region` - least squares circle fit type of region for 2D points
+- `CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region` - least squares plane fit type of region for 3D points
+- `CGAL::Shape_detection::Point_set::Least_squares_sphere_fit_region` - least squares sphere fit type of region for 3D points
+- `CGAL::Shape_detection::Point_set::Least_squares_cylinder_fit_region` - least squares cylinder fit type of region for 3D points
 
-The program associates all points from a region to the best-fit hyperplane (2D line or 3D plane)
+The program associates all points from a region to the best-fit object (2D line, 2D circle, 3D plane, 3D sphere, etc.)
 and controls the quality of this fit.
 
 The quality of region growing in a point set (2D or 3D) can be improved by slightly sacrificing the running time.
@@ -409,6 +412,8 @@ when `angle_threshold` is strict enough (c), or it can be recognized as only one
 
 \subsubsection Shape_detection_RegionGrowingPoints_examples Examples
 
+\paragraph Shape_detection_RegionGrowingPoints_example_2D_lines Detecting 2D Lines
+
 Typical usage of the Region Growing component consists of five steps:
 
 -# Define an input range with points;
@@ -421,15 +426,34 @@ Given a 2D point set, we detect 2D lines using the fuzzy sphere neighborhood. We
 
 The points with assigned to them normal vectors are stored in `std::vector` and the used `Kernel` is `CGAL::Simple_cartesian`, where the number type is `double`.
 
-\cgalExample{Shape_detection/region_growing_on_point_set_2.cpp}
+\cgalExample{Shape_detection/region_growing_lines_on_point_set_2.cpp}
+
+\paragraph Shape_detection_RegionGrowingPoints_example_2D_circles Detecting 2D Circles
+
+The following example shows a similar example, this time detecting circles instead of lines.
+
+\cgalExample{Shape_detection/region_growing_circles_on_point_set_2.cpp}
+
+\paragraph Shape_detection_RegionGrowingPoints_example_3D_planes Detecting 3D Planes
 
 If we are given a 3D point set, then the example below shows how to detect 3D planes using the K nearest neighbors search. We color all points from the found regions
 and save them in a file (see Figure \cgalFigureRef{Region_growing_on_point_set_3}). The example also shows how to retrieve all points, which are not assigned to any region, and how to use a custom output iterator.
 
 The point set with associated normals is stored in `CGAL::Point_set_3` and the used `Kernel` is `CGAL::Exact_predicates_inexact_constructions_kernel`.
 
-\cgalExample{Shape_detection/region_growing_on_point_set_3.cpp}
+\cgalExample{Shape_detection/region_growing_planes_on_point_set_3.cpp}
 
+\paragraph Shape_detection_RegionGrowingPoints_example_3D_spheres Detecting 3D Spheres
+
+The following example shows a similar example, this time detecting spheres instead of planes.
+
+\cgalExample{Shape_detection/region_growing_spheres_on_point_set_3.cpp}
+
+\paragraph Shape_detection_RegionGrowingPoints_example_3D_cylinders Detecting 3D Cylinders
+
+The following example shows another similar example, this time detecting (infinite) cylinders.
+
+\cgalExample{Shape_detection/region_growing_cylinders_on_point_set_3.cpp}
 
 \subsubsection Shape_detection_RegionGrowingPoints_performance Performance
 
@@ -518,7 +542,7 @@ a faster version of region growing, use a floating type based kernel.
 We can improve the quality of region growing by providing a different seeding order (analogously to \ref Shape_detection_RegionGrowingPoints "Point Sets") that is why in this example we also
 sort indices of input faces using the `CGAL::Shape_detection::Polygon_mesh::Least_squares_plane_fit_sorting` and only then detect regions.
 
-\cgalExample{Shape_detection/region_growing_on_polygon_mesh.cpp}
+\cgalExample{Shape_detection/region_growing_planes_on_polygon_mesh.cpp}
 
 
 \subsubsection Shape_detection_RegionGrowingMesh_performance Performance

Shape_detection/doc/Shape_detection/examples.txt

@@ -6,9 +6,12 @@
 \example Shape_detection/include/efficient_RANSAC_with_custom_shape.h
 \example Shape_detection/efficient_RANSAC_with_custom_shape.cpp
 \example Shape_detection/efficient_RANSAC_and_plane_regularization.cpp
-\example Shape_detection/region_growing_on_point_set_2.cpp
+\example Shape_detection/region_growing_lines_on_point_set_2.cpp
-\example Shape_detection/region_growing_on_point_set_3.cpp
+\example Shape_detection/region_growing_circles_on_point_set_2.cpp
-\example Shape_detection/region_growing_on_polygon_mesh.cpp
+\example Shape_detection/region_growing_planes_on_point_set_3.cpp
+\example Shape_detection/region_growing_spheres_on_point_set_3.cpp
+\example Shape_detection/region_growing_cylinders_on_point_set_3.cpp
+\example Shape_detection/region_growing_planes_on_polygon_mesh.cpp
 \example Shape_detection/region_growing_with_custom_classes.cpp
 \example Shape_detection/shape_detection_basic_deprecated.cpp
 */

Shape_detection/examples/Shape_detection/CMakeLists.txt

@@ -18,9 +18,12 @@ if(TARGET CGAL::Eigen3_support)
   create_single_source_cgal_program(
     "efficient_RANSAC_and_plane_regularization.cpp")
 
-  create_single_source_cgal_program("region_growing_on_point_set_2.cpp")
+  create_single_source_cgal_program("region_growing_lines_on_point_set_2.cpp")
-  create_single_source_cgal_program("region_growing_on_point_set_3.cpp")
+  create_single_source_cgal_program("region_growing_planes_on_point_set_3.cpp")
-  create_single_source_cgal_program("region_growing_on_polygon_mesh.cpp")
+  create_single_source_cgal_program("region_growing_cylinders_on_point_set_3.cpp")
+  create_single_source_cgal_program("region_growing_spheres_on_point_set_3.cpp")
+  create_single_source_cgal_program("region_growing_circles_on_point_set_2.cpp")
+  create_single_source_cgal_program("region_growing_planes_on_polygon_mesh.cpp")
   create_single_source_cgal_program("region_growing_with_custom_classes.cpp")
 
   create_single_source_cgal_program("shape_detection_basic_deprecated.cpp")
@@ -30,9 +33,12 @@ if(TARGET CGAL::Eigen3_support)
     efficient_RANSAC_basic
     efficient_RANSAC_with_callback
     efficient_RANSAC_and_plane_regularization
-    region_growing_on_point_set_2
+    region_growing_lines_on_point_set_2
-    region_growing_on_point_set_3
+    region_growing_planes_on_point_set_3
-    region_growing_on_polygon_mesh
+    region_growing_cylinders_on_point_set_3
+    region_growing_spheres_on_point_set_3
+    region_growing_circles_on_point_set_2
+    region_growing_planes_on_polygon_mesh
     region_growing_with_custom_classes
     shape_detection_basic_deprecated)
     target_link_libraries(${target} PUBLIC CGAL::Eigen3_support)

Shape_detection/examples/Shape_detection/region_growing_circles_on_point_set_2.cpp

@@ -0,0 +1,109 @@
+#include <CGAL/Real_timer.h>
+#include <CGAL/Random.h>
+#include <CGAL/Simple_cartesian.h>
+
+#include <CGAL/Point_set_3.h>
+#include <CGAL/Point_set_3/IO.h>
+
+#include <CGAL/Shape_detection/Region_growing/Region_growing.h>
+#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set.h>
+
+#include <boost/iterator/function_output_iterator.hpp>
+
+#include <fstream>
+
+using Kernel = CGAL::Simple_cartesian<double>;
+using Point_3 = Kernel::Point_3;
+using Vector_3 = Kernel::Vector_3;
+using Point_2 = Kernel::Point_2;
+using Vector_2 = Kernel::Vector_2;
+
+using Point_set_3 = CGAL::Point_set_3<Point_3, Vector_3>;
+using Point_set_2 = CGAL::Point_set_3<Point_2, Vector_2>;
+using Point_map = Point_set_2::Point_map;
+using Normal_map = Point_set_2::Vector_map;
+
+namespace Shape_detection = CGAL::Shape_detection::Point_set;
+
+using Neighbor_query = Shape_detection::K_neighbor_query
+  <Kernel, Point_set_2, Point_map>;
+using Region_type = Shape_detection::Least_squares_circle_fit_region
+  <Kernel, Point_set_2, Point_map, Normal_map>;
+using Region_growing = CGAL::Shape_detection::Region_growing
+  <Point_set_2, Neighbor_query, Region_type>;
+
+int main (int argc, char** argv)
+{
+  std::ifstream ifile (argc > 1 ? argv[1] : "data/circles.ply");
+  Point_set_3 points3;
+  ifile >> points3;
+
+  std::cerr << points3.size() << " points read" << std::endl;
+
+  // Input should have normals
+  assert (points.has_normal_map());
+
+  Point_set_2 points;
+  points.add_normal_map();
+  for (Point_set_3::Index idx : points3)
+  {
+    const Point_3& p = points3.point(idx);
+    const Vector_3& n = points3.normal(idx);
+    points.insert (Point_2 (p.x(), p.y()), Vector_2 (n.x(), n.y()));
+  }
+
+  // Default parameters for data/circles.ply
+  const std::size_t k = 12;
+  const double tolerance = 0.01;
+  const double max_angle = 10.;
+  const std::size_t min_region_size = 20;
+
+  // No constraint on radius
+  const double min_radius = 0.;
+  const double max_radius = std::numeric_limits<double>::infinity();
+
+  Neighbor_query neighbor_query(points, k, points.point_map());
+  Region_type region_type(points, tolerance, max_angle, min_region_size,
+                          min_radius, max_radius,
+                          points.point_map(), points.normal_map());
+  Region_growing region_growing(points, neighbor_query, region_type);
+
+  // Add maps to get colored output
+  Point_set_3::Property_map<unsigned char>
+    red = points3.add_property_map<unsigned char>("red", 0).first,
+    green = points3.add_property_map<unsigned char>("green", 0).first,
+    blue = points3.add_property_map<unsigned char>("blue", 0).first;
+
+  CGAL::Random random;
+
+  std::size_t nb_circles = 0;
+  CGAL::Real_timer timer;
+  timer.start();
+  region_growing.detect
+    (boost::make_function_output_iterator
+     ([&](const std::vector<std::size_t>& region)
+      {
+        // Assign a random color to each region
+        unsigned char r = (unsigned char)(random.get_int(64, 192));
+        unsigned char g = (unsigned char)(random.get_int(64, 192));
+        unsigned char b = (unsigned char)(random.get_int(64, 192));
+        for (const std::size_t& idx : region)
+        {
+          red[idx] = r;
+          green[idx] = g;
+          blue[idx] = b;
+        }
+        ++ nb_circles;
+      }));
+  timer.stop();
+
+  std::cerr << nb_circles << " circles detected in "
+            << timer.time() << " seconds" << std::endl;
+
+  // Save in colored_circles.ply
+  std::ofstream out ("colored_circles.ply");
+  CGAL::set_binary_mode (out);
+  out << points3;
+
+  return EXIT_SUCCESS;
+}

Shape_detection/examples/Shape_detection/region_growing_cylinders_on_point_set_3.cpp

@@ -0,0 +1,97 @@
+#include <fstream>
+#include <CGAL/Real_timer.h>
+#include <CGAL/Random.h>
+#include <CGAL/Simple_cartesian.h>
+
+#include <CGAL/Point_set_3.h>
+#include <CGAL/Point_set_3/IO.h>
+
+#include <CGAL/Shape_detection/Region_growing/Region_growing.h>
+#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set.h>
+
+#include <boost/iterator/function_output_iterator.hpp>
+
+
+using Kernel = CGAL::Simple_cartesian<double>;
+using Point_3 = Kernel::Point_3;
+using Vector_3 = Kernel::Vector_3;
+
+using Point_set = CGAL::Point_set_3<Point_3>;
+using Point_map = typename Point_set::Point_map;
+using Normal_map = typename Point_set::Vector_map;
+
+namespace Shape_detection = CGAL::Shape_detection::Point_set;
+
+using Neighbor_query = Shape_detection::K_neighbor_query
+  <Kernel, Point_set, Point_map>;
+using Region_type = Shape_detection::Least_squares_cylinder_fit_region
+  <Kernel, Point_set, Point_map, Normal_map>;
+using Region_growing = CGAL::Shape_detection::Region_growing
+  <Point_set, Neighbor_query, Region_type>;
+
+int main (int argc, char** argv)
+{
+  std::ifstream ifile (argc > 1 ? argv[1] : "data/cube.pwn");
+  Point_set points;
+  ifile >> points;
+
+  std::cerr << points.size() << " points read" << std::endl;
+
+  // Input should have normals
+  assert (points.has_normal_map());
+
+  // Default parameters for data/cube.pwn
+  const std::size_t k = 24;
+  const double tolerance = 0.05;
+  const double max_angle = 5.;
+  const std::size_t min_region_size = 200;
+
+  // No constraint on radius
+  const double min_radius = 0.;
+  const double max_radius = std::numeric_limits<double>::infinity();
+
+  Neighbor_query neighbor_query(points, k, points.point_map());
+  Region_type region_type(points, tolerance, max_angle, min_region_size,
+                          min_radius, max_radius,
+                          points.point_map(), points.normal_map());
+  Region_growing region_growing(points, neighbor_query, region_type);
+
+  // Add maps to get colored output
+  Point_set::Property_map<unsigned char>
+    red = points.add_property_map<unsigned char>("red", 0).first,
+    green = points.add_property_map<unsigned char>("green", 0).first,
+    blue = points.add_property_map<unsigned char>("blue", 0).first;
+
+  CGAL::Random random;
+
+  std::size_t nb_cylinders = 0;
+  CGAL::Real_timer timer;
+  timer.start();
+  region_growing.detect
+    (boost::make_function_output_iterator
+     ([&](const std::vector<std::size_t>& region)
+      {
+        // Assign a random color to each region
+        unsigned char r = (unsigned char)(random.get_int(64, 192));
+        unsigned char g = (unsigned char)(random.get_int(64, 192));
+        unsigned char b = (unsigned char)(random.get_int(64, 192));
+        for (const std::size_t& idx : region)
+        {
+          red[idx] = r;
+          green[idx] = g;
+          blue[idx] = b;
+        }
+        ++ nb_cylinders;
+      }));
+  timer.stop();
+
+  std::cerr << nb_cylinders << " cylinders detected in "
+            << timer.time() << " seconds" << std::endl;
+
+  // Save in colored_cylinders.ply
+  std::ofstream out ("colored_cylinders.ply");
+  CGAL::set_binary_mode (out);
+  out << points;
+
+  return EXIT_SUCCESS;
+}

Shape_detection/examples/Shape_detection/region_growing_lines_on_point_set_2.cpp

@@ -77,7 +77,7 @@ int main(int argc, char *argv[]) {
   << std::endl << std::endl;
 
   // Load xyz data either from a local folder or a user-provided file.
-  std::ifstream in(argc > 1 ? argv[1] : "data/point_set_2.xyz");
+  std::ifstream in(argc > 1 ? argv[1] : "data/buildings_outline.xyz");
   CGAL::set_ascii_mode(in);
 
   if (!in) {

Shape_detection/examples/Shape_detection/region_growing_planes_on_point_set_3.cpp

@@ -108,7 +108,7 @@ int main(int argc, char *argv[]) {
   << std::endl << std::endl;
 
   // Load xyz data either from a local folder or a user-provided file.
-  std::ifstream in(argc > 1 ? argv[1] : "data/point_set_3.xyz");
+  std::ifstream in(argc > 1 ? argv[1] : "data/building.xyz");
   CGAL::set_ascii_mode(in);
 
   if (!in) {

Shape_detection/examples/Shape_detection/region_growing_planes_on_polygon_mesh.cpp

@@ -64,7 +64,7 @@ int main(int argc, char *argv[]) {
   << std::endl << std::endl;
 
   // Load off data either from a local folder or a user-provided file.
-  std::ifstream in(argc > 1 ? argv[1] : "data/polygon_mesh.off");
+  std::ifstream in(argc > 1 ? argv[1] : "data/building.off");
   CGAL::set_ascii_mode(in);
 
   if (!in) {

Shape_detection/examples/Shape_detection/region_growing_spheres_on_point_set_3.cpp

@@ -0,0 +1,97 @@
+#include <CGAL/Real_timer.h>
+#include <CGAL/Random.h>
+#include <CGAL/Simple_cartesian.h>
+
+#include <CGAL/Point_set_3.h>
+#include <CGAL/Point_set_3/IO.h>
+
+#include <CGAL/Shape_detection/Region_growing/Region_growing.h>
+#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set.h>
+
+#include <boost/iterator/function_output_iterator.hpp>
+
+#include <fstream>
+
+using Kernel = CGAL::Simple_cartesian<double>;
+using Point_3 = Kernel::Point_3;
+using Vector_3 = Kernel::Vector_3;
+
+using Point_set = CGAL::Point_set_3<Point_3>;
+using Point_map = typename Point_set::Point_map;
+using Normal_map = typename Point_set::Vector_map;
+
+namespace Shape_detection = CGAL::Shape_detection::Point_set;
+
+using Neighbor_query = Shape_detection::K_neighbor_query
+  <Kernel, Point_set, Point_map>;
+using Region_type = Shape_detection::Least_squares_sphere_fit_region
+  <Kernel, Point_set, Point_map, Normal_map>;
+using Region_growing = CGAL::Shape_detection::Region_growing
+  <Point_set, Neighbor_query, Region_type>;
+
+int main (int argc, char** argv)
+{
+  std::ifstream ifile (argc > 1 ? argv[1] : "data/spheres.ply");
+  Point_set points;
+  ifile >> points;
+
+  std::cerr << points.size() << " points read" << std::endl;
+
+  // Input should have normals
+  assert (points.has_normal_map());
+
+  // Default parameters for data/spheres.ply
+  const std::size_t k = 12;
+  const double tolerance = 0.01;
+  const double max_angle = 10.;
+  const std::size_t min_region_size = 50;
+
+  // No constraint on radius
+  const double min_radius = 0.;
+  const double max_radius = std::numeric_limits<double>::infinity();
+
+  Neighbor_query neighbor_query(points, k, points.point_map());
+  Region_type region_type(points, tolerance, max_angle, min_region_size,
+                          min_radius, max_radius,
+                          points.point_map(), points.normal_map());
+  Region_growing region_growing(points, neighbor_query, region_type);
+
+  // Add maps to get colored output
+  Point_set::Property_map<unsigned char>
+    red = points.add_property_map<unsigned char>("red", 0).first,
+    green = points.add_property_map<unsigned char>("green", 0).first,
+    blue = points.add_property_map<unsigned char>("blue", 0).first;
+
+  CGAL::Random random;
+
+  std::size_t nb_spheres = 0;
+  CGAL::Real_timer timer;
+  timer.start();
+  region_growing.detect
+    (boost::make_function_output_iterator
+     ([&](const std::vector<std::size_t>& region)
+      {
+        // Assign a random color to each region
+        unsigned char r = (unsigned char)(random.get_int(64, 192));
+        unsigned char g = (unsigned char)(random.get_int(64, 192));
+        unsigned char b = (unsigned char)(random.get_int(64, 192));
+        for (const std::size_t& idx : region)
+        {
+          red[idx] = r;
+          green[idx] = g;
+          blue[idx] = b;
+        }
+        ++ nb_spheres;
+      }));
+  timer.stop();
+
+  std::cerr << nb_spheres << " spheres detected in "
+            << timer.time() << " seconds" << std::endl;
+
+  // Save in colored_spheres.ply
+  std::ofstream out ("colored_spheres.ply");
+  CGAL::set_binary_mode (out);
+  out << points;
+
+  return EXIT_SUCCESS;
+}

Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set.h

@@ -20,7 +20,10 @@
 #include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Sphere_neighbor_query.h>
 
 #include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_line_fit_region.h>
+#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_circle_fit_region.h>
 #include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_plane_fit_region.h>
+#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_region.h>
+#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_cylinder_fit_region.h>
 
 #include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_line_fit_sorting.h>
 #include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_plane_fit_sorting.h>

Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_circle_fit_region.h

@@ -0,0 +1,368 @@
+// Copyright (c) 2020 GeometryFactory (France).
+// All rights reserved.
+//
+// This file is part of CGAL (www.cgal.org).
+//
+// $URL$
+// $Id$
+// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
+//
+//
+// Author(s)     : Simon Giraudot
+//
+
+#ifndef CGAL_SHAPE_DETECTION_REGION_GROWING_POINT_SET_LEAST_SQUARES_CIRCLE_FIT_REGION_H
+#define CGAL_SHAPE_DETECTION_REGION_GROWING_POINT_SET_LEAST_SQUARES_CIRCLE_FIT_REGION_H
+
+#include <CGAL/license/Shape_detection.h>
+
+#include <vector>
+
+#include <CGAL/assertions.h>
+#include <CGAL/number_utils.h>
+#include <CGAL/Cartesian_converter.h>
+#include <CGAL/Eigen_diagonalize_traits.h>
+
+#include <CGAL/Shape_detection/Region_growing/internal/utils.h>
+
+namespace CGAL {
+namespace Shape_detection {
+namespace Point_set {
+
+/*!
+  \ingroup PkgShapeDetectionRGOnPoints
+
+  \brief Region type based on the quality of the least squares circle
+  fit applied to 2D points.
+
+  This class fits a circle to chunks of points in a 2D point set and
+  controls the quality of this fit.  If all quality conditions are
+  satisfied, the chunk is accepted as a valid region, otherwise
+  rejected.
+
+  \tparam GeomTraits
+  must be a model of `Kernel`.
+
+  \tparam InputRange
+  must be a model of `ConstRange` whose iterator type is `RandomAccessIterator`.
+
+  \tparam PointMap
+  must be an `LvaluePropertyMap` whose key type is the value type of the input
+  range and value type is `Kernel::Point_3`.
+
+  \tparam NormalMap
+  must be an `LvaluePropertyMap` whose key type is the value type of the input
+  range and value type is `Kernel::Vector_3`.
+
+  \cgalModels `RegionType`
+*/
+template<typename GeomTraits,
+         typename InputRange,
+         typename PointMap,
+         typename NormalMap>
+class Least_squares_circle_fit_region
+{
+
+public:
+
+  /// \name Types
+  /// @{
+
+  /// \cond SKIP_IN_MANUAL
+  using Traits = GeomTraits;
+  using Input_range = InputRange;
+  using Point_map = PointMap;
+  using Normal_map = NormalMap;
+  /// \endcond
+
+  /// Number type.
+  typedef typename GeomTraits::FT FT;
+
+  /// \cond SKIP_IN_MANUAL
+  using Point_2 = typename Traits::Point_2;
+  using Vector_2 = typename Traits::Vector_2;
+
+  using Squared_distance_2 = typename Traits::Compute_squared_distance_2;
+
+  using Get_sqrt = internal::Get_sqrt<Traits>;
+  using Sqrt = typename Get_sqrt::Sqrt;
+
+private:
+
+  const Input_range& m_input_range;
+
+  const FT m_distance_threshold;
+  const FT m_normal_threshold;
+  const std::size_t m_min_region_size;
+  const FT m_min_radius;
+  const FT m_max_radius;
+
+  const Point_map m_point_map;
+  const Normal_map m_normal_map;
+
+  const Squared_distance_2 m_squared_distance_2;
+  const Sqrt m_sqrt;
+
+  Point_2 m_center;
+  FT m_radius;
+
+public:
+
+  /// \endcond
+
+  /// @}
+
+  /// \name Initialization
+  /// @{
+
+  /*!
+    \brief initializes all internal data structures.
+
+    \param input_range an instance of `InputRange` with 2D points and
+    corresponding 2D normal vectors
+
+    \param distance_threshold the maximum distance from a point to a
+    circle. %Default is 1.
+
+    \param angle_threshold the maximum accepted angle in degrees
+    between the normal of a point and the radius of a circle. %Default
+    is 25 degrees.
+
+    \param min_region_size the minimum number of 2D points a region
+    must have. %Default is 3.
+
+    \param minimum_radius the radius below which an estimated circle
+    is considered as invalid and discarded. %Default is 0 (no limit).
+
+    \param maximum_radius the radius above which an estimated circle
+    is considered as invalid and discarded. %Default is infinity (no
+    limit).
+
+    \param point_map an instance of `PointMap` that maps an item from
+    `input_range` to `Kernel::Point_3`
+
+    \param normal_map an instance of `NormalMap` that maps an item
+    from `input_range` to `Kernel::Vector_3`
+
+    \param traits an instance of `GeomTraits`.
+
+    \pre `input_range.size() > 0`
+    \pre `distance_threshold >= 0`
+    \pre `angle_threshold >= 0 && angle_threshold <= 90`
+    \pre `min_region_size > 0`
+  */
+  Least_squares_circle_fit_region  (const InputRange& input_range,
+                                    const FT distance_threshold = FT(1),
+                                    const FT angle_threshold = FT(25),
+                                    const std::size_t min_region_size = 3,
+                                    const FT minimum_radius = FT(0),
+                                    const FT maximum_radius = std::numeric_limits<FT>::infinity(),
+                                    const PointMap point_map = PointMap(),
+                                    const NormalMap normal_map = NormalMap(),
+                                    const GeomTraits traits = GeomTraits())
+  : m_input_range(input_range)
+  , m_distance_threshold(distance_threshold)
+  , m_normal_threshold(static_cast<FT>(
+                         std::cos(
+                           CGAL::to_double(
+                             (angle_threshold * static_cast<FT>(CGAL_PI)) / FT(180)))))
+  , m_min_region_size(min_region_size)
+  , m_min_radius (minimum_radius)
+  , m_max_radius (maximum_radius)
+  , m_point_map(point_map)
+  , m_normal_map(normal_map)
+  , m_squared_distance_2(traits.compute_squared_distance_2_object())
+  , m_sqrt(Get_sqrt::sqrt_object(traits))
+  {
+    CGAL_precondition(input_range.size() > 0);
+    CGAL_precondition(distance_threshold >= FT(0));
+    CGAL_precondition(angle_threshold >= FT(0) && angle_threshold <= FT(90));
+    CGAL_precondition(min_region_size > 0);
+    CGAL_precondition(minimum_radius >= 0);
+    CGAL_precondition(maximum_radius > minimum_radius);
+  }
+
+  /// @}
+
+  /// \name Access
+  /// @{
+
+  /*!
+    \brief implements `RegionType::is_part_of_region()`.
+
+    This function controls if a point with the index `query_index` is
+    within the `distance_threshold` from the corresponding circle and
+    if the angle between its normal and the circle radius is within
+    the `angle_threshold`.  If both conditions are satisfied, it
+    returns `true`, otherwise `false`.
+
+    \param query_index
+    index of the query point
+
+    \param indices indices of the inliers of the region
+
+    The first parameter is not used in this implementation.
+
+    \return Boolean `true` or `false`
+
+    \pre `query_index >= 0 && query_index < input_range.size()`
+  */
+  bool is_part_of_region (const std::size_t,
+                          const std::size_t query_index,
+                          const std::vector<std::size_t>& indices) const
+  {
+    CGAL_precondition(query_index < m_input_range.size());
+
+    // First, we need to integrate at least 6 points so that the
+    // computed circle means something
+    if (indices.size() < 6)
+      return true;
+
+    // If radius is out of bound, nothing fits, early ending
+    if (m_radius < m_min_radius || m_radius > m_max_radius)
+      return false;
+
+    const auto& key = *(m_input_range.begin() + query_index);
+    const Point_2& query_point = get(m_point_map, key);
+    Vector_2 normal = get(m_normal_map, key);
+
+    FT distance_to_center = m_sqrt(m_squared_distance_2 (query_point, m_center));
+    FT distance_to_circle = CGAL::abs (distance_to_center - m_radius);
+
+    if (distance_to_circle > m_distance_threshold)
+      return false;
+
+    normal = normal / m_sqrt (normal * normal);
+
+    Vector_2 ray (m_center, query_point);
+    ray = ray / m_sqrt (ray * ray);
+
+    if (CGAL::abs (normal * ray) < m_normal_threshold)
+      return false;
+
+    return true;
+  }
+
+  /*!
+    \brief implements `RegionType::is_valid_region()`.
+
+    This function controls if the estimated radius is between
+    `minimum_radius` and `maximum_radius` and if the `region` contains
+    at least `min_region_size` points.
+
+    \param region
+    indices of points included in the region
+
+    \return Boolean `true` or `false`
+  */
+  inline bool is_valid_region(const std::vector<std::size_t>& region) const
+  {
+    return ((m_min_radius <= m_radius && m_radius <= m_max_radius)
+            && (region.size() >= m_min_region_size));
+  }
+
+  /*!
+    \brief implements `RegionType::update()`.
+
+    This function fits the least squares circle to all points from the `region`.
+
+    \param region
+    indices of points included in the region
+
+    \pre `region.size() > 0`
+  */
+  void update(const std::vector<std::size_t>& region)
+  {
+    CGAL_precondition(region.size() > 0);
+
+    auto unary_function
+      = [&](const std::size_t& idx) -> const Point_2&
+        {
+          return get (m_point_map, *(m_input_range.begin() + idx));
+        };
+
+    // Use bbox to compute diagonalization with smaller coordinates to
+    // avoid loss of precision when inverting large coordinates
+    CGAL::Bbox_2 bbox = CGAL::bbox_2
+      (boost::make_transform_iterator
+       (region.begin(), unary_function),
+       boost::make_transform_iterator
+       (region.end(), unary_function));
+
+    using Diagonalize_traits = Eigen_diagonalize_traits<FT, 4>;
+    using Covariance_matrix = typename Diagonalize_traits::Covariance_matrix;
+    using Vector = typename Diagonalize_traits::Vector;
+    using Matrix = typename Diagonalize_traits::Matrix;
+
+    // Circle least squares fitting,
+    // Circle of center (a,b) and radius R
+    // Ri = sqrt((xi - a)^2 + (yi - b)^2)
+    // Minimize Sum(Ri^2 - R^2)^2
+    // -> Minimize Sum(xi^2 + yi^2 − 2 a*xi − 2 b*yi + a^2 + b^2 − R^2)^2
+    // let B=-2a ; C=-2b; D= a^2 + b^2 - R^2
+    // let ri = x^2 + y^2
+    // -> Minimize Sum(D + B*xi + C*yi + ri)^2
+    // -> Minimize Sum(1 + B/D*xi + C/D*yi + ri/D)^2
+    // -> system of linear equations
+    // -> diagonalize matrix
+    //    NB   x   y   r
+    //        xx  xy  xr
+    //            yy  yr
+    //                rr
+    //
+    // -> center coordinates = -0.5 * eigenvector(1) / eigenvector(3) ; -0.5 * eigenvector(2) / eigenvector(3)
+    Covariance_matrix A
+      = { FT(0), FT(0), FT(0), FT(0), FT(0),
+          FT(0), FT(0), FT(0), FT(0), FT(0) };
+
+    A[0] = region.size();
+    for (const std::size_t& idx : region)
+    {
+      const auto& key = *(m_input_range.begin() + idx);
+      const Point_2& p = get(m_point_map, key);
+      FT x = p.x() - bbox.xmin();
+      FT y = p.y() - bbox.ymin();
+      FT r = x*x + y*y;
+      A[1] += x;
+      A[2] += y;
+      A[3] += r;
+      A[4] += x * x;
+      A[5] += x * y;
+      A[6] += x * r;
+      A[7] += y * y;
+      A[8] += y * r;
+      A[9] += r * r;
+    }
+
+    Vector eigenvalues = { FT(0), FT(0), FT(0), FT(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) };
+
+    Diagonalize_traits::diagonalize_selfadjoint_covariance_matrix
+      (A, eigenvalues, eigenvectors);
+
+    m_center = Point_2 (bbox.xmin() - FT(0.5) * (eigenvectors[1] / eigenvectors[3]),
+                        bbox.ymin() - FT(0.5) * (eigenvectors[2] / eigenvectors[3]));
+
+    m_radius = FT(0);
+    for (const std::size_t& idx : region)
+    {
+      const auto& key = *(m_input_range.begin() + idx);
+      const Point_2& p = get(m_point_map, key);
+      m_radius += m_sqrt (m_squared_distance_2 (p, m_center));
+    }
+
+    m_radius /= region.size();
+  }
+
+  /// @}
+
+};
+
+} // namespace Point_set
+} // namespace Shape_detection
+} // namespace CGAL
+
+#endif // CGAL_SHAPE_DETECTION_REGION_GROWING_POINT_SET_LEAST_SQUARES_CIRCLE_FIT_REGION_H

Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_cylinder_fit_region.h

@@ -0,0 +1,362 @@
+// Copyright (c) 2020 GeometryFactory (France).
+// All rights reserved.
+//
+// This file is part of CGAL (www.cgal.org).
+//
+// $URL$
+// $Id$
+// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
+//
+//
+// Author(s)     : Simon Giraudot
+//
+
+#ifndef CGAL_SHAPE_DETECTION_REGION_GROWING_POINT_SET_LEAST_SQUARES_CYLINDER_FIT_REGION_H
+#define CGAL_SHAPE_DETECTION_REGION_GROWING_POINT_SET_LEAST_SQUARES_CYLINDER_FIT_REGION_H
+
+#include <CGAL/license/Shape_detection.h>
+
+#include <vector>
+
+#include <CGAL/assertions.h>
+#include <CGAL/number_utils.h>
+#include <CGAL/Cartesian_converter.h>
+#include <CGAL/Eigen_diagonalize_traits.h>
+
+#include <CGAL/Shape_detection/Region_growing/internal/utils.h>
+
+namespace CGAL {
+namespace Shape_detection {
+namespace Point_set {
+
+/*!
+  \ingroup PkgShapeDetectionRGOnPoints
+
+  \brief Region type based on the quality of the least squares cylinder
+  fit applied to 3D points.
+
+  This class fits an infinite cylinder to chunks of points in a 3D
+  point set and controls the quality of this fit.  If all quality
+  conditions are satisfied, the chunk is accepted as a valid region,
+  otherwise rejected.
+
+  \tparam GeomTraits
+  must be a model of `Kernel`.
+
+  \tparam InputRange
+  must be a model of `ConstRange` whose iterator type is `RandomAccessIterator`.
+
+  \tparam PointMap
+  must be an `LvaluePropertyMap` whose key type is the value type of the input
+  range and value type is `Kernel::Point_3`.
+
+  \tparam NormalMap
+  must be an `LvaluePropertyMap` whose key type is the value type of the input
+  range and value type is `Kernel::Vector_3`.
+
+  \cgalModels `RegionType`
+*/
+template<typename GeomTraits,
+         typename InputRange,
+         typename PointMap,
+         typename NormalMap>
+class Least_squares_cylinder_fit_region
+{
+
+public:
+
+  /// \name Types
+  /// @{
+
+  /// \cond SKIP_IN_MANUAL
+  using Traits = GeomTraits;
+  using Input_range = InputRange;
+  using Point_map = PointMap;
+  using Normal_map = NormalMap;
+  /// \endcond
+
+  /// Number type.
+  typedef typename GeomTraits::FT FT;
+
+  /// \cond SKIP_IN_MANUAL
+  using Point_3 = typename Traits::Point_3;
+  using Vector_3 = typename Traits::Vector_3;
+  using Line_3 = typename Traits::Line_3;
+
+  using Squared_distance_3 = typename Traits::Compute_squared_distance_3;
+
+  using Get_sqrt = internal::Get_sqrt<Traits>;
+  using Sqrt = typename Get_sqrt::Sqrt;
+
+private:
+
+  const Input_range& m_input_range;
+
+  const FT m_distance_threshold;
+  const FT m_normal_threshold;
+  const std::size_t m_min_region_size;
+  const FT m_min_radius;
+  const FT m_max_radius;
+
+  const Point_map m_point_map;
+  const Normal_map m_normal_map;
+
+  const Squared_distance_3 m_squared_distance_3;
+  const Sqrt m_sqrt;
+
+  Line_3 m_axis;
+  FT m_radius;
+
+public:
+
+  /// \endcond
+
+  /// @}
+
+  /// \name Initialization
+  /// @{
+
+  /*!
+    \brief initializes all internal data structures.
+
+    \param input_range an instance of `InputRange` with 3D points and
+    corresponding 3D normal vectors
+
+    \param distance_threshold the maximum distance from a point to a
+    cylinder. %Default is 1.
+
+    \param angle_threshold the maximum accepted angle in degrees
+    between the normal of a point and the radius of a cylinder. %Default
+    is 25 degrees.
+
+    \param min_region_size the minimum number of 3D points a region
+    must have. %Default is 3.
+
+    \param minimum_radius the radius below which an estimated cylinder
+    is considered as invalid and discarded. %Default is 0 (no limit).
+
+    \param maximum_radius the radius above which an estimated cylinder
+    is considered as invalid and discarded. %Default is infinity (no
+    limit).
+
+    \param point_map an instance of `PointMap` that maps an item from
+    `input_range` to `Kernel::Point_3`
+
+    \param normal_map an instance of `NormalMap` that maps an item
+    from `input_range` to `Kernel::Vector_3`
+
+    \param traits an instance of `GeomTraits`.
+
+    \pre `input_range.size() > 0`
+    \pre `distance_threshold >= 0`
+    \pre `angle_threshold >= 0 && angle_threshold <= 90`
+    \pre `min_region_size > 0`
+  */
+  Least_squares_cylinder_fit_region  (const InputRange& input_range,
+                                      const FT distance_threshold = FT(1),
+                                      const FT angle_threshold = FT(25),
+                                      const std::size_t min_region_size = 3,
+                                      const FT minimum_radius = FT(0),
+                                      const FT maximum_radius = std::numeric_limits<FT>::infinity(),
+                                      const PointMap point_map = PointMap(),
+                                      const NormalMap normal_map = NormalMap(),
+                                      const GeomTraits traits = GeomTraits())
+  : m_input_range(input_range)
+  , m_distance_threshold(distance_threshold)
+  , m_normal_threshold(static_cast<FT>(
+                         std::cos(
+                           CGAL::to_double(
+                             (angle_threshold * static_cast<FT>(CGAL_PI)) / FT(180)))))
+  , m_min_region_size(min_region_size)
+  , m_min_radius (minimum_radius)
+  , m_max_radius (maximum_radius)
+  , m_point_map(point_map)
+  , m_normal_map(normal_map)
+  , m_squared_distance_3(traits.compute_squared_distance_3_object())
+  , m_sqrt(Get_sqrt::sqrt_object(traits))
+  , m_radius(std::numeric_limits<FT>::quiet_NaN())
+  {
+    CGAL_precondition(input_range.size() > 0);
+    CGAL_precondition(distance_threshold >= FT(0));
+    CGAL_precondition(angle_threshold >= FT(0) && angle_threshold <= FT(90));
+    CGAL_precondition(min_region_size > 0);
+  }
+
+  /// @}
+
+  /// \name Access
+  /// @{
+
+  /*!
+    \brief implements `RegionType::is_part_of_region()`.
+
+    This function controls if a point with the index `query_index` is
+    within the `distance_threshold` from the corresponding cylinder and
+    if the angle between its normal and the cylinder radius is within
+    the `angle_threshold`.  If both conditions are satisfied, it
+    returns `true`, otherwise `false`.
+
+    \param query_index
+    index of the query point
+
+    \param indices indices of the inliers of the region
+
+    The first parameter is not used in this implementation.
+
+    \return Boolean `true` or `false`
+
+    \pre `query_index >= 0 && query_index < input_range.size()`
+  */
+  bool is_part_of_region (const std::size_t,
+                          const std::size_t query_index,
+                          const std::vector<std::size_t>& indices) const
+  {
+    CGAL_precondition(query_index < m_input_range.size());
+
+    // First, we need to integrate at least 6 points so that the
+    // computed cylinder means something
+    if (indices.size() < 6)
+      return true;
+
+    if (std::isnan(m_radius))
+      return false;
+
+    // If radius is out of bound, nothing fits, early ending
+    if (m_radius < m_min_radius || m_radius > m_max_radius)
+      return false;
+
+    const auto& key = *(m_input_range.begin() + query_index);
+    const Point_3& query_point = get(m_point_map, key);
+    Vector_3 normal = get(m_normal_map, key);
+
+    FT distance_to_center = m_sqrt(m_squared_distance_3 (query_point, m_axis));
+    FT distance_to_cylinder = CGAL::abs (distance_to_center - m_radius);
+
+    if (distance_to_cylinder > m_distance_threshold)
+      return false;
+
+    normal = normal / m_sqrt (normal * normal);
+
+    Point_3 proj = m_axis.projection(query_point);
+    Vector_3 ray (proj, query_point);
+    ray = ray / m_sqrt (ray * ray);
+
+    if (CGAL::abs (normal * ray) < m_normal_threshold)
+      return false;
+
+    return true;
+  }
+
+  /*!
+    \brief implements `RegionType::is_valid_region()`.
+
+    This function controls if the estimated radius is between
+    `minimum_radius` and `maximum_radius` and if the `region` contains
+    at least `min_region_size` points.
+
+    \param region
+    indices of points included in the region
+
+    \return Boolean `true` or `false`
+  */
+  inline bool is_valid_region(const std::vector<std::size_t>& region) const
+  {
+    return ((m_min_radius <= m_radius && m_radius <= m_max_radius)
+            && (region.size() >= m_min_region_size));
+  }
+
+  /*!
+    \brief implements `RegionType::update()`.
+
+    This function fits the least squares cylinder to all points from the `region`.
+
+    \param region
+    indices of points included in the region
+
+    \pre `region.size() > 0`
+  */
+  void update(const std::vector<std::size_t>& region)
+  {
+    CGAL_precondition(region.size() > 0);
+
+    Vector_3 mean_axis = CGAL::NULL_VECTOR;
+    std::size_t nb = 0;
+    // Shuffle to avoid always picking 2 close points
+    std::vector<std::size_t>& aregion
+      = const_cast<std::vector<std::size_t>&>(region);
+    cpp98::random_shuffle (aregion.begin(), aregion.end());
+
+    const Point_3& ref = get(m_point_map, *(m_input_range.begin() + region.front()));
+
+    m_radius = FT(0);
+    Point_3 point_on_axis = CGAL::ORIGIN;
+    for (std::size_t i = 0; i < aregion.size() - 1; ++ i)
+    {
+      Vector_3 v0 = get(m_normal_map, *(m_input_range.begin() + aregion[i]));
+      v0 = v0 / CGAL::sqrt(v0*v0);
+      Vector_3 v1 = get(m_normal_map, *(m_input_range.begin() + aregion[i+1]));
+      v1 = v1 / CGAL::sqrt(v1*v1);
+      Vector_3 axis = CGAL::cross_product (v0, v1);
+      if (CGAL::sqrt(axis.squared_length()) < (FT)(0.01))
+        continue;
+      axis = axis / CGAL::sqrt(axis * axis);
+
+      const Point_3& p0 = get(m_point_map, *(m_input_range.begin() + aregion[i]));
+      const Point_3& p1 = get(m_point_map, *(m_input_range.begin() + aregion[i+1]));
+
+      Vector_3 xdir = v0 - axis * (v0 * axis);
+      xdir = xdir / CGAL::sqrt (xdir * xdir);
+
+      Vector_3 ydir = CGAL::cross_product (axis, xdir);
+      ydir = ydir / CGAL::sqrt (ydir * ydir);
+
+      FT v1x = v1 * ydir;
+      FT v1y = -v1 * xdir;
+
+      Vector_3 d (p0, p1);
+
+      FT ox = xdir * d;
+      FT oy = ydir * d;
+      FT ldist = v1x * ox + v1y * oy;
+
+      FT radius = ldist / v1x;
+
+      Point_3 point = p0 + xdir * radius;
+      Line_3 line (point, axis);
+      point = line.projection(ref);
+
+      point_on_axis = CGAL::barycenter (point_on_axis, nb, point, 1);
+
+      m_radius += CGAL::abs(radius);
+
+      if (nb != 0 && axis * mean_axis < 0)
+        axis = -axis;
+
+      mean_axis = mean_axis + axis;
+      ++ nb;
+    }
+
+    if (nb == 0)
+      return;
+
+    mean_axis = mean_axis / CGAL::sqrt(mean_axis * mean_axis);
+    m_axis = Line_3 (point_on_axis, mean_axis);
+    m_radius /= nb;
+
+    m_radius = FT(0);
+    for (std::size_t i = 0; i < aregion.size(); ++ i)
+    {
+      const Point_3& p0 = get(m_point_map, *(m_input_range.begin() + aregion[i]));
+      m_radius += m_sqrt(m_squared_distance_3(p0, m_axis));
+    }
+    m_radius /= aregion.size();
+  }
+  /// @}
+
+};
+
+} // namespace Point_set
+} // namespace Shape_detection
+} // namespace CGAL
+
+#endif // CGAL_SHAPE_DETECTION_REGION_GROWING_POINT_SET_LEAST_SQUARES_CYLINDER_FIT_REGION_H

Shape_detection/include/CGAL/Shape_detection/Region_growing/Region_growing_on_point_set/Least_squares_sphere_fit_region.h

@@ -0,0 +1,368 @@
+// Copyright (c) 2020 GeometryFactory (France).
+// All rights reserved.
+//
+// This file is part of CGAL (www.cgal.org).
+//
+// $URL$
+// $Id$
+// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
+//
+//
+// Author(s)     : Simon Giraudot
+//
+
+#ifndef CGAL_SHAPE_DETECTION_REGION_GROWING_POINT_SET_LEAST_SQUARES_SPHERE_FIT_REGION_H
+#define CGAL_SHAPE_DETECTION_REGION_GROWING_POINT_SET_LEAST_SQUARES_SPHERE_FIT_REGION_H
+
+#include <CGAL/license/Shape_detection.h>
+
+#include <vector>
+
+#include <CGAL/assertions.h>
+#include <CGAL/number_utils.h>
+#include <CGAL/Cartesian_converter.h>
+#include <CGAL/Eigen_diagonalize_traits.h>
+
+#include <CGAL/Shape_detection/Region_growing/internal/utils.h>
+
+namespace CGAL {
+namespace Shape_detection {
+namespace Point_set {
+
+/*!
+  \ingroup PkgShapeDetectionRGOnPoints
+
+  \brief Region type based on the quality of the least squares sphere
+  fit applied to 3D points.
+
+  This class fits a sphere to chunks of points in a 3D point set and
+  controls the quality of this fit.  If all quality conditions are
+  satisfied, the chunk is accepted as a valid region, otherwise
+  rejected.
+
+  \tparam GeomTraits
+  must be a model of `Kernel`.
+
+  \tparam InputRange
+  must be a model of `ConstRange` whose iterator type is `RandomAccessIterator`.
+
+  \tparam PointMap
+  must be an `LvaluePropertyMap` whose key type is the value type of the input
+  range and value type is `Kernel::Point_3`.
+
+  \tparam NormalMap
+  must be an `LvaluePropertyMap` whose key type is the value type of the input
+  range and value type is `Kernel::Vector_3`.
+
+  \cgalModels `RegionType`
+*/
+template<typename GeomTraits,
+         typename InputRange,
+         typename PointMap,
+         typename NormalMap>
+class Least_squares_sphere_fit_region
+{
+
+public:
+
+  /// \name Types
+  /// @{
+
+  /// \cond SKIP_IN_MANUAL
+  using Traits = GeomTraits;
+  using Input_range = InputRange;
+  using Point_map = PointMap;
+  using Normal_map = NormalMap;
+  /// \endcond
+
+  /// Number type.
+  typedef typename GeomTraits::FT FT;
+
+  /// \cond SKIP_IN_MANUAL
+  using Point_3 = typename Traits::Point_3;
+  using Vector_3 = typename Traits::Vector_3;
+  using Plane_3 = typename Traits::Plane_3;
+
+  using Squared_length_3 = typename Traits::Compute_squared_length_3;
+  using Squared_distance_3 = typename Traits::Compute_squared_distance_3;
+  using Scalar_product_3 = typename Traits::Compute_scalar_product_3;
+
+  using Get_sqrt = internal::Get_sqrt<Traits>;
+  using Sqrt = typename Get_sqrt::Sqrt;
+
+private:
+
+  const Input_range& m_input_range;
+
+  const FT m_distance_threshold;
+  const FT m_normal_threshold;
+  const std::size_t m_min_region_size;
+  const FT m_min_radius;
+  const FT m_max_radius;
+
+  const Point_map m_point_map;
+  const Normal_map m_normal_map;
+
+  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;
+
+  Point_3 m_center;
+  FT m_radius;
+
+public:
+
+  /// \endcond
+
+  /// @}
+
+  /// \name Initialization
+  /// @{
+
+  /*!
+    \brief initializes all internal data structures.
+
+    \param input_range an instance of `InputRange` with 3D points and
+    corresponding 3D normal vectors
+
+    \param distance_threshold the maximum distance from a point to a
+    sphere. %Default is 1.
+
+    \param angle_threshold the maximum accepted angle in degrees
+    between the normal of a point and the radius of a sphere. %Default
+    is 25 degrees.
+
+    \param min_region_size the minimum number of 3D points a region
+    must have. %Default is 3.
+
+    \param minimum_radius the radius below which an estimated sphere
+    is considered as invalid and discarded. %Default is 0 (no limit).
+
+    \param maximum_radius the radius above which an estimated sphere
+    is considered as invalid and discarded. %Default is infinity (no
+    limit).
+
+    \param point_map an instance of `PointMap` that maps an item from
+    `input_range` to `Kernel::Point_3`
+
+    \param normal_map an instance of `NormalMap` that maps an item
+    from `input_range` to `Kernel::Vector_3`
+
+    \param traits an instance of `GeomTraits`.
+
+    \pre `input_range.size() > 0`
+    \pre `distance_threshold >= 0`
+    \pre `angle_threshold >= 0 && angle_threshold <= 90`
+    \pre `min_region_size > 0`
+  */
+  Least_squares_sphere_fit_region  (const InputRange& input_range,
+                                    const FT distance_threshold = FT(1),
+                                    const FT angle_threshold = FT(25),
+                                    const std::size_t min_region_size = 3,
+                                    const FT minimum_radius = FT(0),
+                                    const FT maximum_radius = std::numeric_limits<FT>::infinity(),
+                                    const PointMap point_map = PointMap(),
+                                    const NormalMap normal_map = NormalMap(),
+                                    const GeomTraits traits = GeomTraits())
+    : m_input_range(input_range)
+    , m_distance_threshold(distance_threshold)
+    , m_normal_threshold(static_cast<FT>(
+                           std::cos(
+                             CGAL::to_double(
+                               (angle_threshold * static_cast<FT>(CGAL_PI)) / FT(180)))))
+    , m_min_region_size(min_region_size)
+    , m_min_radius (minimum_radius)
+    , m_max_radius (maximum_radius)
+    , m_point_map(point_map)
+    , m_normal_map(normal_map)
+    , m_squared_length_3(traits.compute_squared_length_3_object())
+    , m_squared_distance_3(traits.compute_squared_distance_3_object())
+    , m_scalar_product_3(traits.compute_scalar_product_3_object())
+    , m_sqrt(Get_sqrt::sqrt_object(traits))
+  {
+    CGAL_precondition(input_range.size() > 0);
+    CGAL_precondition(distance_threshold >= FT(0));
+    CGAL_precondition(angle_threshold >= FT(0) && angle_threshold <= FT(90));
+    CGAL_precondition(min_region_size > 0);
+    CGAL_precondition(minimum_radius >= 0);
+    CGAL_precondition(maximum_radius > minimum_radius);
+  }
+
+  /// @}
+
+  /// \name Access
+  /// @{
+
+  /*!
+    \brief implements `RegionType::is_part_of_region()`.
+
+    This function controls if a point with the index `query_index` is
+    within the `distance_threshold` from the corresponding sphere and
+    if the angle between its normal and the sphere radius is within
+    the `angle_threshold`.  If both conditions are satisfied, it
+    returns `true`, otherwise `false`.
+
+    \param query_index index of the query point
+
+    \param indices indices of the inliers of the region
+
+    The first parameter is not used in this implementation.
+
+    \return Boolean `true` or `false`
+
+    \pre `query_index >= 0 && query_index < input_range.size()`
+  */
+  bool is_part_of_region (const std::size_t,
+                          const std::size_t query_index,
+                          const std::vector<std::size_t>& indices) const
+  {
+    CGAL_precondition(query_index < m_input_range.size());
+
+    // First, we need to integrate at least 6 points so that the
+    // computed sphere means something
+    if (indices.size() < 6)
+      return true;
+
+    // If radius is out of bound, nothing fits, early ending
+    if (m_radius < m_min_radius || m_radius > m_max_radius)
+      return false;
+
+    const auto& key = *(m_input_range.begin() + query_index);
+    const Point_3& query_point = get(m_point_map, key);
+    Vector_3 normal = get(m_normal_map, key);
+
+    FT distance_to_center = m_sqrt(m_squared_distance_3 (query_point, m_center));
+    FT distance_to_sphere = CGAL::abs (distance_to_center - m_radius);
+
+    if (distance_to_sphere > m_distance_threshold)
+      return false;
+
+    normal = normal / m_sqrt (normal * normal);
+
+    Vector_3 ray (m_center, query_point);
+    ray = ray / m_sqrt (ray * ray);
+
+    if (CGAL::abs (normal * ray) < m_normal_threshold)
+      return false;
+
+    return true;
+  }
+
+  /*!
+    \brief implements `RegionType::is_valid_region()`.
+
+    This function controls if the estimated radius is between
+    `minimum_radius` and `maximum_radius` and if the `region` contains
+    at least `min_region_size` points.
+
+    \param region
+    indices of points included in the region
+
+    \return Boolean `true` or `false`
+  */
+  inline bool is_valid_region(const std::vector<std::size_t>& region) const
+  {
+    return ((m_min_radius <= m_radius && m_radius <= m_max_radius)
+            && (region.size() >= m_min_region_size));
+  }
+
+  /*!
+    \brief implements `RegionType::update()`.
+
+    This function fits the least squares sphere to all points from the `region`.
+
+    \param region
+    indices of points included in the region
+
+    \pre `region.size() > 0`
+  */
+  void update(const std::vector<std::size_t>& 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));
+        };
+
+    // Use bbox to compute diagonalization with smaller coordinates to
+    // avoid loss of precision when inverting large coordinates
+    CGAL::Bbox_3 bbox = CGAL::bbox_3
+      (boost::make_transform_iterator
+       (region.begin(), unary_function),
+       boost::make_transform_iterator
+       (region.end(), unary_function));
+
+    using Diagonalize_traits = Eigen_diagonalize_traits<FT, 5>;
+    using Covariance_matrix = typename Diagonalize_traits::Covariance_matrix;
+    using Vector = typename Diagonalize_traits::Vector;
+    using Matrix = typename Diagonalize_traits::Matrix;
+
+    // Sphere least squares fitting
+    // (see Least_square_circle_fit_region for details about 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) };
+
+    A[0] = region.size();
+    for (const std::size_t& idx : region)
+    {
+      const auto& key = *(m_input_range.begin() + idx);
+      const Point_3& p = get(m_point_map, key);
+      FT x = p.x() - bbox.xmin();
+      FT y = p.y() - bbox.ymin();
+      FT z = p.z() - bbox.zmin();
+      FT r = x*x + y*y + z*z;
+      A[1] += x;
+      A[2] += y;
+      A[3] += z;
+      A[4] += r;
+      A[5] += x * x;
+      A[6] += x * y;
+      A[7] += x * z;
+      A[8] += x * r;
+      A[9] += y * y;
+      A[10] += y * z;
+      A[11] += y * r;
+      A[12] += z * z;
+      A[13] += z * r;
+      A[14] += r * r;
+    }
+
+    Vector eigenvalues = { FT(0), FT(0), FT(0), FT(0), FT(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) };
+
+    Diagonalize_traits::diagonalize_selfadjoint_covariance_matrix
+      (A, eigenvalues, eigenvectors);
+
+    m_center = Point_3 (bbox.xmin() - FT(0.5) * (eigenvectors[1] / eigenvectors[4]),
+                        bbox.ymin() - FT(0.5) * (eigenvectors[2] / eigenvectors[4]),
+                        bbox.zmin() - FT(0.5) * (eigenvectors[3] / eigenvectors[4]));
+
+    m_radius = FT(0);
+    for (const std::size_t& idx : region)
+    {
+      const auto& key = *(m_input_range.begin() + idx);
+      const Point_3& p = get(m_point_map, key);
+      m_radius += m_sqrt (m_squared_distance_3 (p, m_center));
+    }
+
+    m_radius /= region.size();
+  }
+
+  /// @}
+
+};
+
+} // namespace Point_set
+} // namespace Shape_detection
+} // namespace CGAL
+
+#endif // CGAL_SHAPE_DETECTION_REGION_GROWING_POINT_SET_LEAST_SQUARES_SPHERE_FIT_REGION_H