update 2d tests and add 3d tests

jesswang7 · jesswang7 · commit 58021dc888c1 · 2025-02-04T22:26:26.000-05:00
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -74,6 +74,21 @@ target_include_directories(test_suite
 )
 target_compile_definitions(test_suite PRIVATE TEST)
 
+set(TEST_SOURCES_3D tests/test3d.cpp)
+add_executable(test_suite3d ${TEST_SOURCES_3D})
+target_link_libraries(test_suite3d
+  cevicp
+)
+target_include_directories(test_suite3d
+  PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+    /usr/local/include/simple_test
+)
+target_compile_definitions(test_suite3d PRIVATE TEST)
+
+# add_executable(test3d_suite tests/test3d.cpp)
+# target_link_libraries(test3d_suite PRIVATE icp)
+
 set(BENCH_SOURCES bench/bench.cpp common/parse_scan.cpp)
 add_executable(bench_suite ${BENCH_SOURCES})
 target_link_libraries(bench_suite
diff --git a/Makefile b/Makefile
@@ -4,6 +4,7 @@ LIB_TARGET := cevicp
 MAIN_TARGET := main
 TEST_TARGET := test_suite
 BENCH_TARGET := bench_suite
+TEST3D_TARGET := test_suite3d
 
 N := 1
 METHOD := vanilla
@@ -39,6 +40,14 @@ $(TEST_TARGET): configure
 .PHONY: $(BENCH_TARGET)
 $(BENCH_TARGET): configure
 	cmake --build $(BUILD_DIR) --target $(BENCH_TARGET) -- $(MAKE_FLAGS)
+
+.PHONY: $(TEST3D_TARGET)
+$(TEST3D_TARGET): configure
+	cmake --build $(BUILD_DIR) --target $(TEST3D_TARGET) -- $(MAKE_FLAGS)
+
+.PHONY: test3d
+test3d: $(TEST3D_TARGET)
+	./$(BUILD_DIR)/$(TEST3D_TARGET)
 	
 .PHONY: test
 test: $(TEST_TARGET)
diff --git a/lib/icp/driver.cpp b/lib/icp/driver.cpp
@@ -76,6 +76,7 @@ namespace icp {
             return true;
         }
         // 3d rotation decompose in 3 param
+        //used Axis_angle to decompose it now: avoiding gimbal lock for 90 degree rotation that happens in euler angles
         if (angle_tolerance_rad_ && translation_tolerance_) {
             icp::Matrix rotation_step = current_state.transform.rotation
                                         * last_state.value().transform.rotation.transpose();
diff --git a/tests/test.cpp b/tests/test.cpp
@@ -62,6 +62,7 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)
         // Check translation
         assert_true(std::abs(result.transform.translation.x() - 100) <= TRANS_EPS);
         assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(icp::Matrix::Identity(2, 2)));
     }
 
     // Test case 2: Identity test
@@ -80,10 +81,11 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)
 
         assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS);
         assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(icp::Matrix::Identity(2, 2)));
     }
 
     // Test case 3: Rotation at different angles
-    for (int deg = 0; deg < 20; deg++) {
+    for (int deg = 170; deg < 180; deg++) {
         std::vector<icp::Vector> a = {
             icp::Vector(Eigen::Vector2d(-100, -100)), icp::Vector(Eigen::Vector2d(100, 100))};
         std::vector<icp::Vector> b = {};
@@ -98,11 +100,16 @@ 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';
 
         auto result = driver.converge(a, b, icp::RBTransform(2));
 
         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
     }
 
     {
@@ -122,6 +129,7 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)
 
         assert_true(std::abs(result.transform.translation.x() - 100) <= TRANS_EPS);
         assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(icp::Matrix::Identity(2, 2)));
     }
 
     // need more test case that works for trimmed and feature aware(more points? since it filtered
@@ -152,7 +160,7 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)
 
         assert_true(std::abs(result.transform.translation.x() - 50) <= TRANS_EPS);
         assert_true(std::abs(result.transform.translation.y() - 50) <= TRANS_EPS);
-        // assert_true(std::abs(result.transform.rotation.angle() - angle) <= RAD_EPS);
+        assert_true(result.transform.rotation.isApprox(rotation_matrix));
     }
 
     {
@@ -183,6 +191,9 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)
 
         assert_true(std::abs(result.transform.translation.x() - 20) <= TRANS_EPS + 1.0);
         assert_true(std::abs(result.transform.translation.y() - 10) <= TRANS_EPS + 1.0);
+        double angle_compute = std::atan2(result.transform.rotation(1,0), result.transform.rotation(0,0));
+        assert_true(std::abs(angle_compute - angle) <= RAD_EPS);
+        //need to find a way to test the rotation
     }
 }
 
