Refactor math for ColoredICP

[hanzheteng]

I made a few changes to improve the readability of the code and make it consistent with the paper: Colored Point Cloud Registration Revisited, ICCV 2017.

Specifically, changes are the following:

• Rename is0_proj to is_proj
• Remove the minus sign before dit.transpose() * M; for Jacobian and swap is and is_proj accordingly for residual. (Note that adding/removing a minus sign for Jacobian and residual at the same time will not affect the optimization.) After this change it is now consistent with the math derived in the paper. The gradient derived herein should not include the minus sign, which was confusing to me.
• Rewrite matrix M in its matrix form, which is more concise and readable.

---

This change is 

[update-docs]

Thanks for submitting this pull request! The maintainers of this repository would appreciate if you could update the CHANGELOG.md based on your changes.

[yxlao]

Compilation error:

Open3D/cpp/open3d/pipelines/registration/ColoredICP.cpp:172:35: error: ‘dit’ was not declared in this scope
                 double is_proj = (dit.dot(vs_proj - vt)) + it;
                                   ^~~

[hanzheteng]

My bad. I should not swap the line of dit and is_proj.

[yxlao]

We need to add a unit test to verify that the results are numerically the same as master branch.

Please add this test to your branch:

diff --git a/cpp/tests/pipelines/registration/ColoredICP.cpp b/cpp/tests/pipelines/registration/ColoredICP.cpp
index 3ed0bd9ab..0e707b05a 100644
--- 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);
+}

This test works on master, it should also work in your branch. To run the test, copmile with -DBUILD_UNIT_TESTS=ON and run

make tests -j20 && ./bin/tests --gtest_filter="*RegistrationColoredICP*"

[hanzheteng]

Thanks for the code and instructions. Unit test added. It passed the test on my Ubuntu 18 machine (with CMake 3.22 installed under virtualenv).

[hanzheteng]

For anyone who is interested in the math discussed herein for the Colored ICP algorithm, please see my note here.

My implementation of this algorithm is hosted in this repo. (I made it public just now.)