iterative closest points
CEV ICP algorithm library
Loading...
Searching...
No Matches
icp.h
Go to the documentation of this file.
1
7#pragma once
8
9#include <cmath>
10#include <vector>
11#include <memory>
12#include <string>
13#include <functional>
14#include <unordered_map>
15#include <variant>
16#include <optional>
17
18#include "geo.h"
19
20namespace icp {
49 class ICP {
50 protected:
52 struct Match {
53 size_t point;
54 size_t pair;
55 double cost;
56 };
57
60
62 std::vector<Vector> a;
63
65 std::vector<Vector> b;
66
68 std::vector<Match> matches;
69
70 ICP();
71
77 virtual void setup() = 0;
78
79 public:
81 class Config {
82 using Param = std::variant<int, double, std::string>;
83 std::unordered_map<std::string, Param> params;
84
85 public:
87 Config() {}
88
90 template<typename T>
91 void set(std::string key, T value) {
92 params[key] = value;
93 }
94
97 template<typename T>
98 T get(std::string key, T otherwise) const {
99 if (params.find(key) == params.end()) {
100 return otherwise;
101 } else {
102 return std::get<T>(params.at(key));
103 }
104 }
105 };
106
107 virtual ~ICP() = default;
108
111 void begin(const std::vector<Vector>& a, const std::vector<Vector>& b, RBTransform t);
112
119 virtual void iterate() = 0;
120
127 double calculate_cost() const;
128
130 const RBTransform& current_transform() const;
131
134 static bool register_method(std::string name,
135 std::function<std::unique_ptr<ICP>(const Config&)> constructor);
136
138 static bool is_method_registered(std::string name);
139
142 static const std::vector<std::string>& registered_methods();
143
151 static std::optional<std::unique_ptr<ICP>> from_method(std::string name,
152 const Config& params = Config());
153
154 private:
155 struct Methods {
156 std::vector<std::string> registered_method_names;
157 std::vector<std::function<std::unique_ptr<ICP>(const ICP::Config&)>>
158 registered_method_constructors;
159 };
160
161 static Methods global;
162 static bool builtins_registered;
163
166 static void ensure_builtins_registered();
167
169 static void register_method_internal(std::string name,
170 std::function<std::unique_ptr<ICP>(const Config&)> constructor);
171 };
172}
Configuration for ICP instances.
Definition icp.h:81
Config()
Constructs an empty configuration.
Definition icp.h:87
T get(std::string key, T otherwise) const
Retrieves the integer, double, or string value associated with key.
Definition icp.h:98
void set(std::string key, T value)
Associates key with an integer, double, or string value.
Definition icp.h:91
Interface for iterative closest points.
Definition icp.h:49
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 ...
Definition icp.cpp:51
static std::optional< std::unique_ptr< ICP > > from_method(std::string name, const Config &params=Config())
Factory constructor for the ICP method name with configuration config.
Definition icp.cpp:77
virtual void setup()=0
Per-method setup code.
Definition icp.cpp:19
RBTransform transform
The current point cloud transformation that is being optimized.
Definition icp.h:59
static bool is_method_registered(std::string name)
Returns true if the ICP method name is registered.
Definition icp.cpp:63
ICP()
Definition icp.cpp:17
std::vector< Vector > a
The source point cloud.
Definition icp.h:62
virtual ~ICP()=default
static const std::vector< std::string > & registered_methods()
Returns a current list of the names of currently registered ICP methods.
Definition icp.cpp:71
std::vector< Vector > b
The destination point cloud.
Definition icp.h:65
std::vector< Match > matches
The pairing of each point in a to its closest in b.
Definition icp.h:68
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.
Definition icp.cpp:21
const RBTransform & current_transform() const
The current transform.
Definition icp.cpp:44
virtual void iterate()=0
Perform one iteration of ICP for the point clouds a and b provided with ICP::begin.
double calculate_cost() const
Computes the cost of the current transformation.
Definition icp.cpp:36
Definition driver.h:12
A matching between point and pair at (arbitrary) cost cost.
Definition icp.h:52
size_t pair
Definition icp.h:54
size_t point
Definition icp.h:53
double cost
Definition icp.h:55
Rigid-body transformation.
Definition geo.h:19