Fix: Basic ICP algorithm works for small transformations but fails for large rotations/translations. Need improved nearest neighbor matching (KD-Tree with nanoflann/Eigen, soft assignments) and better initial alignment (PCA, FPFH, manual registration).

jesswang7 · jesswang7 · commit b99dcd9fee4a · 2025-02-12T13:26:06.000-05:00
diff --git a/lib/icp/driver.cpp b/lib/icp/driver.cpp
@@ -5,6 +5,7 @@
 #include "Eigen/src/Geometry/AngleAxis.h"
 #include "icp/geo.h"
 #include "icp/icp.h"
+#include "iostream"
 
 namespace icp {
     ICPDriver::ICPDriver(std::unique_ptr<ICP> icp) {
@@ -15,9 +16,6 @@ namespace icp {
         const std::vector<Vector>& b, RBTransform t) {
         start_time_ = std::chrono::steady_clock::now();
         icp_->begin(a, b, t);
-
-        // ConvergenceState state{};
-
         size_t dimension = a[0].size();
         ConvergenceState state(dimension);
 
@@ -27,12 +25,20 @@ namespace icp {
 
         std::optional<ConvergenceState> last_state{};
 
+        
         while (!should_terminate(state, last_state)) {
             icp_->iterate();
             last_state = state;
             state.iteration_count++;
             state.cost = icp_->calculate_cost();
             state.transform = icp_->current_transform();
+
+            // Debug: Print current state
+            std::cout << "Iteration: " << state.iteration_count << std::endl;
+            std::cout << "Cost: " << state.cost << std::endl;
+            std::cout << "Transform: " << std::endl;
+            std::cout << state.transform.rotation << std::endl;
+            std::cout << state.transform.translation.transpose() << std::endl;
         }
 
         return state;
@@ -94,6 +100,10 @@ namespace icp {
             auto translation = current_state.transform.translation
                                - last_state.value().transform.translation;
 
+            // Debug: Print angle and translation differences
+            std::cout << "Angle difference: " << angle_diff << std::endl;
+            std::cout << "Translation difference: " << translation.norm() << std::endl;
+
             if (angle_diff < angle_tolerance_rad_.value()
                 && translation.norm() < translation_tolerance_.value()) {
                 return true;
diff --git a/lib/icp/impl/vanilla.cpp b/lib/icp/impl/vanilla.cpp
@@ -2,12 +2,14 @@
  * @author Ethan Uppal
  * @copyright Copyright (C) 2024 Ethan Uppal. All rights reserved.
  */
-
+#include <iostream>
 #include <cassert>
+#include <cstddef>
 #include <cstdlib>
 #include <Eigen/Core>
 #include <Eigen/SVD>
 #include <Eigen/Dense>
+#include <vector>
 #include "icp/geo.h"
 
 #include "icp/impl/vanilla.h"
@@ -25,13 +27,20 @@ namespace icp {
 
     void Vanilla::setup() {
         a_current.resize(a.size());//make function in icp
-        b_cm = get_centroid(b);
+        // b_cm = get_centroid(b);
 
         for (size_t i = 0; i < a.size(); i++) {
             a_current[i] = transform.apply_to(a[i]);
         }
 
         compute_matches();
+
+        icp::Vector corr_cm = icp::Vector::Zero(2);
+        for (size_t i = 0; i < matches.size(); i++) {
+            corr_cm += b[matches[i].pair];
+        }
+        corr_cm /= matches.size();
+        b_cm = corr_cm;
     }
 
     void Vanilla::iterate() {
@@ -76,6 +85,8 @@ namespace icp {
         const Matrix U = svd.matrixU();
         Matrix V = svd.matrixV();
         Matrix R = V * U.transpose();
+        std::cout << "0: ___" << std::endl;
+        std::cout << R << std::endl;
 
         /*
             #step
@@ -93,9 +104,20 @@ namespace icp {
             https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965
         */
         if (R.determinant() < 0) {
+            std::cout << "1: ___" << std::endl;
+            std::cout << R << std::endl;
+            std::cout << "Utrans: " << U.transpose() << std::endl;
+            std::cout << "Determinant: " << R.determinant() << std::endl;
+            std::cout << "V: " << V << std::endl;
             V = V * Eigen::DiagonalMatrix<double, 2>(1, -1);
+            std::cout << "V: " << V << std::endl;
             R = V * U.transpose();
+            std::cout << "2: ___" << std::endl;
+            std::cout << R << std::endl;
         }
+        std::cout << "3: ___" << std::endl;
+        std::cout << get_matches()[0].pair << std::endl;
+        std::cout << get_matches()[1].pair << std::endl;
 
         //transform.rotation = R * transform.rotation;
 
diff --git a/lib/icp/impl/vanilla_3d.cpp b/lib/icp/impl/vanilla_3d.cpp
@@ -1,7 +1,7 @@
 /*
  *
  */
-
+# include <iostream>
 #include <numeric>
 #include <vector>
 #include <cmath>
@@ -33,8 +33,8 @@ namespace icp {
         int row_src = src.rows();
         int row_dst = dst.rows();
         NEIGHBOR neigh;
-
         for (int i = 0; i < row_src; i++) {
+            // std::cout << "row:" << row_src << std::endl;
             Eigen::Vector3d pta = src.row(i).transpose();
             float min_dist = std::numeric_limits<float>::max();
             int index = 0;
@@ -79,27 +79,52 @@ namespace icp {
         Eigen::MatrixXd T = Eigen::MatrixXd::Identity(A.cols() + 1, A.cols() + 1);
 
         T.block<3, 3>(0, 0) = R;
-        T.block<3, 1>(0, A.cols()) = t;
+        T.block<3, 1>(0, 3) = t;
         return T;
     }
 
     void Vanilla_3d::setup() {
+        A.resize(a.size(), dim);
+        for (size_t i = 0; i < a.size(); ++i) {
+            for (int j = 0; j < dim; ++j) {
+                A(i, j) = a[i][j];
+            }
+        }
+        
+        B.resize(b.size(), dim);
+        for (size_t i = 0; i < b.size(); ++i) {
+            for (int j = 0; j < dim; ++j) {
+                B(i, j) = b[i][j];
+            }
+        }
+
         C = A;
+        // std::cout << "hi______________ " << std::endl;
+        // std::cout << "A: "<< A << std::endl;
+        // std::cout << "B: "<< B << std::endl;
     }
 
     void Vanilla_3d::iterate() {
+        // std::cout << "bye 1 ______________ " << std::endl;
         int row = A.rows();
+        // std::cout << row << std::endl;
 
         // Reorder target point set based on nearest neighbor
+        // std::cout << "C:" << C << std::endl;
         NEIGHBOR neighbor = nearest_neighbor(C, B);
+        // std::cout << "bye 2 ______________ " << std::endl;
         Eigen::MatrixXd dst_reordered(row, 3);
+        // std::cout << "bye 3 ______________ " << std::endl;
         for (int i = 0; i < row; i++) {
             dst_reordered.row(i) = B.row(neighbor.indices[i]);
         }
-
+        // std::cout << "bye 4 ______________ " << std::endl;
         Eigen::Matrix4d T = best_fit_transform(C, dst_reordered);
-        C = (T * C.transpose().colwise().homogeneous()).transpose();
+        Eigen::MatrixXd C_homogeneous = C.transpose().colwise().homogeneous();
+        C = (T * C_homogeneous).transpose().leftCols(3);
 
+        // std::cout << "bye 5 ______________ " << std::endl;
         transform = transform.update(T);
     }
-}
+}
+//more iteration should not diverge
diff --git a/tests/test.cpp b/tests/test.cpp
@@ -85,11 +85,11 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)
     }
 
     // Test case 3: Rotation at different angles
-    for (int deg = 170; deg < 180; deg++) {
+    for (int deg = 90; deg < 91; deg++) {
         std::vector<icp::Vector> a = {
             icp::Vector(Eigen::Vector2d(-100, -100)), icp::Vector(Eigen::Vector2d(100, 100))};
         std::vector<icp::Vector> b = {};
-
+        
         double angle = (double)deg * M_PI / 180.0;
         icp::Vector center = icp::get_centroid(a);
         icp::Matrix rotation_matrix{
@@ -100,16 +100,37 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)
         }
 
         std::cout << "testing angle: " << deg << '\n';
-        std::cout << "the result for the matrix " << rotation_matrix << '\n';
+        std::cout << "the result for the matrix (expect) " << rotation_matrix << '\n';
 
         auto result = driver.converge(a, b, icp::RBTransform(2));
 
+        std::cout << "the result for the matrix (true)" << result.transform.rotation << '\n';
+        std::cout << "the result for the matrix (true translate)" << result.transform.translation << '\n';
+
         assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS);
         assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
-
-        std::cout << "the result for the matrix " << result.transform.rotation << '\n';
         assert_true(result.transform.rotation.isApprox(rotation_matrix));
-        //having problem when the angle is 90, 180
+    }
+
+    // Test case 3: Rotation at different angles
+    for (int deg = 90; deg < 91; deg++) {
+        double angle = deg * M_PI / 180.0;
+        Eigen::Matrix2d rotation_matrix;
+        rotation_matrix << cos(angle), -sin(angle), sin(angle), cos(angle);
+
+        std::vector<icp::Vector> a = {
+            icp::Vector(Eigen::Vector2d(0, 0)), icp::Vector(Eigen::Vector2d(1, 0))};
+        std::vector<icp::Vector> b = {
+            icp::Vector(Eigen::Vector2d(0, 0)), icp::Vector(rotation_matrix * Eigen::Vector2d(1, 0))};
+
+        auto result = driver.converge(a, b, icp::RBTransform(2));
+
+        std::cout << "testing angle: " << deg << std::endl;
+        std::cout << "the result for the matrix (expect) " << rotation_matrix << std::endl;
+        std::cout << "the result for the matrix (true)" << result.transform.rotation << std::endl;
+        std::cout << "the result for the matrix (true translate)" << result.transform.translation << std::endl;
+
+        assert_true(result.transform.rotation.isApprox(rotation_matrix, 1e-6));
     }
 
     {
diff --git a/tests/test3d.cpp b/tests/test3d.cpp