diff --git a/tests/test3d.cpp b/tests/test3d.cpp
@@ -0,0 +1,246 @@
+#include <string>
+#include "icp/geo.h"
+#include "icp/icp.h"
+#include "icp/driver.h"
+#include <iostream>
+#include <random>
+
+extern "C" {
+#include <simple_test/simple_test.h>
+}
+
+#define BURN_IN 0                 // Minimum required iterations for the algorithm
+#define TRANS_EPS 0.5             // Translation tolerance in units
+#define RAD_EPS ((double)(0.01))  // Rotation tolerance in radians
+
+void test_icp_3d(const std::string& method, const icp::ICP::Config& config) {
+
+    std::unique_ptr<icp::ICP> icp = icp::ICP::from_method(method, config).value();
+    icp::ICPDriver driver(std::move(icp));
+    driver.set_min_iterations(BURN_IN);
+    driver.set_max_iterations(100);
+    driver.set_transform_tolerance(0.1 * M_PI / 180, 0.1);
+
+    // Test case 1: Single point translation
+    {
+        std::vector<icp::Vector> a = {icp::Vector(Eigen::Vector3d(0, 0, 0))};
+        std::vector<icp::Vector> b = {icp::Vector(Eigen::Vector3d(100, 0, 0))};
+        auto result = driver.converge(a, b, icp::RBTransform(3));
+
+        // Debug: Print transformation results
+        std::cout << "[1]Result Transform Translation X: " << result.transform.translation.x()
+                  << std::endl;
+        std::cout << "[1]Result Transform Translation Y: " << result.transform.translation.y()
+                  << std::endl;
+        std::cout << "[1]Result Iteration Count: " << result.iteration_count << std::endl;
+
+        // Check iteration count
+        assert_true(result.iteration_count <= BURN_IN + 10);
+
+        // Check translation
+        assert_true(std::abs(result.transform.translation.x() - 100) <= TRANS_EPS);
+        assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(icp::Matrix::Identity(3, 3)));
+    }
+
+    // Test case 2: Identity test
+    {
+        std::vector<icp::Vector> a = {
+            icp::Vector(Eigen::Vector3d(0, 0, 0)), icp::Vector(Eigen::Vector3d(100, 100, 100))};
+        std::vector<icp::Vector> b = {
+            icp::Vector(Eigen::Vector3d(0, 0, 0)), icp::Vector(Eigen::Vector3d(100, 100, 100))};
+        auto result = driver.converge(a, b, icp::RBTransform(3));
+
+        std::cout << "[2]Result Transform Translation X: " << result.transform.translation.x()
+                  << std::endl;
+        std::cout << "[2]Result Transform Translation Y: " << result.transform.translation.y()
+                  << std::endl;
+        std::cout << "[2]Result Iteration Count: " << result.iteration_count << std::endl;
+
+        assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS);
+        assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(icp::Matrix::Identity(3, 3)));
+    }
+
+    // Test case 3: Rotation about z-axis
+    {
+        std::vector<icp::Vector> a = {icp::Vector(Eigen::Vector3d(1, 0, 0))};
+        std::vector<icp::Vector> b = {icp::Vector(Eigen::Vector3d(0, 1, 0))};
+        auto result = driver.converge(a, b, icp::RBTransform(3));
+
+        Eigen::Matrix3d rotation_matrix;
+        rotation_matrix << 0, -1, 0, 1, 0, 0, 0, 0, 1;  // 90 degree rotation about z-axis
+        
+        // Check iteration count
+        assert_true(result.iteration_count <= BURN_IN + 10);
+
+        // Check translation
+        assert_true(std::abs(result.transform.translation.x() - 100) <= TRANS_EPS);
+        assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(rotation_matrix));
+    }
+     // Test case 3: Rotation about one of the axis
+    for (int deg = 0; deg < 180; deg++) {
+        std::vector<icp::Vector> a = {
+            icp::Vector(Eigen::Vector3d(1, 0, 0)), icp::Vector(Eigen::Vector3d(0, 1, 0)),icp::Vector(Eigen::Vector3d(0, 0, 1))};
+        std::vector<icp::Vector> b = {};
+
+        double angle = (double)deg * M_PI / 180.0;
+        icp::Vector center = icp::get_centroid(a);
+        icp::Matrix rotation_matrix(3, 3);
+        rotation_matrix << 1, 0, 0,
+                           0, std::cos(angle), -std::sin(angle),
+                           0, std::sin(angle), std::cos(angle);
+
+        for (const auto& point: a) {
+            b.push_back(rotation_matrix * (point - center) + center);
+        }
+
+        std::cout << "testing angle: " << deg << '\n';
+
+        auto result = driver.converge(a, b, icp::RBTransform(3));
+
+        assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS);
+        assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(rotation_matrix));
+    }
+
+    // Test case 3: Rotation about multiple the axis
+    {
+        std::vector<icp::Vector> a = {
+            icp::Vector(Eigen::Vector3d(1, 0, 0)), icp::Vector(Eigen::Vector3d(0, 1, 0)),icp::Vector(Eigen::Vector3d(0, 0, 1))};
+        std::vector<icp::Vector> b = {};
+
+        int deg_1 = rand()%360;
+        int deg_2 = rand()%360;
+        int deg_3 = rand()%360;
+
+        double angle_1 = (double)deg_1 * M_PI / 180.0;
+        double angle_2 = (double)deg_2 * M_PI / 180.0;
+        double angle_3 = (double)deg_3 * M_PI / 180.0;
+        icp::Vector center = icp::get_centroid(a);
+        icp::Matrix rotation_matrix(3, 3);
+        rotation_matrix << std::cos(angle_2)*std::cos(angle_3), std::sin(angle_1)*std::sin(angle_2)*std::cos(angle_3)-std::cos(angle_1)*std::sin(angle_3), std::cos(angle_1)*std::sin(angle_2)*std::cos(angle_3)+std::sin(angle_1)*std::sin(angle_3),
+                           std::cos(angle_2)*std::sin(angle_3), std::sin(angle_1)*std::sin(angle_2)*std::sin(angle_3)+std::cos(angle_1)*std::cos(angle_3), std::cos(angle_1)*std::sin(angle_2)*std::sin(angle_3)-std::sin(angle_1)*std::cos(angle_3),
+                           -std::sin(angle_2), std::sin(angle_1)*std::cos(angle_2), std::cos(angle_1)*std::cos(angle_2);
+
+        for (const auto& point: a) {
+            b.push_back(rotation_matrix * (point - center) + center);
+        }
+
+        auto result = driver.converge(a, b, icp::RBTransform(3));
+
+        assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS);
+        assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(rotation_matrix));
+    }
+
+    {
+        // Test case 4: Pure translation multiple axis
+        std::vector<icp::Vector> a = {
+            icp::Vector(Eigen::Vector3d(1, 0, 0)), icp::Vector(Eigen::Vector3d(0, 1, 0)),icp::Vector(Eigen::Vector3d(0, 0, 1))};
+        std::vector<icp::Vector> b = {
+            icp::Vector(Eigen::Vector3d(51, 73, 2)), icp::Vector(Eigen::Vector3d(50, 74, 2)),icp::Vector(Eigen::Vector3d(50, 73, 3))};
+
+        auto result = driver.converge(a, b, icp::RBTransform(3));
+
+        std::cout << "[3]Result Transform Translation X: " << result.transform.translation.x()
+                  << std::endl;
+        std::cout << "[3]Result Transform Translation Y: " << result.transform.translation.y()
+                  << std::endl;
+        std::cout << "[3]Result Transform Translation Y: " << result.transform.translation.z()
+                  << std::endl;
+        std::cout << "[3]Result Iteration Count: " << result.iteration_count << std::endl;
+
+        assert_true(std::abs(result.transform.translation.x() - 50) <= TRANS_EPS);
+        assert_true(std::abs(result.transform.translation.y() - 73) <= TRANS_EPS);
+        assert_true(std::abs(result.transform.translation.y() - 2) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(icp::Matrix::Identity(3, 3)));
+
+    }
+
+    // // need more test case that works for trimmed and feature aware(more points? since it filtered
+    // // out points)
+    {
+        // Translation + rotation
+        std::vector<icp::Vector> a = {
+            icp::Vector(Eigen::Vector3d(1, 0, 0)), icp::Vector(Eigen::Vector3d(0, 1, 0)),icp::Vector(Eigen::Vector3d(0, 0, 1))};
+        std::vector<icp::Vector> b;
+
+        double angle_1 = 45 * M_PI / 180.0;
+        double angle_2 = 45 * M_PI / 180.0;
+        double angle_3 = 45 * M_PI / 180.0;
+        icp::Vector translation(Eigen::Vector3d(50, 50, 50)); 
+
+        icp::Matrix rotation_matrix(3, 3); 
+        rotation_matrix << std::cos(angle_2)*std::cos(angle_3), std::sin(angle_1)*std::sin(angle_2)*std::cos(angle_3)-std::cos(angle_1)*std::sin(angle_3), std::cos(angle_1)*std::sin(angle_2)*std::cos(angle_3)+std::sin(angle_1)*std::sin(angle_3),
+                           std::cos(angle_2)*std::sin(angle_3), std::sin(angle_1)*std::sin(angle_2)*std::sin(angle_3)+std::cos(angle_1)*std::cos(angle_3), std::cos(angle_1)*std::sin(angle_2)*std::sin(angle_3)-std::sin(angle_1)*std::cos(angle_3),
+                           -std::sin(angle_2), std::sin(angle_1)*std::cos(angle_2), std::cos(angle_1)*std::cos(angle_2);
+
+        for (const auto& point: a) {
+            b.push_back(rotation_matrix * point + translation);
+        }
+
+        auto result = driver.converge(a, b, icp::RBTransform(3));
+
+        std::cout << "[4]Result Transform Translation X: " << result.transform.translation.x()
+                  << std::endl;
+        std::cout << "[4]Result Transform Translation Y: " << result.transform.translation.y()
+                  << std::endl;
+        std::cout << "[4]Result Transform Translation Z: " << result.transform.translation.z()
+                  << std::endl;
+        std::cout << "[4]Result Iteration Count: " << result.iteration_count << std::endl;
+
+        assert_true(std::abs(result.transform.translation.x() - 50) <= TRANS_EPS);
+        assert_true(std::abs(result.transform.translation.y() - 50) <= TRANS_EPS);
+        assert_true(std::abs(result.transform.translation.z() - 50) <= TRANS_EPS);
+        assert_true(result.transform.rotation.isApprox(rotation_matrix));
+    }
+
+    {
+        // Add noise
+        std::vector<icp::Vector> a = {
+            icp::Vector(Eigen::Vector3d(1, 0, 0)), icp::Vector(Eigen::Vector3d(0, 1, 0)),icp::Vector(Eigen::Vector3d(0, 0, 1))};
+        std::vector<icp::Vector> b;
+
+        double angle_1 = 30 * M_PI / 180.0;
+        double angle_2 = 40 * M_PI / 180.0;
+        double angle_3 = 50 * M_PI / 180.0;
+        icp::Vector translation(Eigen::Vector3d(20, 10, 30));  // Translate by (20, 10)
+
+        icp::Matrix rotation_matrix(3, 3); 
+        rotation_matrix << std::cos(angle_2)*std::cos(angle_3), std::sin(angle_1)*std::sin(angle_2)*std::cos(angle_3)-std::cos(angle_1)*std::sin(angle_3), std::cos(angle_1)*std::sin(angle_2)*std::cos(angle_3)+std::sin(angle_1)*std::sin(angle_3),
+                           std::cos(angle_2)*std::sin(angle_3), std::sin(angle_1)*std::sin(angle_2)*std::sin(angle_3)+std::cos(angle_1)*std::cos(angle_3), std::cos(angle_1)*std::sin(angle_2)*std::sin(angle_3)-std::sin(angle_1)*std::cos(angle_3),
+                           -std::sin(angle_2), std::sin(angle_1)*std::cos(angle_2), std::cos(angle_1)*std::cos(angle_2);
+
+        std::default_random_engine generator;
+        std::normal_distribution<double> noise_dist(0.0,
+            1.0);  // Noise with standard deviation of 1.0
+
+        for (const auto& point: a) {
+            Eigen::Vector3d noisy_point;
+            noisy_point = rotation_matrix * point + translation;
+            noisy_point.x() += noise_dist(generator);
+            noisy_point.y() += noise_dist(generator);
+            noisy_point.z() += noise_dist(generator);
+            b.push_back(noisy_point);
+        }
+
+        auto result = driver.converge(a, b, icp::RBTransform());
+
+        assert_true(std::abs(result.transform.translation.x() - 20) <= TRANS_EPS + 1.0);
+        assert_true(std::abs(result.transform.translation.y() - 10) <= TRANS_EPS + 1.0);
+        assert_true(std::abs(result.transform.translation.z() - 10) <= TRANS_EPS + 1.0);
+
+        Eigen::Matrix3d rotation_matrix_2 = result.transform.rotation.block<3,3>(0,0); 
+        Eigen::Vector3d euler_angles = rotation_matrix_2.eulerAngles(0, 1, 2);
+        // assert_true(std::abs(euler_angles[0] - angle_1) <= RAD_EPS);
+        // assert_true(std::abs(euler_angles[1] - angle_2) <= RAD_EPS);
+        // assert_true(std::abs(euler_angles[2] - angle_3) <= RAD_EPS);
+    }
+    //for testing the rotation, should we decompose the rotation matrix and compare the angles seperately?
+}
+
+void test_main() {
+    test_icp_3d("vanilla_3d", icp::ICP::Config());
+}

