added named parameters for pointmatcher icp config

Using CGAL::pointmatcher::ICP_config, config (name,params) could be passed to registration method through named parameters. Having pointmatcher style (name,params) pair as ICP_config allows to use extended pointmatcher configurations without needing to change wrapper implementation.

BGL/include/CGAL/boost/graph/parameters_interface.h

@@ -121,6 +121,14 @@ CGAL_add_named_parameter(plane_index_t, plane_index_map, plane_index_map)
 CGAL_add_named_parameter(select_percentage_t, select_percentage, select_percentage)
 CGAL_add_named_parameter(require_uniform_sampling_t, require_uniform_sampling, require_uniform_sampling)
 CGAL_add_named_parameter(point_is_constrained_t, point_is_constrained, point_is_constrained_map)
+CGAL_add_named_parameter(pm_reading_data_points_filters_t, pm_reading_data_points_filters, pm_reading_data_points_filters)
+CGAL_add_named_parameter(pm_reference_data_points_filters_t, pm_reference_data_points_filters, pm_reference_data_points_filters)
+CGAL_add_named_parameter(pm_matcher_t, pm_matcher, pm_matcher)
+CGAL_add_named_parameter(pm_outlier_filters_t, pm_outlier_filters, pm_outlier_filters)
+CGAL_add_named_parameter(pm_error_minimizer_t, pm_error_minimizer, pm_error_minimizer)
+CGAL_add_named_parameter(pm_transformation_checkers_t, pm_transformation_checkers, pm_transformation_checkers)
+CGAL_add_named_parameter(pm_inspector_t, pm_inspector, pm_inspector)
+CGAL_add_named_parameter(pm_logger_t, pm_logger, pm_logger)
 
 // List of named parameters used in Surface_mesh_approximation package
 CGAL_add_named_parameter(verbose_level_t, verbose_level, verbose_level)

Point_set_processing_3/examples/Point_set_processing_3/registration_with_pointmatcher.cpp

@@ -10,6 +10,7 @@
 #include <fstream>
 #include <iostream>
 #include <utility>
+#include <vector>
 
 typedef CGAL::Simple_cartesian<float> K;
 typedef K::Point_3 Point_3;
@@ -48,9 +49,64 @@ int main(int argc, const char** argv)
   }
   input.close();
 
+  // TODO: Another example with default settings, refering to default settings in doc
+  
+  //
+  // Prepare ICP config
+  //
+  using CGAL::pointmatcher::ICP_config;
+
+  // TODO: Change naming convention of filters? (for example, reference data points filters -> reference point set filters)
+  // TODO: Get reference point set filters from np2?
+  // TODO: Refer to https://github.com/ethz-asl/libpointmatcher/blob/master/doc/Configuration.md while doc
+
+  // Prepare reading data points filters
+  std::vector<ICP_config> reading_data_points_filters;
+  reading_data_points_filters.push_back( ICP_config { .name="MinDistDataPointsFilter"       , .params={ {"minDist", "0.5" }}  } );
+  reading_data_points_filters.push_back( ICP_config { .name="RandomSamplingDataPointsFilter", .params={ {"prob"   , "0.05"}}  } );
+
+  // Prepare reference data points filters
+  std::vector<ICP_config> reference_data_points_filters;
+  reference_data_points_filters.push_back( ICP_config { .name="MinDistDataPointsFilter"       , .params={ {"minDist", "0.5" }}  } );
+  reference_data_points_filters.push_back( ICP_config { .name="RandomSamplingDataPointsFilter", .params={ {"prob"   , "0.05"}}  } );
+
+	// Prepare matcher function
+  ICP_config matcher { .name="KDTreeMatcher", .params={ {"knn", "1"}, {"epsilon", "3.16"} } };
+
+  // Prepare outlier filters
+  std::vector<ICP_config> outlier_filters;
+  outlier_filters.push_back( ICP_config { .name="TrimmedDistOutlierFilter", .params={ {"ratio", "0.75" }}  } );
+
+  // Prepare error minimizer
+  ICP_config error_minimizer { .name = "PointToPointErrorMinimizer"};
+
+  // Prepare transformation checker
+  std::vector<ICP_config> transformation_checkers;
+  transformation_checkers.push_back( ICP_config { .name="CounterTransformationChecker", .params={ {"maxIterationCount", "150" }}  } );
+  transformation_checkers.push_back( ICP_config { .name="DifferentialTransformationChecker", .params={ {"minDiffRotErr"  , "0.001" },
+                                                                                                       {"minDiffTransErr", "0.01"  },
+                                                                                                       {"smoothLength"   , "4"     } }  
+                                                } );
+  // Prepare inspector
+  ICP_config inspector { .name="NullInspector" };
+
+  // Prepare logger
+  ICP_config logger { .name= "FileLogger" };
+
+
 K::Aff_transformation_3 res = 
