iterative closest points
CEV ICP algorithm library
Loading...
Searching...
No Matches
icp.cpp
Go to the documentation of this file.
1
7#include <numeric>
8
9#include "icp/icp.h"
10
11// methods for builtin registration
12#include "icp/impl/vanilla.h"
13#include "icp/impl/trimmed.h"
15
16namespace icp {
18
19 void ICP::setup() {}
20
21 void ICP::begin(const std::vector<Vector>& a, const std::vector<Vector>& b, RBTransform t) {
22 // Initial transform guess
23 this->transform = t;
24
25 // Copy in point clouds
26 this->a = a;
27 this->b = b;
28
29 // Ensure arrays are the right size
30 matches.resize(this->a.size());
31
32 // Per-instance customization routine
33 setup();
34 }
35
36 double ICP::calculate_cost() const {
37 double sum_squares{};
38 for (auto& match: matches) {
39 sum_squares += match.cost;
40 }
41 return std::sqrt(sum_squares / a.size());
42 }
43
45 return transform;
46 }
47
48 ICP::Methods ICP::global;
49 bool ICP::builtins_registered = false;
50
51 bool ICP::register_method(std::string name,
52 std::function<std::unique_ptr<ICP>(const ICP::Config&)> constructor) {
53 ensure_builtins_registered();
54
55 if (is_method_registered(name)) {
56 return false;
57 }
58
59 register_method_internal(name, constructor);
60 return true;
61 }
62
63 bool ICP::is_method_registered(std::string name) {
64 ensure_builtins_registered();
65
66 return std::find(global.registered_method_names.begin(),
67 global.registered_method_names.end(), name)
68 != global.registered_method_names.end();
69 }
70
71 const std::vector<std::string>& ICP::registered_methods() {
72 ensure_builtins_registered();
73
74 return global.registered_method_names;
75 }
76
77 std::optional<std::unique_ptr<ICP>> ICP::from_method(std::string name,
78 const ICP::Config& config) {
79 ensure_builtins_registered();
80
81 auto name_it = std::find(global.registered_method_names.begin(),
82 global.registered_method_names.end(), name);
83
84 if (name_it == global.registered_method_names.end()) {
85 return {};
86 }
87
88 size_t index = name_it - global.registered_method_names.begin();
89
90 return global.registered_method_constructors[index](config);
91 }
92
93 void ICP::ensure_builtins_registered() {
94 if (builtins_registered) {
95 return;
96 }
97
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); });
104
105 builtins_registered = true;
106 }
107
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);
112 }
113}
Configuration for ICP instances.
Definition icp.h:81
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
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
double calculate_cost() const
Computes the cost of the current transformation.
Definition icp.cpp:36
Definition driver.h:12
Rigid-body transformation.
Definition geo.h:19