-
Notifications
You must be signed in to change notification settings - Fork 2.2k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Refactor math for ColoredICP #4988
Refactor math for ColoredICP #4988
Conversation
Thanks for submitting this pull request! The maintainers of this repository would appreciate if you could update the CHANGELOG.md based on your changes. |
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;
^~~ |
My bad. I should not swap the line of |
We need to add a unit test to verify that the results are numerically the same as 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 make tests -j20 && ./bin/tests --gtest_filter="*RegistrationColoredICP*" |
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). |
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.) |
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:
is0_proj
tois_proj
dit.transpose() * M;
for Jacobian and swapis
andis_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.This change is