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 7c7b5412f97..f2f76190b1e 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 @@ -88,7 +88,7 @@ int main(int argc, const char** argv) } ); // Prepare inspector ICP_config inspector { .name="NullInspector" }; - +// TODO: Do we really need to set logger through named parameters? (alternative: some other static method) // Prepare logger ICP_config logger { .name= "FileLogger" }; @@ -107,9 +107,7 @@ int main(int argc, const char** argv) params::point_map(Point_map()).normal_map(Normal_map()) .point_set_filters(point_set_2_filters)); - // OR call the ICP registration method from pointmatcher and apply the transformation to pwn1 - // TODO: Check the latest convention on ref point cloud. Currently, point_set_2 is the ref, as in pointmatcher - // Therefore, the transformation is applied on point_set_1 + // OR call the ICP registration method from pointmatcher and apply the transformation to pwn2 CGAL::pointmatcher::register_point_sets (pwns1, pwns2, params::point_map(Point_map()).normal_map(Normal_map()) 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 1db03a9de23..db77f267948 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 @@ -53,20 +53,12 @@ construct_icp(const NamedParameters1& np1, const NamedParameters2& np2) ICP_config null_config { .name = "_null_config" }; auto is_null_config = [&](const ICP_config& c) { return !c.name.compare(null_config.name); }; - // np1.point_set_filters -> PM::ReadingDataPointsFilters - auto reading_data_points_filter_configs = choose_param(get_param(np1, internal_np::point_set_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); - } - } + // In CGAL, point_set_1 is the reference while point_set_2 is the data + // However, in pointmatcher, the order is reverse: point_set_1 is the data while point_set_2 is the reference + // Therefore, filter params from np1 applies to reference data points while params from np2 applies to reading data points - // np2.point_set_filters -> PM::ReferenceDataPointsFilter - auto reference_data_points_filter_configs = choose_param(get_param(np2, internal_np::point_set_filters), std::vector()); + // np1.point_set_filters -> PM::ReferenceDataPointsFilter + auto reference_data_points_filter_configs = choose_param(get_param(np1, internal_np::point_set_filters), std::vector()); for(const auto& conf : reference_data_points_filter_configs) { std::cerr << "Reference data point filter found, name: " << conf.name << std::endl; @@ -77,6 +69,18 @@ construct_icp(const NamedParameters1& np1, const NamedParameters2& np2) } } + // np2.point_set_filters -> PM::ReadingDataPointsFilters + auto reading_data_points_filter_configs = choose_param(get_param(np2, internal_np::point_set_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); + } + } + // Matcher auto matcher_config = choose_param(get_param(np1, internal_np::matcher), null_config); if(!is_null_config(matcher_config)) @@ -210,7 +214,9 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2 PM_matrix ref_points_normal_matrix = PM_matrix (3, nb_ref_points); PM_matrix points_pos_matrix = PM_matrix (4, nb_points); PM_matrix points_normal_matrix = PM_matrix (3, nb_points); - + + // In CGAL, point_set_1 is the reference while point_set_2 is the data + // convert cgal points to pointmatcher points internal::copy_cgal_points_to_pm_matrix(range1, point_map1, @@ -244,6 +250,7 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2 try { const PM_transform_params prior = transform_params; + // In pointmatcher::icp, param1 is the data while param2 is the reference (in constrast to CGAL) transform_params = icp(cloud, ref_cloud, prior); // TODO: Convergence? Can we return some sort of score? std::cerr << transform_params << std::endl; // TODO: Remove @@ -270,8 +277,7 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2 } // end of namespace internal // TODO: Document -// TODO: Here, point_set_2 is the reference. Therefore, Aff_transformation_3 corresponds to the -// transformation that is suggested to be applied on point_set_1. Change the order? +// point_set_1 is reference while point_set_2 is data template #ifdef DOXYGEN_RUNNING @@ -320,7 +326,7 @@ template typename CGAL::Point_set_processing_3::GetK ::Kernel::Aff_transformation_3 -compute_registration_transformation(const PointRange1& point_set_1, PointRange2& point_set_2, +compute_registration_transformation(const PointRange1& point_set_1, const PointRange2& point_set_2, const NamedParameters1& np1) { namespace params = CGAL::Point_set_processing_3::parameters; @@ -331,7 +337,7 @@ template typename CGAL::Point_set_processing_3::GetK > ::Kernel::Aff_transformation_3 -compute_registration_transformation(const PointRange1& point_set_1, PointRange2& point_set_2) +compute_registration_transformation(const PointRange1& point_set_1, const PointRange2& point_set_2) { namespace params = CGAL::Point_set_processing_3::parameters; return compute_registration_transformation(point_set_1, point_set_2, diff --git a/Point_set_processing_3/include/CGAL/pointmatcher/register_point_sets.h b/Point_set_processing_3/include/CGAL/pointmatcher/register_point_sets.h index e2a52fc8be1..d1fb0f329ec 100644 --- a/Point_set_processing_3/include/CGAL/pointmatcher/register_point_sets.h +++ b/Point_set_processing_3/include/CGAL/pointmatcher/register_point_sets.h @@ -30,7 +30,7 @@ template void // TODO: Can we return some sort of score? -register_point_sets(PointRange1& range1, const PointRange2& range2, // TODO: Check which one is ref, currently range2 +register_point_sets(const PointRange1& range1, PointRange2& range2, PointMap1 point_map1, PointMap2 point_map2, VectorMap1 vector_map1, VectorMap2 vector_map2, ICP icp) @@ -42,13 +42,10 @@ register_point_sets(PointRange1& range1, const PointRange2& range2, // TODO: Che icp); // update CGAL points - // TODO: Here, point_set_2 is the reference. Therefore, Aff_transformation_3 corresponds to the - // transformation that is suggested to be applied on point_set_1. Check the latest convention - // with compute_registration_transformation() method - for (typename PointRange1::iterator it=range1.begin(), - end=range1.end(); it!=end; ++it) + for (typename PointRange2::iterator it=range2.begin(), + end=range2.end(); it!=end; ++it) { - put(point_map1, *it, get(point_map1, *it).transform(res)); + put(point_map2, *it, get(point_map2, *it).transform(res)); } } @@ -58,7 +55,7 @@ register_point_sets(PointRange1& range1, const PointRange2& range2, // TODO: Che template void // TODO: Can we return some sort of score? -register_point_sets (PointRange1& point_set_1, const PointRange2& point_set_2, // TODO: Check which one is ref, currently range2 +register_point_sets (const PointRange1& point_set_1, PointRange2& point_set_2, const NamedParameters1& np1, const NamedParameters2& np2) { using boost::choose_param; @@ -97,7 +94,7 @@ register_point_sets (PointRange1& point_set_1, const PointRange2& point_set_2, / template void // TODO: Can we return some sort of score? -register_point_sets(PointRange1& point_set_1, const PointRange2& point_set_2, // TODO: Check which one is ref, currently range2 +register_point_sets(const PointRange1& point_set_1, PointRange2& point_set_2, const NamedParameters1& np1) { namespace params = CGAL::Point_set_processing_3::parameters; @@ -106,7 +103,7 @@ register_point_sets(PointRange1& point_set_1, const PointRange2& point_set_2, // template void // TODO: Can we return some sort of score? -register_point_sets(PointRange1& point_set_1, const PointRange2& point_set_2) // TODO: Check which one is ref, currently range2 +register_point_sets(const PointRange1& point_set_1, PointRange2& point_set_2) { namespace params = CGAL::Point_set_processing_3::parameters; return register_point_sets(point_set_1, point_set_2,