Original file line number	Diff line number	Diff line change
`@@ -76,6 +76,7 @@ namespace icp {`
`76`	`76`	`return true;`
`77`	`77`	`}`
`78`	`78`	`// 3d rotation decompose in 3 param`
	`79`	`+ //used Axis_angle to decompose it now: avoiding gimbal lock for 90 degree rotation that happens in euler angles`
`79`	`80`	`if (angle_tolerance_rad_ && translation_tolerance_) {`
`80`	`81`	`icp::Matrix rotation_step = current_state.transform.rotation`
`81`	`82`	`* last_state.value().transform.rotation.transpose();`
Original file line number	Diff line number	Diff line change
`@@ -62,6 +62,7 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)`
`62`	`62`	`// Check translation`
`63`	`63`	`assert_true(std::abs(result.transform.translation.x() - 100) <= TRANS_EPS);`
`64`	`64`	`assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);`
	`65`	`+ assert_true(result.transform.rotation.isApprox(icp::Matrix::Identity(2, 2)));`
`65`	`66`	`}`
`66`	`67`
`67`	`68`	`// Test case 2: Identity test`
`@@ -80,10 +81,11 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)`
`80`	`81`
`81`	`82`	`assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS);`
`82`	`83`	`assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);`
	`84`	`+ assert_true(result.transform.rotation.isApprox(icp::Matrix::Identity(2, 2)));`
