29 a_current.resize(
a.size());
35 const size_t n =
a.size();
37 for (
size_t i = 0; i < n; i++) {
59 for (
size_t i = 0; i <
matches.size(); i++) {
75 for (
size_t i = 0; i < n; i++) {
76 N += (a_current[i] - a_current_cm) * (
b[
matches[i].pair] - corr_cm).transpose();
78 auto svd = N.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
79 const Matrix U = svd.matrixU();
81 Matrix R = V * U.transpose();
98 if (R.determinant() < 0) {
99 V = V * Eigen::DiagonalMatrix<double, 2>(1, -1);
100 R = V * U.transpose();
121void icp::Vanilla::compute_matches() {
122 const size_t n =
a.size();
123 const size_t m =
b.size();
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++) {
129 double dist_ij = (
b[j] - a_current[i]).squaredNorm();
131 if (dist_ij <
matches[i].cost) {
Configuration for ICP instances.
Interface for iterative closest points.
RBTransform transform
The current point cloud transformation that is being optimized.
std::vector< Vector > a
The source point cloud.
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 iterate() override
Perform one iteration of ICP for the point clouds a and b provided with ICP::begin.
void setup() override
Per-method setup code.
Vector get_centroid(const std::vector< Vector > &points)