-  CGAL::pointmatcher::compute_registration_transformation<K>
+  CGAL::pointmatcher::compute_registration_transformation
-    (pwns1, pwns2, Point_map(), Point_map(), Normal_map(), Normal_map());
+    (pwns1, pwns2, 
+     params::point_map(Point_map()).normal_map(Normal_map())
+     .pm_reading_data_points_filters(reading_data_points_filters)
+     .pm_reference_data_points_filters(reference_data_points_filters)
+     .pm_matcher(matcher)
+     .pm_outlier_filters(outlier_filters)
+     .pm_error_minimizer(error_minimizer)
+     .pm_transformation_checkers(transformation_checkers)
+     .pm_inspector(inspector)
+     .pm_logger(logger),
+     params::point_map(Point_map()).normal_map(Normal_map()));
 
   std::ofstream out("pwns2_aligned.ply");
   if (!out ||

Point_set_processing_3/include/CGAL/pointmatcher/compute_registration_transformation.h

@@ -5,16 +5,153 @@
 
 #include <CGAL/license/Point_set_processing_3.h>
 
-#include <CGAL/Point_set_3.h>
+#include <CGAL/Aff_transformation_3.h>
+#include <CGAL/assertions.h>
+#include <CGAL/boost/graph/named_function_params.h>
+#include <CGAL/boost/graph/named_params_helper.h>
+
+#include <boost/type_traits/is_same.hpp>
 
 #include <pointmatcher/PointMatcher.h>
 
+#include <iostream>
+#include <string>
+#include <map>
+
 namespace CGAL {
 
 namespace pointmatcher {
 
+template<typename Scalar>
+using ICP = typename PointMatcher<Scalar>::ICP;
+
+struct ICP_config {
+  std::string name;
+  std::map<std::string, std::string> params;
+};
+
 namespace internal {
 
+void dump_invalid_point_matcher_config_exception_msg(const PointMatcherSupport::InvalidElement& err) {
+  std::cerr << "ERROR Invalid configuration for PM::ICP, omitting configuration: " << std::endl;
+	std::cerr << "   " << err.what() << std::endl;
+}
+
+template<typename Scalar, typename NamedParameters>
+ICP<Scalar>
+construct_icp(const NamedParameters& np)
+{
+  typedef PointMatcher<Scalar> PM;
+
+  using boost::choose_param;
+  using boost::get_param;
+  
+  ICP<Scalar> icp;
+
+  icp.setDefault();
+
+  ICP_config null_config { .name = "_null_config" };
+  auto is_null_config = [&](const ICP_config& c) { return !c.name.compare(null_config.name); };
+
+  // ReadingDataPointsFilters
+  auto reading_data_points_filter_configs = choose_param(get_param(np, internal_np::pm_reading_data_points_filters), std::vector<ICP_config>());
+  for(const auto& conf : reading_data_points_filter_configs)
+  {
+    std::cerr << "Reading data point filter found, name: " << conf.name << std::endl;
+    try {
+      icp.readingDataPointsFilters.push_back( PM::get().DataPointsFilterRegistrar.create(conf.name, conf.params) );
+    } catch(typename PointMatcherSupport::InvalidElement& error) {
+      dump_invalid_point_matcher_config_exception_msg(error);
+    }
+  }
+
+  // RefenceDataPointsFilters
+  auto reference_data_points_filter_configs = choose_param(get_param(np, internal_np::pm_reference_data_points_filters), std::vector<ICP_config>());
+  for(const auto& conf : reference_data_points_filter_configs)
+  {
+    std::cerr << "Reference data point filter found, name: " << conf.name << std::endl;
+    try {
+      icp.referenceDataPointsFilters.push_back( PM::get().DataPointsFilterRegistrar.create(conf.name, conf.params) );
+    } catch(typename PointMatcherSupport::InvalidElement& error) {
+      dump_invalid_point_matcher_config_exception_msg(error);
+    }
+  }
+
+  // Matcher
+  auto matcher_config = choose_param(get_param(np, internal_np::pm_matcher), null_config);
+  if(!is_null_config(matcher_config))
+  {
+    std::cerr << "Matcher found, setting matcher to: " << matcher_config.name << std::endl;
+    try {
+      icp.matcher = PM::get().MatcherRegistrar.create(matcher_config.name, matcher_config.params);
+    } catch(typename PointMatcherSupport::InvalidElement& error) {
+      dump_invalid_point_matcher_config_exception_msg(error);
+    }
+  }
+
+  // Outlier Filters
+  auto outlier_filters_config = choose_param(get_param(np, internal_np::pm_outlier_filters), std::vector<ICP_config>());
+  for(const auto& conf : outlier_filters_config)
+  {
+    std::cerr << "Outlier filter found, name: " << conf.name << std::endl;
+    try {
+      icp.outlierFilters.push_back( PM::get().OutlierFilterRegistrar.create(conf.name, conf.params) );
+    } catch(typename PointMatcherSupport::InvalidElement& error) {
+      dump_invalid_point_matcher_config_exception_msg(error);
+    }
+  }
+
+  // Error Minimizer
+  auto error_minimizer_config = choose_param(get_param(np, internal_np::pm_error_minimizer), null_config);
+  if(!is_null_config(error_minimizer_config))
+  {
+    std::cerr << "Error minimizer found, setting to: " << error_minimizer_config.name << std::endl;
+    try {
+      icp.errorMinimizer = PM::get().ErrorMinimizerRegistrar.create(error_minimizer_config.name, error_minimizer_config.params);
+    } catch(typename PointMatcherSupport::InvalidElement& error) {
+      dump_invalid_point_matcher_config_exception_msg(error);
+    }
+  }
+
+  // Transformation Checkers
+  auto transformation_checkers_config = choose_param(get_param(np, internal_np::pm_transformation_checkers), std::vector<ICP_config>());
+  for(const auto& conf : transformation_checkers_config)
+  {
+    std::cerr << "Transformation checker found, name: " << conf.name << std::endl;
+    try {
+      icp.transformationCheckers.push_back( PM::get().TransformationCheckerRegistrar.create(conf.name, conf.params) );
+    } catch(typename PointMatcherSupport::InvalidElement& error) {
+      dump_invalid_point_matcher_config_exception_msg(error);
+    }
+  }
+
+  // Inspector
+  auto inspector_config = choose_param(get_param(np, internal_np::pm_inspector), null_config);
+  if(!is_null_config(error_minimizer_config))
+  {
+    std::cerr << "Inspector found, setting to: " << inspector_config.name << std::endl;
+    try {
+      icp.inspector = PM::get().InspectorRegistrar.create(inspector_config.name, inspector_config.params);
+    } catch(typename PointMatcherSupport::InvalidElement& error) {
+      dump_invalid_point_matcher_config_exception_msg(error);
+    }
+  }
+
+  // Logger
+  auto logger_config = choose_param(get_param(np, internal_np::pm_logger), null_config);
+  if(!is_null_config(logger_config))
+  {
+    std::cerr << "Logger found, setting to: " << logger_config.name << std::endl;
+    try {
+      PointMatcherSupport::setLogger( PM::get().LoggerRegistrar.create(logger_config.name, logger_config.params) );
+    } catch(typename PointMatcherSupport::InvalidElement& error) {
+      dump_invalid_point_matcher_config_exception_msg(error);
+    }
+  }
+
+  return icp;
+}
+
 template<typename Scalar,
          typename PointRange,
          typename PointMap,
@@ -44,8 +181,6 @@ copy_cgal_points_to_pm_matrix
   }
 }
 
-} // end of namespace internal
-
 template <class Kernel,
           class PointRange1,
           class PointRange2,
@@ -56,7 +191,7 @@ template <class Kernel,
 typename Kernel::Aff_transformation_3
 compute_registration_transformation(const PointRange1& range1, const PointRange2& range2,
                                     PointMap1 point_map1, PointMap2 point_map2,
-                                    VectorMap1 vector_map1, VectorMap2 vector_map2)
+                                    VectorMap1 vector_map1, VectorMap2 vector_map2, ICP<typename Kernel::FT> icp)
 {
   using Scalar    = typename Kernel::FT;
   
@@ -105,26 +240,21 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2
   PM_cloud ref_cloud = construct_PM_cloud(ref_points_pos_matrix, ref_points_normal_matrix);
   PM_cloud cloud     = construct_PM_cloud(points_pos_matrix,     points_normal_matrix);
   
-  typename PM::ICP icp;
-  
-  // TODO: Make it configurable? Use named parameters to have possible settings?
-  icp.setDefault();
-  
   PM_transform_params transform_params = PM_transform_params::Identity(4,4);
   try 
   {
 		const PM_transform_params prior = transform_params;
-    std::cerr << "Cloud points nb: " << cloud.getNbPoints() << std::endl;
+    std::cerr << "Cloud points nb: " << cloud.getNbPoints() << std::endl; // TODO: Remove
-    std::cerr << "Ref Cloud points nb: " << ref_cloud.getNbPoints() << std::endl;
+    std::cerr << "Ref Cloud points nb: " << ref_cloud.getNbPoints() << std::endl; // TODO: Remove
 		transform_params = icp(cloud, ref_cloud, prior);
-    std::cerr << transform_params << std::endl;
+    // TODO: Convergence?
+    std::cerr << transform_params << std::endl; // TODO: Remove
 	}
 	catch (typename PM::ConvergenceError& error)
 	{
 		std::cerr << "ERROR PM::ICP failed to converge: " << std::endl;
 		std::cerr << "   " << error.what() << std::endl;
     // TODO: What to do?
-		//continue;
 	}
 
 	// Rigid transformation
@@ -139,6 +269,52 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2
   return cgal_transform;
 }
 
