Refactor math for ColoredICP (#4988)

isl-org · Apr 12, 2022 · 7aa838c · 7aa838c
1 parent 8fb7ee6
commit 7aa838c
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 10 deletions.
diff --git a/cpp/open3d/pipelines/registration/ColoredICP.cpp b/cpp/open3d/pipelines/registration/ColoredICP.cpp
@@ -170,20 +170,16 @@ Eigen::Matrix4d TransformationEstimationForColoredICP::ComputeTransformation(
                              target.colors_[ct](2)) /
                             3.0;
                 const Eigen::Vector3d &dit = target_c.color_gradient_[ct];
-                double is0_proj = (dit.dot(vs_proj - vt)) + it;
+                double is_proj = (dit.dot(vs_proj - vt)) + it;
 
-                const Eigen::Matrix3d M =
-                        (Eigen::Matrix3d() << 1.0 - nt(0) * nt(0),
-                         -nt(0) * nt(1), -nt(0) * nt(2), -nt(0) * nt(1),
-                         1.0 - nt(1) * nt(1), -nt(1) * nt(2), -nt(0) * nt(2),
-                         -nt(1) * nt(2), 1.0 - nt(2) * nt(2))
-                                .finished();
+                const Eigen::Matrix3d &M =
+                        Eigen::Matrix3d::Identity() - nt * nt.transpose();
+                const Eigen::Vector3d &ditM = dit.transpose() * M;
 
-                const Eigen::Vector3d &ditM = -dit.transpose() * M;
                 J_r[1].block<3, 1>(0, 0) =
                         sqrt_lambda_photometric * vs.cross(ditM);
                 J_r[1].block<3, 1>(3, 0) = sqrt_lambda_photometric * ditM;
-                r[1] = sqrt_lambda_photometric * (is - is0_proj);
+                r[1] = sqrt_lambda_photometric * (is_proj - is);
                 w[1] = kernel_->Weight(r[1]);
             };
 

diff --git a/cpp/tests/pipelines/registration/ColoredICP.cpp b/cpp/tests/pipelines/registration/ColoredICP.cpp
@@ -24,12 +24,34 @@
 // IN THE SOFTWARE.
 // ----------------------------------------------------------------------------
 
+#include "open3d/pipelines/registration/ColoredICP.h"
+
+#include "open3d/io/PointCloudIO.h"
 #include "tests/Tests.h"
 
 namespace open3d {
 namespace tests {
 
-TEST(ColoredICP, DISABLED_RegistrationColoredICP) { NotImplemented(); }
+TEST(ColoredICP, RegistrationColoredICP) {
+    data::DemoICPPointClouds dataset;
+    std::shared_ptr<geometry::PointCloud> src =
+            io::CreatePointCloudFromFile(dataset.GetPaths()[0]);
+    std::shared_ptr<geometry::PointCloud> dst =
+            io::CreatePointCloudFromFile(dataset.GetPaths()[1]);
+
+    Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
+    auto result = pipelines::registration::RegistrationColoredICP(
+            *src, *dst, 0.07, transformation,
+            pipelines::registration::TransformationEstimationForColoredICP(),
+            pipelines::registration::ICPConvergenceCriteria(1e-6, 1e-6, 50));
+    transformation = result.transformation_;
+
+    Eigen::Matrix4d ref_transformation;
+    ref_transformation << 0.748899, 0.00425476, 0.662671, -0.275425, 0.115777,
+            0.98376, -0.137159, 0.108227, -0.652492, 0.17944, 0.736244, 1.21057,
+            0, 0, 0, 1;
+    ExpectEQ(ref_transformation, transformation, /*threshold=*/1e-4);
+}
 
 TEST(ColoredICP, DISABLED_ICPConvergenceCriteria) { NotImplemented(); }