iterative closest points
CEV ICP algorithm library
Loading...
Searching...
No Matches
vanilla.cpp
Go to the documentation of this file.
1/*
2 * @author Ethan Uppal
3 * @copyright Copyright (C) 2024 Ethan Uppal. All rights reserved.
4 */
5
6#include <cassert>
7#include <cstddef>
8#include <cstdlib>
9#include <Eigen/Core>
10#include <Eigen/SVD>
11#include <Eigen/Dense>
12#include <vector>
13#include "icp/geo.h"
14
15#include "icp/impl/vanilla.h"
16
17/* #name Vanilla */
18/* #register vanilla */
19
20/* #desc The vanilla algorithm for ICP will match the point-cloud centers
21exactly and then iterate until an optimal rotation has been found. */
22
23namespace icp {
24 Vanilla::Vanilla([[maybe_unused]] const Config& config): ICP() {}
27
29 a_current.resize(a.size());
30
31 compute_matches();
32 }
33
35 const size_t n = a.size();
36
37 for (size_t i = 0; i < n; i++) {
38 a_current[i] = transform.apply_to(a[i]);
39 }
40
41 // can optimize by just transforming prev centroid if necessary
42 auto a_current_cm = get_centroid(a_current);
43
44 /*
45 #step
46 Matching Step: match closest points.
47
48 Sources:
49 https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965
50 https://arxiv.org/pdf/2206.06435.pdf
51 https://web.archive.org/web/20220615080318/https://www.cs.technion.ac.il/~cs236329/tutorials/ICP.pdf
52 https://en.wikipedia.org/wiki/Iterative_closest_point
53 https://courses.cs.duke.edu/spring07/cps296.2/scribe_notes/lecture24.pdf
54 -> use k-d tree
55 */
56 compute_matches();
57
58 icp::Vector corr_cm = icp::Vector::Zero();
59 for (size_t i = 0; i < matches.size(); i++) {
60 corr_cm += b[matches[i].pair];
61 }
62 corr_cm /= matches.size();
63
64 /*
65 #step
66 SVD
67
68 We compute the SVD of this magical matrix. A proof that this yields the optimal
69 transform R is in the source below.
70
71 Sources:
72 https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965
73 */
74 Matrix N = Matrix::Zero();
75 for (size_t i = 0; i < n; i++) {
76 N += (a_current[i] - a_current_cm) * (b[matches[i].pair] - corr_cm).transpose();
77 }
78 auto svd = N.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
79 const Matrix U = svd.matrixU();
80 Matrix V = svd.matrixV();
81 Matrix R = V * U.transpose();
82
83 /*
84 #step
85 Reflection Handling
86
87 SVD may return a reflection instead of a rotation if it's equally good or better.
88 This is exceedingly rare with real data but may happen in very high noise
89 environment with sparse point cloud.
90
91 In the 2D case, we can always recover a reasonable rotation by negating the last
92 column of V. I do not know if this is the optimal rotation among rotations, but
93 we can probably get an answer to that question with more effort.
94
95 Sources:
96 https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965
97 */
98 if (R.determinant() < 0) {
99 V = V * Eigen::DiagonalMatrix<double, 2>(1, -1);
100 R = V * U.transpose();
101 }
102
103 /*
104 #step
105 Transformation Step: determine optimal transformation.
106
107 The translation vector is determined by the displacement between
108 the centroids of both point clouds. The rotation matrix is
109 calculated via singular value decomposition.
110
111 Sources:
112 https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965
113 https://courses.cs.duke.edu/spring07/cps296.2/scribe_notes/lecture24.pdf
114 */
115 RBTransform step(corr_cm - R * a_current_cm, R);
116
118 }
119}
120
121void icp::Vanilla::compute_matches() {
122 const size_t n = a.size();
123 const size_t m = b.size();
124
125 for (size_t i = 0; i < n; i++) {
126 matches[i].cost = std::numeric_limits<double>::infinity();
127 for (size_t j = 0; j < m; j++) {
128 // Point-to-point matching
129 double dist_ij = (b[j] - a_current[i]).squaredNorm();
130
131 if (dist_ij < matches[i].cost) {
132 matches[i].cost = dist_ij;
133 matches[i].pair = j;
134 }
135 }
136 }
137}
Configuration for ICP instances.
Definition icp.h:80
Interface for iterative closest points.
Definition icp.h:48
RBTransform transform
The current point cloud transformation that is being optimized.
Definition icp.h:58
std::vector< Vector > a
The source point cloud.
Definition icp.h:61
std::vector< Vector > b
The destination point cloud.
Definition icp.h:64
std::vector< Match > matches
The pairing of each point in a to its closest in b.
Definition icp.h:67
void iterate() override
Perform one iteration of ICP for the point clouds a and b provided with ICP::begin.
Definition vanilla.cpp:34
void setup() override
Per-method setup code.
Definition vanilla.cpp:28
Definition driver.h:7
Eigen::Vector2d Vector
Definition geo.h:14
Eigen::Matrix2d Matrix
Definition geo.h:15
Vector get_centroid(const std::vector< Vector > &points)
Definition geo.cpp:10
Rigid-body transformation.
Definition geo.h:18
RBTransform and_then(const RBTransform &next) const
Definition geo.h:35
Vector apply_to(Vector v) const
Definition geo.h:31