39 sum_squares += match.cost;
41 return std::sqrt(sum_squares /
a.size());
48 ICP::Methods ICP::global;
49 bool ICP::builtins_registered =
false;
52 std::function<std::unique_ptr<ICP>(
const ICP::Config&)> constructor) {
53 ensure_builtins_registered();
59 register_method_internal(name, constructor);
64 ensure_builtins_registered();
66 return std::find(global.registered_method_names.begin(),
67 global.registered_method_names.end(), name)
68 != global.registered_method_names.end();
72 ensure_builtins_registered();
74 return global.registered_method_names;
79 ensure_builtins_registered();
81 auto name_it = std::find(global.registered_method_names.begin(),
82 global.registered_method_names.end(), name);
84 if (name_it == global.registered_method_names.end()) {
88 size_t index = name_it - global.registered_method_names.begin();
90 return global.registered_method_constructors[index](config);
93 void ICP::ensure_builtins_registered() {
94 if (builtins_registered) {
98 register_method_internal(
"vanilla",
99 [](
const ICP::Config& config) {
return std::make_unique<Vanilla>(config); });
100 register_method_internal(
"trimmed",
101 [](
const ICP::Config& config) {
return std::make_unique<Trimmed>(config); });
102 register_method_internal(
"feature_aware",
103 [](
const ICP::Config& config) {
return std::make_unique<FeatureAware>(config); });
105 builtins_registered =
true;
108 void ICP::register_method_internal(std::string name,
109 std::function<std::unique_ptr<ICP>(
const Config&)> constructor) {
110 global.registered_method_constructors.push_back(constructor);
111 global.registered_method_names.push_back(name);
Configuration for ICP instances.
static bool register_method(std::string name, std::function< std::unique_ptr< ICP >(const Config &)> constructor)
Registers a new ICP method that can be created with constructor, returning false if name has already ...
static std::optional< std::unique_ptr< ICP > > from_method(std::string name, const Config ¶ms=Config())
Factory constructor for the ICP method name with configuration config.
virtual void setup()=0
Per-method setup code.
RBTransform transform
The current point cloud transformation that is being optimized.
static bool is_method_registered(std::string name)
Returns true if the ICP method name is registered.
std::vector< Vector > a
The source point cloud.
static const std::vector< std::string > & registered_methods()
Returns a current list of the names of currently registered ICP methods.
std::vector< Vector > b
The destination point cloud.
std::vector< Match > matches
The pairing of each point in a to its closest in b.
void begin(const std::vector< Vector > &a, const std::vector< Vector > &b, RBTransform t)
Begins the ICP process for point clouds a and b with an initial guess for the transform t.
const RBTransform & current_transform() const
The current transform.
double calculate_cost() const
Computes the cost of the current transformation.