+} // end of namespace internal
+
+template <class PointRange1, class PointRange2,
+          class NamedParameters1, class NamedParameters2>
+#ifdef DOXYGEN_RUNNING
+std::pair<geom_traits::Aff_transformation_3, double>
+#else
+typename CGAL::Point_set_processing_3::GetK<PointRange1, NamedParameters1>
+  ::Kernel::Aff_transformation_3
+#endif
+compute_registration_transformation (const PointRange1& point_set_1, const PointRange2& point_set_2,
+                                     const NamedParameters1& np1, const NamedParameters2& np2)
+{
+  using boost::choose_param;
+  using boost::get_param;
+
+  // Parse named parameters in some other function to get it as pointmatcher parameters
+  namespace PSP = CGAL::Point_set_processing_3;
+
+  // property map types
+  typedef typename PSP::GetPointMap<PointRange1, NamedParameters1>::type PointMap1;
+  typedef typename PSP::GetPointMap<PointRange2, NamedParameters2>::type PointMap2;
+  CGAL_static_assertion_msg((boost::is_same< typename boost::property_traits<PointMap1>::value_type,
+                                             typename boost::property_traits<PointMap2>::value_type> ::value),
+                            "The point type of input ranges must be the same");
+
+  typedef typename PSP::GetNormalMap<PointRange1, NamedParameters1>::type NormalMap1;
+  typedef typename PSP::GetNormalMap<PointRange2, NamedParameters2>::type NormalMap2;
+  CGAL_static_assertion_msg((boost::is_same< typename boost::property_traits<NormalMap1>::value_type,
+                                             typename boost::property_traits<NormalMap2>::value_type> ::value),
+                            "The vector type of input ranges must be the same");
+
+  typedef typename PSP::GetK<PointRange1, NamedParameters1>::Kernel Kernel;
+  typedef typename Kernel::FT Scalar;
+
+  PointMap1 point_map1 = choose_param(get_param(np1, internal_np::point_map), PointMap1());
+  NormalMap1 normal_map1 = choose_param(get_param(np1, internal_np::normal_map), NormalMap1());
+  PointMap2 point_map2 = choose_param(get_param(np2, internal_np::point_map), PointMap2());
+  NormalMap2 normal_map2 = choose_param(get_param(np2, internal_np::normal_map), NormalMap2());
+
+  return internal::compute_registration_transformation<Kernel>(point_set_1, point_set_2,
+                                                               point_map1, point_map2,
+                                                               normal_map1, normal_map2,
+                                                               internal::construct_icp<Scalar>(np1));
+}
+
 } } // end of namespace CGAL::pointmatcher
 
 #endif // CGAL_POINTMATCHER_COMPUTE_REGISTRATION_TRANSFORMATION_H