CGAL 5.6 - Point Set Processing
Point_set_processing_3/registration_with_pointmatcher.cpp
#include <CGAL/Simple_cartesian.h>
#include <CGAL/IO/read_points.h>
#include <CGAL/IO/write_points.h>
#include <CGAL/property_map.h>
#include <CGAL/Aff_transformation_3.h>
#include <CGAL/aff_transformation_tags.h>
#include <CGAL/pointmatcher/compute_registration_transformation.h>
#include <CGAL/pointmatcher/register_point_sets.h>
#include <fstream>
#include <iostream>
#include <utility>
#include <vector>
typedef K::Point_3 Point_3;
typedef K::Vector_3 Vector_3;
typedef std::pair<Point_3, Vector_3> Pwn;
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)
{
const std::string fname1 = (argc>1)?argv[1]:CGAL::data_file_path("points_3/hippo1.ply");
const std::string fname2 = (argc>2)?argv[2]:CGAL::data_file_path("points_3/hippo2.ply");
std::vector<Pwn> pwns1, pwns2;
if(!CGAL::IO::read_points(fname1, std::back_inserter(pwns1),
CGAL::parameters::point_map(CGAL::First_of_pair_property_map<Pwn>())
.normal_map(Normal_map())))
{
std::cerr << "Error: cannot read file " << fname1 << std::endl;
return EXIT_FAILURE;
}
if(!CGAL::IO::read_points(fname2, std::back_inserter(pwns2),
CGAL::parameters::point_map(Point_map())
.normal_map(Normal_map())))
{
std::cerr << "Error: cannot read file " << fname2 << std::endl;
return EXIT_FAILURE;
}
//
// Prepare ICP config
//
// Possible config modules/components: https://libpointmatcher.readthedocs.io/en/latest/Configuration/#configuration-of-an-icp-chain
// See documentation of optional named parameters for CGAL PM ICP configuration / pointmatcher config module mapping
// Prepare point set 1 filters (PM::ReferenceDataPointsFilters)
std::vector<ICP_config> point_set_1_filters;
point_set_1_filters.push_back( ICP_config { /*.name=*/"MinDistDataPointsFilter" , /*.params=*/{ {"minDist", "0.5" }} } );
point_set_1_filters.push_back( ICP_config { /*.name=*/"RandomSamplingDataPointsFilter", /*.params=*/{ {"prob" , "0.05"}} } );
// Prepare point set 2 filters (PM::ReadingDataPointsFilters)
std::vector<ICP_config> point_set_2_filters;
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
ICP_config matcher { /*.name=*/"KDTreeMatcher", /*.params=*/{ {"knn", "1"}, {"epsilon", "3.16"} } };
// Prepare outlier filters
// NOTE: `GenericDescriptorOutlierFilter` supports only one `descName` that is `weights`!
std::vector<ICP_config> 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", /*.params=*/{ } };
// 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", /*.params=*/{ } };
// Prepare logger
ICP_config logger { /*.name=*/"FileLogger", /*.params=*/{ } };
const K::Aff_transformation_3 identity_transform = K::Aff_transformation_3(CGAL::Identity_transformation());
// EITHER call the ICP registration method pointmatcher to get the transformation to apply to pwns2
std::pair<K::Aff_transformation_3, bool> res =
(pwns1, pwns2,
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)
.error_minimizer(error_minimizer)
.transformation_checkers(transformation_checkers)
.inspector(inspector)
.logger(logger),
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.
* a proper initial transform could be given, for example,
* a transform returned from a coarse registration algorithm.
* */
);
bool converged = false;
do
{
// OR call the ICP registration method from pointmatcher and apply the transformation to pwn2
converged =
(pwns1, pwns2,
params::point_map(Point_map()).normal_map(Normal_map())
.point_set_filters(point_set_1_filters)
.matcher(matcher)
.outlier_filters(outlier_filters)
.error_minimizer(error_minimizer)
.transformation_checkers(transformation_checkers)
.inspector(inspector)
.logger(logger),
params::point_map(Point_map()).normal_map(Normal_map())
.point_set_filters(point_set_2_filters)
.transformation(res.first) /* pass the above computed transformation as initial transformation.
* as a result, the registration will require less iterations to converge.
* */
);
// Algorithm may randomly not converge, repeat until it does
if (converged)
std::cerr << "Success" << std::endl;
else
std::cerr << "Did not converge, try again" << std::endl;
}
while (!converged);
if(!CGAL::IO::write_points("pwns2_aligned.ply", pwns2,
CGAL::parameters::point_map(Point_map()).normal_map(Normal_map())))
return EXIT_FAILURE;
std::cout << "Transformed version of " << fname2 << " written to pwn2_aligned.ply.\n";
return EXIT_SUCCESS;
}