Skip to content

Commit 5762eaa

Browse files
committed
Fix: Basic ICP algorithm works for small transformations but fails for large rotations/translations. Need improved nearest neighbor matching (KD-Tree/soft assignments) and better initial alignment (PCA, FPFH, manual registration).
1 parent 58021dc commit 5762eaa

File tree

5 files changed

+141
-50
lines changed

5 files changed

+141
-50
lines changed

lib/icp/driver.cpp

+13-3
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
#include "Eigen/src/Geometry/AngleAxis.h"
66
#include "icp/geo.h"
77
#include "icp/icp.h"
8+
#include "iostream"
89

910
namespace icp {
1011
ICPDriver::ICPDriver(std::unique_ptr<ICP> icp) {
@@ -15,9 +16,6 @@ namespace icp {
1516
const std::vector<Vector>& b, RBTransform t) {
1617
start_time_ = std::chrono::steady_clock::now();
1718
icp_->begin(a, b, t);
18-
19-
// ConvergenceState state{};
20-
2119
size_t dimension = a[0].size();
2220
ConvergenceState state(dimension);
2321

@@ -27,12 +25,20 @@ namespace icp {
2725

2826
std::optional<ConvergenceState> last_state{};
2927

28+
3029
while (!should_terminate(state, last_state)) {
3130
icp_->iterate();
3231
last_state = state;
3332
state.iteration_count++;
3433
state.cost = icp_->calculate_cost();
3534
state.transform = icp_->current_transform();
35+
36+
// Debug: Print current state
37+
std::cout << "Iteration: " << state.iteration_count << std::endl;
38+
std::cout << "Cost: " << state.cost << std::endl;
39+
std::cout << "Transform: " << std::endl;
40+
std::cout << state.transform.rotation << std::endl;
41+
std::cout << state.transform.translation.transpose() << std::endl;
3642
}
3743

3844
return state;
@@ -94,6 +100,10 @@ namespace icp {
94100
auto translation = current_state.transform.translation
95101
- last_state.value().transform.translation;
96102

103+
// Debug: Print angle and translation differences
104+
std::cout << "Angle difference: " << angle_diff << std::endl;
105+
std::cout << "Translation difference: " << translation.norm() << std::endl;
106+
97107
if (angle_diff < angle_tolerance_rad_.value()
98108
&& translation.norm() < translation_tolerance_.value()) {
99109
return true;

lib/icp/impl/vanilla.cpp

+24-2
Original file line numberDiff line numberDiff line change
@@ -2,12 +2,14 @@
22
* @author Ethan Uppal
33
* @copyright Copyright (C) 2024 Ethan Uppal. All rights reserved.
44
*/
5-
5+
#include <iostream>
66
#include <cassert>
7+
#include <cstddef>
78
#include <cstdlib>
89
#include <Eigen/Core>
910
#include <Eigen/SVD>
1011
#include <Eigen/Dense>
12+
#include <vector>
1113
#include "icp/geo.h"
1214

1315
#include "icp/impl/vanilla.h"
@@ -25,13 +27,20 @@ namespace icp {
2527

2628
void Vanilla::setup() {
2729
a_current.resize(a.size());//make function in icp
28-
b_cm = get_centroid(b);
30+
// b_cm = get_centroid(b);
2931

3032
for (size_t i = 0; i < a.size(); i++) {
3133
a_current[i] = transform.apply_to(a[i]);
3234
}
3335

3436
compute_matches();
37+
38+
icp::Vector corr_cm = icp::Vector::Zero(2);
39+
for (size_t i = 0; i < matches.size(); i++) {
40+
corr_cm += b[matches[i].pair];
41+
}
42+
corr_cm /= matches.size();
43+
b_cm = corr_cm;
3544
}
3645

3746
void Vanilla::iterate() {
@@ -76,6 +85,8 @@ namespace icp {
7685
const Matrix U = svd.matrixU();
7786
Matrix V = svd.matrixV();
7887
Matrix R = V * U.transpose();
88+
std::cout << "0: ___" << std::endl;
89+
std::cout << R << std::endl;
7990

8091
/*
8192
#step
@@ -93,9 +104,20 @@ namespace icp {
93104
https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965
94105
*/
95106
if (R.determinant() < 0) {
107+
std::cout << "1: ___" << std::endl;
108+
std::cout << R << std::endl;
109+
std::cout << "Utrans: " << U.transpose() << std::endl;
110+
std::cout << "Determinant: " << R.determinant() << std::endl;
111+
std::cout << "V: " << V << std::endl;
96112
V = V * Eigen::DiagonalMatrix<double, 2>(1, -1);
113+
std::cout << "V: " << V << std::endl;
97114
R = V * U.transpose();
115+
std::cout << "2: ___" << std::endl;
116+
std::cout << R << std::endl;
98117
}
118+
std::cout << "3: ___" << std::endl;
119+
std::cout << get_matches()[0].pair << std::endl;
120+
std::cout << get_matches()[1].pair << std::endl;
99121

100122
//transform.rotation = R * transform.rotation;
101123

lib/icp/impl/vanilla_3d.cpp

+31-6
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
/*
22
*
33
*/
4-
4+
# include <iostream>
55
#include <numeric>
66
#include <vector>
77
#include <cmath>
@@ -33,8 +33,8 @@ namespace icp {
3333
int row_src = src.rows();
3434
int row_dst = dst.rows();
3535
NEIGHBOR neigh;
36-
3736
for (int i = 0; i < row_src; i++) {
37+
// std::cout << "row:" << row_src << std::endl;
3838
Eigen::Vector3d pta = src.row(i).transpose();
3939
float min_dist = std::numeric_limits<float>::max();
4040
int index = 0;
@@ -79,27 +79,52 @@ namespace icp {
7979
Eigen::MatrixXd T = Eigen::MatrixXd::Identity(A.cols() + 1, A.cols() + 1);
8080

8181
T.block<3, 3>(0, 0) = R;
82-
T.block<3, 1>(0, A.cols()) = t;
82+
T.block<3, 1>(0, 3) = t;
8383
return T;
8484
}
8585

8686
void Vanilla_3d::setup() {
87+
A.resize(a.size(), dim);
88+
for (size_t i = 0; i < a.size(); ++i) {
89+
for (int j = 0; j < dim; ++j) {
90+
A(i, j) = a[i][j];
91+
}
92+
}
93+
94+
B.resize(b.size(), dim);
95+
for (size_t i = 0; i < b.size(); ++i) {
96+
for (int j = 0; j < dim; ++j) {
97+
B(i, j) = b[i][j];
98+
}
99+
}
100+
87101
C = A;
102+
// std::cout << "hi______________ " << std::endl;
103+
// std::cout << "A: "<< A << std::endl;
104+
// std::cout << "B: "<< B << std::endl;
88105
}
89106

90107
void Vanilla_3d::iterate() {
108+
// std::cout << "bye 1 ______________ " << std::endl;
91109
int row = A.rows();
110+
// std::cout << row << std::endl;
92111

93112
// Reorder target point set based on nearest neighbor
113+
// std::cout << "C:" << C << std::endl;
94114
NEIGHBOR neighbor = nearest_neighbor(C, B);
115+
// std::cout << "bye 2 ______________ " << std::endl;
95116
Eigen::MatrixXd dst_reordered(row, 3);
117+
// std::cout << "bye 3 ______________ " << std::endl;
96118
for (int i = 0; i < row; i++) {
97119
dst_reordered.row(i) = B.row(neighbor.indices[i]);
98120
}
99-
121+
// std::cout << "bye 4 ______________ " << std::endl;
100122
Eigen::Matrix4d T = best_fit_transform(C, dst_reordered);
101-
C = (T * C.transpose().colwise().homogeneous()).transpose();
123+
Eigen::MatrixXd C_homogeneous = C.transpose().colwise().homogeneous();
124+
C = (T * C_homogeneous).transpose().leftCols(3);
102125

126+
// std::cout << "bye 5 ______________ " << std::endl;
103127
transform = transform.update(T);
104128
}
105-
}
129+
}
130+
//more iteration should not diverge

tests/test.cpp

+27-6
Original file line numberDiff line numberDiff line change
@@ -85,11 +85,11 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)
8585
}
8686

8787
// Test case 3: Rotation at different angles
88-
for (int deg = 170; deg < 180; deg++) {
88+
for (int deg = 90; deg < 91; deg++) {
8989
std::vector<icp::Vector> a = {
9090
icp::Vector(Eigen::Vector2d(-100, -100)), icp::Vector(Eigen::Vector2d(100, 100))};
9191
std::vector<icp::Vector> b = {};
92-
92+
9393
double angle = (double)deg * M_PI / 180.0;
9494
icp::Vector center = icp::get_centroid(a);
9595
icp::Matrix rotation_matrix{
@@ -100,16 +100,37 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)
100100
}
101101

102102
std::cout << "testing angle: " << deg << '\n';
103-
std::cout << "the result for the matrix " << rotation_matrix << '\n';
103+
std::cout << "the result for the matrix (expect) " << rotation_matrix << '\n';
104104

105105
auto result = driver.converge(a, b, icp::RBTransform(2));
106106

107+
std::cout << "the result for the matrix (true)" << result.transform.rotation << '\n';
108+
std::cout << "the result for the matrix (true translate)" << result.transform.translation << '\n';
109+
107110
assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS);
108111
assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
109-
110-
std::cout << "the result for the matrix " << result.transform.rotation << '\n';
111112
assert_true(result.transform.rotation.isApprox(rotation_matrix));
112-
//having problem when the angle is 90, 180
113+
}
114+
115+
// Test case 3: Rotation at different angles
116+
for (int deg = 90; deg < 91; deg++) {
117+
double angle = deg * M_PI / 180.0;
118+
Eigen::Matrix2d rotation_matrix;
119+
rotation_matrix << cos(angle), -sin(angle), sin(angle), cos(angle);
120+
121+
std::vector<icp::Vector> a = {
122+
icp::Vector(Eigen::Vector2d(0, 0)), icp::Vector(Eigen::Vector2d(1, 0))};
123+
std::vector<icp::Vector> b = {
124+
icp::Vector(Eigen::Vector2d(0, 0)), icp::Vector(rotation_matrix * Eigen::Vector2d(1, 0))};
125+
126+
auto result = driver.converge(a, b, icp::RBTransform(2));
127+
128+
std::cout << "testing angle: " << deg << std::endl;
129+
std::cout << "the result for the matrix (expect) " << rotation_matrix << std::endl;
130+
std::cout << "the result for the matrix (true)" << result.transform.rotation << std::endl;
131+
std::cout << "the result for the matrix (true translate)" << result.transform.translation << std::endl;
132+
133+
assert_true(result.transform.rotation.isApprox(rotation_matrix, 1e-6));
113134
}
114135

115136
{

0 commit comments

Comments
 (0)