mirror of https://github.com/CGAL/cgal
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.
This commit is contained in:
Necip Yildiran
parent
a9889a12e3
commit
a87cc959d4
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
(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 ||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
@ -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 <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
|
||||
|
|
|
|||
Loading…
Reference in New Issue