`83`	`85`	`}`
`84`	`86`
`85`	`87`	`// Test case 3: Rotation at different angles`
`86`		`- for (int deg = 0; deg < 20; deg++) {`
	`88`	`+ for (int deg = 170; deg < 180; deg++) {`
`87`	`89`	`std::vector<icp::Vector> a = {`
`88`	`90`	`icp::Vector(Eigen::Vector2d(-100, -100)), icp::Vector(Eigen::Vector2d(100, 100))};`
`89`	`91`	`std::vector<icp::Vector> b = {};`
`@@ -98,11 +100,16 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)`
`98`	`100`	`}`
`99`	`101`
`100`	`102`	`std::cout << "testing angle: " << deg << '\n';`
	`103`	`+ std::cout << "the result for the matrix " << rotation_matrix << '\n';`
`101`	`104`
`102`	`105`	`auto result = driver.converge(a, b, icp::RBTransform(2));`
`103`	`106`
`104`	`107`	`assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS);`
`105`	`108`	`assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);`
	`109`	`+`
	`110`	`+ std::cout << "the result for the matrix " << result.transform.rotation << '\n';`
	`111`	`+ assert_true(result.transform.rotation.isApprox(rotation_matrix));`
	`112`	`+ //having problem when the angle is 90, 180`
