From c8307dc0d66ff1aa6f7e47e94e0ac0ff9bbae24d Mon Sep 17 00:00:00 2001 From: Dmitry Anisimov Date: Thu, 29 Apr 2021 15:23:38 +0200 Subject: [PATCH] added support for GenericDescriptorOutlierFilter to the CGAL libpointmatcher interface --- .../CGAL/boost/graph/named_params_helper.h | 24 +++++++++ .../CGAL/boost/graph/parameters_interface.h | 1 + .../registration_with_pointmatcher.cpp | 18 +++++-- .../compute_registration_transformation.h | 52 ++++++++++++++++--- 4 files changed, 84 insertions(+), 11 deletions(-) diff --git a/BGL/include/CGAL/boost/graph/named_params_helper.h b/BGL/include/CGAL/boost/graph/named_params_helper.h index b3a32e15dc4..53969a90367 100644 --- a/BGL/include/CGAL/boost/graph/named_params_helper.h +++ b/BGL/include/CGAL/boost/graph/named_params_helper.h @@ -435,6 +435,30 @@ CGAL_DEF_GET_INITIALIZED_INDEX_MAP(face, typename boost::graph_traits::fa > ::type type; }; + template + class GetScalarMap + { + struct DummyScalarMap + { + typedef typename std::iterator_traits::value_type key_type; + typedef typename GetK::Kernel::FT value_type; + typedef value_type reference; + typedef boost::read_write_property_map_tag category; + + typedef DummyScalarMap Self; + friend reference get(const Self&, const key_type&) { return value_type(1); } + friend void put(const Self&, const key_type&, const value_type&) { } + }; + + public: + typedef DummyScalarMap NoMap; + typedef typename internal_np::Lookup_named_param_def < + internal_np::scalar_t, + NamedParameters, + DummyScalarMap // default + > ::type type; + }; + template class GetPlaneMap { diff --git a/BGL/include/CGAL/boost/graph/parameters_interface.h b/BGL/include/CGAL/boost/graph/parameters_interface.h index a1fe60c5f75..cf944feb899 100644 --- a/BGL/include/CGAL/boost/graph/parameters_interface.h +++ b/BGL/include/CGAL/boost/graph/parameters_interface.h @@ -174,6 +174,7 @@ CGAL_add_named_parameter(pointmatcher_config_t, pointmatcher_config, pointmatche CGAL_add_named_parameter(adjacencies_t, adjacencies, adjacencies) CGAL_add_named_parameter(scan_angle_t, scan_angle_map, scan_angle_map) CGAL_add_named_parameter(scanline_id_t, scanline_id_map, scanline_id_map) +CGAL_add_named_parameter(scalar_t, scalar_map, scalar_map) // 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 cfbb6463d16..c4893acaa01 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 @@ -21,6 +21,17 @@ typedef std::pair Pwn; typedef CGAL::First_of_pair_property_map Point_map; typedef CGAL::Second_of_pair_property_map Normal_map; +struct Weight_map +{ + typedef Pwn key_type; + typedef typename K::FT value_type; + typedef value_type reference; + typedef boost::readable_property_map_tag category; + + typedef Weight_map Self; + friend reference get(const Self&, const key_type&) { return value_type(1); } +}; + namespace params = CGAL::parameters; int main(int argc, const char** argv) @@ -63,12 +74,13 @@ int main(int argc, const char** argv) point_set_2_filters.push_back( ICP_config { /*.name=*/"MinDistDataPointsFilter" , /*.params=*/{ {"minDist", "0.5" }} } ); point_set_2_filters.push_back( ICP_config { /*.name=*/"RandomSamplingDataPointsFilter", /*.params=*/{ {"prob" , "0.05"}} } ); - // Prepare matcher function + // 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" }} } ); + outlier_filters.push_back( ICP_config { /*.name=*/"GenericDescriptorOutlierFilter", /*.params=*/{ {"descName", "weights" }} } ); // Prepare error minimizer ICP_config error_minimizer { /*.name=*/"PointToPointErrorMinimizer"}; @@ -92,7 +104,7 @@ int main(int argc, const char** argv) std::pair res = CGAL::pointmatcher::compute_registration_transformation (pwns1, pwns2, - params::point_map(Point_map()).normal_map(Normal_map()) + params::point_map(Point_map()).normal_map(Normal_map()).scalar_map(Weight_map()) .point_set_filters(point_set_1_filters) .matcher(matcher) .outlier_filters(outlier_filters) @@ -100,7 +112,7 @@ int main(int argc, const char** argv) .transformation_checkers(transformation_checkers) .inspector(inspector) .logger(logger), - params::point_map(Point_map()).normal_map(Normal_map()) + params::point_map(Point_map()).normal_map(Normal_map()).scalar_map(Weight_map()) .point_set_filters(point_set_2_filters) .transformation(identity_transform) /* initial transform for pwns2. * default value is already identity transform. 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 d561b3b26a4..735414a3766 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 @@ -220,10 +220,11 @@ template void copy_cgal_points_to_pm_matrix -(const PointRange& prange, PointMap point_map, VectorMap vector_map, PM_matrix& pm_points, PM_matrix& pm_normals) +(const PointRange& prange, PointMap point_map, VectorMap vector_map, ScalarMap scalar_map, PM_matrix& pm_points, PM_matrix& pm_normals, PM_matrix& pm_weights) { int idx = 0; for(const auto& p : prange) @@ -236,11 +237,15 @@ copy_cgal_points_to_pm_matrix pm_points(3, idx) = Scalar(1.); // normal - const auto& normal = get (vector_map, p); + const auto& normal = get(vector_map, p); pm_normals(0, idx) = normal.x(); pm_normals(1, idx) = normal.y(); pm_normals(2, idx) = normal.z(); + // weight + const auto& weight = get(scalar_map, p); + pm_weights(0, idx) = weight; + ++idx; } } @@ -251,11 +256,14 @@ template + class VectorMap2, + class ScalarMap1, + class ScalarMap2> std::pair compute_registration_transformation(const PointRange1& range1, const PointRange2& range2, PointMap1 point_map1, PointMap2 point_map2, VectorMap1 vector_map1, VectorMap2 vector_map2, + ScalarMap1 scalar_map1, ScalarMap2 scalar_map2, const typename Kernel::Aff_transformation_3& initial_transform, ICP icp) { @@ -273,8 +281,10 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2 PM_matrix ref_points_pos_matrix = PM_matrix (4, nb_ref_points); PM_matrix ref_points_normal_matrix = PM_matrix (3, nb_ref_points); + PM_matrix ref_points_weight_matrix = PM_matrix (1, nb_ref_points); PM_matrix points_pos_matrix = PM_matrix (4, nb_points); PM_matrix points_normal_matrix = PM_matrix (3, nb_points); + PM_matrix points_weight_matrix = PM_matrix (1, nb_points); // In CGAL, point_set_1 is the reference while point_set_2 is the data @@ -282,16 +292,20 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2 internal::copy_cgal_points_to_pm_matrix(range1, point_map1, vector_map1, + scalar_map1, ref_points_pos_matrix, // out - ref_points_normal_matrix); // out + ref_points_normal_matrix, // out + ref_points_weight_matrix); // out internal::copy_cgal_points_to_pm_matrix(range2, point_map2, vector_map2, + scalar_map2, points_pos_matrix, // out - points_normal_matrix); // out + points_normal_matrix, // out + points_weight_matrix); // out - auto construct_PM_cloud = [](const PM_matrix& positions, const PM_matrix& normals) -> PM_cloud + auto construct_PM_cloud = [](const PM_matrix& positions, const PM_matrix& normals, const PM_matrix& weights) -> PM_cloud { PM_cloud cloud; @@ -300,12 +314,13 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2 cloud.addFeature("z", positions.row(2)); cloud.addFeature("pad", positions.row(3)); cloud.addDescriptor("normals", normals); + cloud.addDescriptor("weights", weights); return cloud; }; - 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); + PM_cloud ref_cloud = construct_PM_cloud(ref_points_pos_matrix, ref_points_normal_matrix, ref_points_weight_matrix); + PM_cloud cloud = construct_PM_cloud(points_pos_matrix, points_normal_matrix, points_weight_matrix); PM_transform_params pm_transform_params = PM_transform_params::Identity(4,4); @@ -385,6 +400,12 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2 of the iterator of `PointRange1` and whose value type is `geom_traits::Vector_3`} \cgalParamNEnd + \cgalParamNBegin{scalar_map} + \cgalParamDescription{a property map associating 1D values - scalars to the elements of the point set `point_set_1`} + \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type + of the iterator of `PointRange1` and whose value type is `geom_traits::FT`} + \cgalParamNEnd + \cgalParamNBegin{point_set_filters} \cgalParamDescription{a chain of filters to be applied to the point set} \cgalParamType{a class model of `Range`. The value type of its iterator must be `ICP_config`.} @@ -510,6 +531,12 @@ compute_registration_transformation(const PointRange1& range1, const PointRange2 of the iterator of `PointRange2` and whose value type is `geom_traits::Vector_3`} \cgalParamNEnd + \cgalParamNBegin{scalar_map} + \cgalParamDescription{a property map associating 1D values - scalars to the elements of the point set `point_set_2`} + \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type + of the iterator of `PointRange2` and whose value type is `geom_traits::FT`} + \cgalParamNEnd + \cgalParamNBegin{point_set_filters} \cgalParamDescription{a chain of filters to be applied to the point set} \cgalParamType{a class model of `Range`. The value type of its iterator must be `ICP_config`.} @@ -576,14 +603,22 @@ compute_registration_transformation (const PointRange1& point_set_1, const Point typename boost::property_traits::value_type> ::value), "The vector type of input ranges must be the same"); + typedef typename PSP::GetScalarMap::type WeightMap1; + typedef typename PSP::GetScalarMap::type WeightMap2; + CGAL_static_assertion_msg((boost::is_same< typename boost::property_traits::value_type, + typename boost::property_traits::value_type> ::value), + "The scalar type of input ranges must be the same"); + typedef typename PSP::GetK::Kernel Kernel; typedef typename Kernel::FT Scalar; typedef typename Kernel::Aff_transformation_3 Transformation; PointMap1 point_map1 = choose_parameter(get_parameter(np1, internal_np::point_map), PointMap1()); NormalMap1 normal_map1 = choose_parameter(get_parameter(np1, internal_np::normal_map), NormalMap1()); + WeightMap1 weight_map1 = choose_parameter(get_parameter(np1, internal_np::scalar_map), WeightMap1()); PointMap2 point_map2 = choose_parameter(get_parameter(np2, internal_np::point_map), PointMap2()); NormalMap2 normal_map2 = choose_parameter(get_parameter(np2, internal_np::normal_map), NormalMap2()); + WeightMap2 weight_map2 = choose_parameter(get_parameter(np2, internal_np::scalar_map), WeightMap2()); // initial transformation Transformation initial_transformation @@ -592,6 +627,7 @@ compute_registration_transformation (const PointRange1& point_set_1, const Point return internal::compute_registration_transformation(point_set_1, point_set_2, point_map1, point_map2, normal_map1, normal_map2, + weight_map1, weight_map2, initial_transformation, internal::construct_icp(np1, np2)); }