iterative closest points
CEV ICP algorithm library
Loading...
Searching...
No Matches
driver.cpp
Go to the documentation of this file.
1
6#include "icp/driver.h"
7#include <limits>
8
9namespace icp {
10 ICPDriver::ICPDriver(std::unique_ptr<ICP> icp) {
11 icp_ = std::move(icp);
12 }
13
15 const std::vector<Vector>& b, RBTransform t) {
16 start_time_ = std::chrono::steady_clock::now();
17 icp_->begin(a, b, t);
18
19 ConvergenceState state{};
20 state.iteration_count = 0;
21 state.cost = icp_->calculate_cost();
22 state.transform = icp_->current_transform();
23
24 std::optional<ConvergenceState> last_state{};
25
26 while (!should_terminate(state, last_state)) {
27 icp_->iterate();
28 last_state = state;
29 state.iteration_count++;
30 state.cost = icp_->calculate_cost();
31 state.transform = icp_->current_transform();
32 }
33
34 return state;
35 }
36
37 bool ICPDriver::should_terminate(ConvergenceState current_state,
38 std::optional<ConvergenceState> last_state) {
39 // absolute conditions based only on current state
40 if (min_iterations_ && current_state.iteration_count < min_iterations_.value()) {
41 return false;
42 }
43
44 if (max_iterations_ && current_state.iteration_count >= max_iterations_.value()) {
45 return true;
46 }
47
48 if (stop_cost_ && current_state.cost < stop_cost_.value()) {
49 return true;
50 }
51
52 if (time_limit_) {
53 auto current_time = std::chrono::steady_clock::now();
54 if (current_time - start_time_ > time_limit_.value()) {
55 return true;
56 }
57 }
58
59 // end if we don't have a last state
60 if (!last_state) {
61 return false;
62 }
63
64 // relative conditions based on progress
65 double delta_cost = current_state.cost - last_state.value().cost;
66 if (absolute_cost_tolerance_ && std::abs(delta_cost) < absolute_cost_tolerance_.value()) {
67 return true;
68 }
69
70 double relative_cost_change = std::abs(delta_cost) / current_state.cost;
71 if (relative_cost_tolerance_ && relative_cost_change < relative_cost_tolerance_.value()) {
72 return true;
73 }
74
75 if (angle_tolerance_rad_ && translation_tolerance_) {
76 icp::Vector prev_rot_vector = last_state.value().transform.rotation * icp::Vector(1, 0);
77 icp::Vector current_rot_vector = current_state.transform.rotation * icp::Vector(1, 0);
78 double dot = prev_rot_vector.dot(current_rot_vector);
79 double angle = std::acos(std::clamp(dot, -1.0, 1.0));
80
81 auto translation = current_state.transform.translation
82 - last_state.value().transform.translation;
83
84 if (angle < angle_tolerance_rad_.value()
85 && translation.norm() < translation_tolerance_.value()) {
86 return true;
87 }
88 }
89
90 return false;
91 }
92
93 void ICPDriver::set_min_iterations(uint64_t min_iterations) {
94 assert(!max_iterations_ || min_iterations <= max_iterations_.value());
95 min_iterations_ = min_iterations;
96 }
97
98 void ICPDriver::set_max_iterations(uint64_t max_iterations) {
99 assert(!min_iterations_ || max_iterations >= min_iterations_.value());
100 max_iterations_ = max_iterations;
101 }
102
103 void ICPDriver::set_stop_cost(double stop_cost) {
104 stop_cost_ = stop_cost;
105 }
106
107 void ICPDriver::set_relative_cost_tolerance(double relative_cost_tolerance) {
108 relative_cost_tolerance_ = relative_cost_tolerance;
109 }
110
111 void ICPDriver::set_absolute_cost_tolerance(double absolute_cost_tolerance) {
112 absolute_cost_tolerance_ = absolute_cost_tolerance;
113 }
114
115 void ICPDriver::set_transform_tolerance(double angle_tolerance, double translation_tolerance) {
116 angle_tolerance_rad_ = angle_tolerance;
117 translation_tolerance_ = translation_tolerance;
118 }
119
120 void ICPDriver::set_time_limit(std::chrono::duration<double> time_limit) {
121 time_limit_ = time_limit;
122 }
123}
void set_min_iterations(uint64_t min_iterations)
Sets the minimum number of iterations to run.
Definition driver.cpp:93
ICPDriver(std::unique_ptr< ICP > icp)
Constructs a new ICPDriver using the given ICP method.
Definition driver.cpp:10
void set_transform_tolerance(double angle_tolerance, double translation_tolerance)
Set the transform tolerance.
Definition driver.cpp:115
void set_absolute_cost_tolerance(double absolute_cost_tolerance)
Set the absolute cost tolerance.
Definition driver.cpp:111
void set_relative_cost_tolerance(double relative_cost_tolerance)
Sets the relative cost tolerance.
Definition driver.cpp:107
void set_time_limit(std::chrono::duration< double > time_limit)
Set the time limit for converge.
Definition driver.cpp:120
void set_stop_cost(double stop_cost)
Sets the cost at which to stop ICP.
Definition driver.cpp:103
void set_max_iterations(uint64_t max_iterations)
Sets the maximum number of iterations to run.
Definition driver.cpp:98
ConvergenceState converge(const std::vector< Vector > &a, const std::vector< Vector > &b, RBTransform t)
Runs ICP to convergence based on the termination conditions set.
Definition driver.cpp:14
Definition driver.h:12
Eigen::Vector2d Vector
Definition geo.h:15
The result of running ICPDriver::converge.
Definition driver.h:20
size_t iteration_count
The number of iterations performed.
Definition driver.h:25
Rigid-body transformation.
Definition geo.h:19