diff --git a/BGL/include/CGAL/boost/graph/parameters_interface.h b/BGL/include/CGAL/boost/graph/parameters_interface.h index 3273de59c53..d374c5ad7a0 100644 --- a/BGL/include/CGAL/boost/graph/parameters_interface.h +++ b/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) diff --git a/Point_set_processing_3/examples/Point_set_processing_3/registration_with_pointmatcher.cpp b/Point_set_processing_3/examples/Point_set_processing_3/registration_with_pointmatcher.cpp index 6e8292add18..531e1cdc52b 100644 --- a/Point_set_processing_3/examples/Point_set_processing_3/registration_with_pointmatcher.cpp +++ b/Point_set_processing_3/examples/Point_set_processing_3/registration_with_pointmatcher.cpp @@ -10,6 +10,7 @@ #include #include #include +#include typedef CGAL::Simple_cartesian 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 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 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 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 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 - (pwns1, pwns2, Point_map(), Point_map(), Normal_map(), Normal_map()); + CGAL::pointmatcher::compute_registration_transformation + (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 || diff --git a/Point_set_processing_3/include/CGAL/pointmatcher/compute_registration_transformation.h b/Point_set_processing_3/include/CGAL/pointmatcher/compute_registration_transformation.h index 6b8dd765e9c..d8d120c1a0f 100644 --- a/Point_set_processing_3/include/CGAL/pointmatcher/compute_registration_transformation.h +++ b/Point_set_processing_3/include/CGAL/pointmatcher/compute_registration_transformation.h @@ -5,16 +5,153 @@ #include -#include +#include +#include +#include +#include + +#include #include +#include +#include +#include + namespace CGAL { namespace pointmatcher { +template +using ICP = typename PointMatcher::ICP; + +struct ICP_config { + std::string name; + std::map 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 +ICP +construct_icp(const NamedParameters& np) +{ + typedef PointMatcher PM; + + using boost::choose_param; + using boost::get_param; + + ICP 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()); + 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()); + 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()); + 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()); + 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 icp) { using Scalar = typename Kernel::FT; @@ -104,27 +239,22 @@ 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 << "Ref Cloud points nb: " << ref_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; // 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 +#ifdef DOXYGEN_RUNNING +std::pair +#else +typename CGAL::Point_set_processing_3::GetK + ::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::type PointMap1; + typedef typename PSP::GetPointMap::type PointMap2; + CGAL_static_assertion_msg((boost::is_same< typename boost::property_traits::value_type, + typename boost::property_traits::value_type> ::value), + "The point type of input ranges must be the same"); + + typedef typename PSP::GetNormalMap::type NormalMap1; + typedef typename PSP::GetNormalMap::type NormalMap2; + CGAL_static_assertion_msg((boost::is_same< typename boost::property_traits::value_type, + typename boost::property_traits::value_type> ::value), + "The vector type of input ranges must be the same"); + + typedef typename PSP::GetK::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(point_set_1, point_set_2, + point_map1, point_map2, + normal_map1, normal_map2, + internal::construct_icp(np1)); +} + } } // end of namespace CGAL::pointmatcher #endif // CGAL_POINTMATCHER_COMPUTE_REGISTRATION_TRANSFORMATION_H