`106`	`113`	`}`
`107`	`114`
`108`	`115`	`{`
`@@ -122,6 +129,7 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)`
`122`	`129`
`123`	`130`	`assert_true(std::abs(result.transform.translation.x() - 100) <= TRANS_EPS);`
`124`	`131`	`assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS);`
	`132`	`+ assert_true(result.transform.rotation.isApprox(icp::Matrix::Identity(2, 2)));`
`125`	`133`	`}`
`126`	`134`
`127`	`135`	`// need more test case that works for trimmed and feature aware(more points? since it filtered`
`@@ -152,7 +160,7 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)`
`152`	`160`
`153`	`161`	`assert_true(std::abs(result.transform.translation.x() - 50) <= TRANS_EPS);`
`154`	`162`	`assert_true(std::abs(result.transform.translation.y() - 50) <= TRANS_EPS);`
`155`		`- // assert_true(std::abs(result.transform.rotation.angle() - angle) <= RAD_EPS);`
	`163`	`+ assert_true(result.transform.rotation.isApprox(rotation_matrix));`
`156`	`164`	`}`
`157`	`165`
`158`	`166`	`{`
`@@ -183,6 +191,9 @@ void test_icp_generic(const std::string& method, const icp::ICP::Config& config)`
`183`	`191`
`184`	`192`	`assert_true(std::abs(result.transform.translation.x() - 20) <= TRANS_EPS + 1.0);`
`185`	`193`	`assert_true(std::abs(result.transform.translation.y() - 10) <= TRANS_EPS + 1.0);`
	`194`	`+ double angle_compute = std::atan2(result.transform.rotation(1,0), result.transform.rotation(0,0));`
	`195`	`+ assert_true(std::abs(angle_compute - angle) <= RAD_EPS);`
	`196`	`+ //need to find a way to test the rotation`
`186`	`197`	`}`
`187`	`198`	`}`
`188`	`199`