Refactors and fixes Symmetric ICP

eclipse0922 · eclipse0922 · commit 02720e1e78b1 · 2025-06-16T00:13:35.000+09:00
Refactors the Symmetric ICP implementation for better readability
and consistency. Addresses minor issues in the symmetric
transformation estimation and improves code formatting.
diff --git a/cpp/open3d/pipelines/registration/SymmetricICP.cpp b/cpp/open3d/pipelines/registration/SymmetricICP.cpp
@@ -88,22 +88,23 @@ Eigen::Matrix4d TransformationEstimationSymmetric::ComputeTransformation(
 
 std::tuple<std::shared_ptr<const geometry::PointCloud>,
            std::shared_ptr<const geometry::PointCloud>>
-TransformationEstimationSymmetric::
-        InitializePointCloudsForTransformation(
-                const geometry::PointCloud &source,
-                const geometry::PointCloud &target,
-                double max_correspondence_distance) const {
+TransformationEstimationSymmetric::InitializePointCloudsForTransformation(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        double max_correspondence_distance) const {
     if (!target.HasNormals() || !source.HasNormals()) {
-        utility::LogError("SymmetricICP requires both source and target to "
-                          "have normals.");
+        utility::LogError(
+                "SymmetricICP requires both source and target to "
+                "have normals.");
     }
     std::shared_ptr<const geometry::PointCloud> source_initialized_c(
             &source, [](const geometry::PointCloud *) {});
     std::shared_ptr<const geometry::PointCloud> target_initialized_c(
             &target, [](const geometry::PointCloud *) {});
     if (!source_initialized_c || !target_initialized_c) {
-        utility::LogError("Internal error: InitializePointCloudsFor"
-                          "Transformation returns nullptr.");
+        utility::LogError(
+                "Internal error: InitializePointCloudsFor"
+                "Transformation returns nullptr.");
     }
     return std::make_tuple(source_initialized_c, target_initialized_c);
 }
@@ -122,4 +123,3 @@ RegistrationResult RegistrationSymmetricICP(
 }  // namespace registration
 }  // namespace pipelines
 }  // namespace open3d
-
diff --git a/cpp/open3d/pipelines/registration/SymmetricICP.h b/cpp/open3d/pipelines/registration/SymmetricICP.h
@@ -74,4 +74,3 @@ RegistrationResult RegistrationSymmetricICP(
 }  // namespace registration
 }  // namespace pipelines
 }  // namespace open3d
-
diff --git a/cpp/open3d/t/pipelines/kernel/Registration.cpp b/cpp/open3d/t/pipelines/kernel/Registration.cpp
@@ -63,13 +63,11 @@ core::Tensor ComputePoseSymmetric(const core::Tensor &source_points,
     int inlier_count = 0;
 
     if (source_points.IsCPU()) {
-        ComputePoseSymmetricCPU(source_points.Contiguous(),
-                                target_points.Contiguous(),
-                                source_normals.Contiguous(),
-                                target_normals.Contiguous(),
-                                correspondence_indices.Contiguous(), pose,
-                                residual, inlier_count, source_points.GetDtype(),
-                                device, kernel);
+        ComputePoseSymmetricCPU(
+                source_points.Contiguous(), target_points.Contiguous(),
+                source_normals.Contiguous(), target_normals.Contiguous(),
+                correspondence_indices.Contiguous(), pose, residual,
+                inlier_count, source_points.GetDtype(), device, kernel);
     } else {
         utility::LogError("Unimplemented device.");
     }
diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp b/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp
@@ -128,15 +128,14 @@ void ComputePosePointToPlaneCPU(const core::Tensor &source_points,
 }
 
 template <typename scalar_t, typename funct_t>
-static void ComputePoseSymmetricKernelCPU(
-        const scalar_t *source_points_ptr,
-        const scalar_t *target_points_ptr,
-        const scalar_t *source_normals_ptr,
-        const scalar_t *target_normals_ptr,
-        const int64_t *correspondence_indices,
-        const int n,
-        scalar_t *global_sum,
-        funct_t GetWeightFromRobustKernel) {
+static void ComputePoseSymmetricKernelCPU(const scalar_t *source_points_ptr,
+                                          const scalar_t *target_points_ptr,
+                                          const scalar_t *source_normals_ptr,
+                                          const scalar_t *target_normals_ptr,
+                                          const int64_t *correspondence_indices,
+                                          const int n,
+                                          scalar_t *global_sum,
+                                          funct_t GetWeightFromRobustKernel) {
     std::vector<scalar_t> A_1x29(29, 0.0);
 #ifdef _WIN32
     std::vector<scalar_t> zeros_29(29, 0.0);
@@ -174,27 +173,27 @@ static void ComputePoseSymmetricKernelCPU(
                     scalar_t r1 = dx * ntx + dy * nty + dz * ntz;
                     scalar_t r2 = dx * nsx + dy * nsy + dz * nsz;
 
-                    scalar_t J1[6] = { -sz * nty + sy * ntz,
-                                        sz * ntx - sx * ntz,
-                                        -sy * ntx + sx * nty,
-                                        ntx,
-                                        nty,
-                                        ntz };
-                    scalar_t J2[6] = { -sz * nsy + sy * nsz,
-                                        sz * nsx - sx * nsz,
-                                        -sy * nsx + sx * nsy,
-                                        nsx,
-                                        nsy,
-                                        nsz };
+                    scalar_t J1[6] = {-sz * nty + sy * ntz,
+                                      sz * ntx - sx * ntz,
+                                      -sy * ntx + sx * nty,
+                                      ntx,
+                                      nty,
+                                      ntz};
+                    scalar_t J2[6] = {-sz * nsy + sy * nsz,
+                                      sz * nsx - sx * nsz,
+                                      -sy * nsx + sx * nsy,
+                                      nsx,
+                                      nsy,
+                                      nsz};
 
                     scalar_t w1 = GetWeightFromRobustKernel(r1);
                     scalar_t w2 = GetWeightFromRobustKernel(r2);
 
                     int i = 0;
                     for (int j = 0; j < 6; ++j) {
                         for (int k = 0; k <= j; ++k) {
-                            A_reduction[i] += J1[j] * w1 * J1[k] +
-                                              J2[j] * w2 * J2[k];
+                            A_reduction[i] +=
+                                    J1[j] * w1 * J1[k] + J2[j] * w2 * J2[k];
                             ++i;
                         }
                         A_reduction[21 + j] +=
@@ -221,16 +220,16 @@ static void ComputePoseSymmetricKernelCPU(
 }
 
 void ComputePoseSymmetricCPU(const core::Tensor &source_points,
-                              const core::Tensor &target_points,
-                              const core::Tensor &source_normals,
-                              const core::Tensor &target_normals,
-                              const core::Tensor &correspondence_indices,
-                              core::Tensor &pose,
-                              float &residual,
-                              int &inlier_count,
-                              const core::Dtype &dtype,
-                              const core::Device &device,
-                              const registration::RobustKernel &kernel) {
+                             const core::Tensor &target_points,
+                             const core::Tensor &source_normals,
+                             const core::Tensor &target_normals,
+                             const core::Tensor &correspondence_indices,
+                             core::Tensor &pose,
+                             float &residual,
+                             int &inlier_count,
+                             const core::Dtype &dtype,
+                             const core::Device &device,
+                             const registration::RobustKernel &kernel) {
     int n = source_points.GetLength();
 
     core::Tensor global_sum = core::Tensor::Zeros({29}, dtype, device);
diff --git a/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp b/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp
@@ -174,7 +174,8 @@ double TransformationEstimationSymmetric::ComputeRMSE(
     }
     if (!target.HasPointNormals() || !source.HasPointNormals()) {
         utility::LogError(
-                "SymmetricICP requires both source and target to have normals.");
+                "SymmetricICP requires both source and target to have "
+                "normals.");
     }
 
     core::AssertTensorDtype(target.GetPointPositions(),
@@ -199,11 +200,12 @@ double TransformationEstimationSymmetric::ComputeRMSE(
     core::Tensor diff = source_points_indexed - target_points_indexed;
     core::Tensor r1 = diff.Mul(target_normals_indexed).Sum({1});
     core::Tensor r2 = diff.Mul(source_normals_indexed).Sum({1});
-    
+
     // Compute symmetric error
     core::Tensor error_t = r1.Mul(r1) + r2.Mul(r2);
     double error = error_t.Sum({0}).To(core::Float64).Item<double>();
-    return std::sqrt(error / static_cast<double>(neighbour_indices.GetLength()));
+    return std::sqrt(error /
+                     static_cast<double>(neighbour_indices.GetLength()));
 }
 
 core::Tensor TransformationEstimationSymmetric::ComputeTransformation(
@@ -217,7 +219,8 @@ core::Tensor TransformationEstimationSymmetric::ComputeTransformation(
     }
     if (!target.HasPointNormals() || !source.HasPointNormals()) {
         utility::LogError(
-                "SymmetricICP requires both source and target to have normals.");
+                "SymmetricICP requires both source and target to have "
+                "normals.");
     }
 
     core::AssertTensorDtypes(source.GetPointPositions(),
@@ -235,8 +238,8 @@ core::Tensor TransformationEstimationSymmetric::ComputeTransformation(
     // Get pose {6} of type Float64.
     core::Tensor pose = pipelines::kernel::ComputePoseSymmetric(
             source.GetPointPositions(), target.GetPointPositions(),
-            source.GetPointNormals(), target.GetPointNormals(),
-            correspondences, this->kernel_);
+            source.GetPointNormals(), target.GetPointNormals(), correspondences,
+            this->kernel_);
 
     // Get rigid transformation tensor of {4, 4} of type Float64 on CPU:0
     // device, from pose {6}.
diff --git a/cpp/pybind/pipelines/registration/registration.cpp b/cpp/pybind/pipelines/registration/registration.cpp
@@ -16,8 +16,8 @@
 #include "open3d/pipelines/registration/FastGlobalRegistration.h"
 #include "open3d/pipelines/registration/Feature.h"
 #include "open3d/pipelines/registration/GeneralizedICP.h"
-#include "open3d/pipelines/registration/SymmetricICP.h"
 #include "open3d/pipelines/registration/RobustKernel.h"
+#include "open3d/pipelines/registration/SymmetricICP.h"
 #include "open3d/pipelines/registration/TransformationEstimation.h"
 #include "open3d/utility/Logging.h"
 #include "pybind/docstring.h"
@@ -109,10 +109,9 @@ void pybind_registration_declarations(py::module &m) {
     py::class_<TransformationEstimationSymmetric,
                PyTransformationEstimation<TransformationEstimationSymmetric>,
                TransformationEstimation>
-            te_sym(m_registration,
-                    "TransformationEstimationSymmetric",
-                    "Class to estimate a transformation for symmetric "
-                    "point to plane distance.");
+            te_sym(m_registration, "TransformationEstimationSymmetric",
+                   "Class to estimate a transformation for symmetric "
+                   "point to plane distance.");
     py::class_<
             TransformationEstimationForColoredICP,
             PyTransformationEstimation<TransformationEstimationForColoredICP>,
@@ -318,8 +317,8 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``.
     auto te_sym = static_cast<py::class_<
             TransformationEstimationSymmetric,
             PyTransformationEstimation<TransformationEstimationSymmetric>,
-            TransformationEstimation>>(m_registration.attr(
-            "TransformationEstimationSymmetric"));
+            TransformationEstimation>>(
+            m_registration.attr("TransformationEstimationSymmetric"));
     py::detail::bind_default_constructor<TransformationEstimationSymmetric>(
             te_sym);
     py::detail::bind_copy_functions<TransformationEstimationSymmetric>(te_sym);
@@ -332,10 +331,9 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``.
                  [](const TransformationEstimationSymmetric &te) {
                      return std::string("TransformationEstimationSymmetric");
                  })
-            .def_readwrite(
-                    "kernel",
-                    &TransformationEstimationSymmetric::kernel_,
-                    "Robust Kernel used in the Optimization");
+            .def_readwrite("kernel",
+                           &TransformationEstimationSymmetric::kernel_,
+                           "Robust Kernel used in the Optimization");
 
     // open3d.registration.TransformationEstimationForColoredICP :
     auto te_col = static_cast<py::class_<
@@ -655,11 +653,11 @@ must hold true for all edges.)");
                     {"criteria", "Convergence criteria"},
                     {"estimation_method",
                      "Estimation method. One of "
-                    "(``TransformationEstimationPointToPoint``, "
-                    "``TransformationEstimationPointToPlane``, "
-                    "``TransformationEstimationSymmetric``, "
-                    "``TransformationEstimationForGeneralizedICP``, "
-                    "``TransformationEstimationForColoredICP``)"},
+                     "(``TransformationEstimationPointToPoint``, "
+                     "``TransformationEstimationPointToPlane``, "
+                     "``TransformationEstimationSymmetric``, "
+                     "``TransformationEstimationForGeneralizedICP``, "
+                     "``TransformationEstimationForColoredICP``)"},
                     {"init", "Initial transformation estimation"},
                     {"lambda_geometric", "lambda_geometric value"},
                     {"epsilon", "epsilon value"},
@@ -706,8 +704,7 @@ must hold true for all edges.)");
             "Function for symmetric ICP registration", "source"_a, "target"_a,
             "max_correspondence_distance"_a,
             "init"_a = Eigen::Matrix4d::Identity(),
-            "estimation_method"_a =
-                    TransformationEstimationSymmetric(),
+            "estimation_method"_a = TransformationEstimationSymmetric(),
             "criteria"_a = ICPConvergenceCriteria());
     docstring::FunctionDocInject(m_registration, "registration_symmetric_icp",
                                  map_shared_argument_docstrings);
diff --git a/cpp/pybind/t/pipelines/registration/registration.cpp b/cpp/pybind/t/pipelines/registration/registration.cpp
@@ -494,8 +494,7 @@ void pybind_registration_definitions(py::module &m) {
                     core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")),
             "estimation_method"_a = TransformationEstimationSymmetric(),
             "criteria"_a = ICPConvergenceCriteria());
-    docstring::FunctionDocInject(m_registration,
-                                 "registration_symmetric_icp",
+    docstring::FunctionDocInject(m_registration, "registration_symmetric_icp",
                                  map_shared_argument_docstrings);
 
     m_registration.def(
diff --git a/cpp/tests/pipelines/registration/SymmetricICP.cpp b/cpp/tests/pipelines/registration/SymmetricICP.cpp
@@ -84,7 +84,7 @@ TEST(SymmetricICP, TransformationEstimationSymmetricComputeTransformation) {
     registration::TransformationEstimationSymmetric estimation;
 
     Eigen::Matrix4d transformation =
-        estimation.ComputeTransformation(source, target, corres);
+            estimation.ComputeTransformation(source, target, corres);
 
     // Should be close to identity for perfect correspondence
     EXPECT_TRUE(transformation.isApprox(Eigen::Matrix4d::Identity(), 1e-3));
@@ -98,7 +98,7 @@ TEST(SymmetricICP,
     registration::TransformationEstimationSymmetric estimation;
 
     Eigen::Matrix4d transformation =
-        estimation.ComputeTransformation(source, target, corres);
+            estimation.ComputeTransformation(source, target, corres);
 
     EXPECT_TRUE(transformation.isApprox(Eigen::Matrix4d::Identity()));
 }
@@ -116,7 +116,7 @@ TEST(SymmetricICP,
     registration::TransformationEstimationSymmetric estimation;
 
     Eigen::Matrix4d transformation =
-        estimation.ComputeTransformation(source, target, corres);
+            estimation.ComputeTransformation(source, target, corres);
 
     EXPECT_TRUE(transformation.isApprox(Eigen::Matrix4d::Identity()));
 }
@@ -130,18 +130,17 @@ TEST(SymmetricICP, RegistrationSymmetricICP) {
     source.normals_ = {{0, 0, 1}, {0, 0, 1}, {0, 0, 1}};
 
     // Target is slightly translated
-    target.points_ = {{0.05, 0.05, 0.05},
-                      {1.05, 0.05, 0.05},
-                      {0.05, 1.05, 0.05}};
+    target.points_ = {
+            {0.05, 0.05, 0.05}, {1.05, 0.05, 0.05}, {0.05, 1.05, 0.05}};
     target.normals_ = {{0, 0, 1}, {0, 0, 1}, {0, 0, 1}};
 
     registration::TransformationEstimationSymmetric estimation;
     registration::ICPConvergenceCriteria criteria;
 
     registration::RegistrationResult result =
-            registration::RegistrationSymmetricICP(
-                    source, target, 0.1, Eigen::Matrix4d::Identity(),
-                    estimation, criteria);
+            registration::RegistrationSymmetricICP(source, target, 0.1,
+                                                   Eigen::Matrix4d::Identity(),
+                                                   estimation, criteria);
 
     EXPECT_GT(result.correspondence_set_.size(), 0);
     EXPECT_GT(result.fitness_, 0.0);
@@ -164,13 +163,13 @@ TEST(SymmetricICP, RegistrationSymmetricICPConvergence) {
         double z = rand_gen();
 
         source.points_.push_back({x, y, z});
-        source.normals_.push_back({0, 0, 1}); // Simple normal for testing
+        source.normals_.push_back({0, 0, 1});  // Simple normal for testing
     }
 
     // Create target by transforming source with known transformation
     Eigen::Matrix4d true_transformation = Eigen::Matrix4d::Identity();
-    true_transformation(0, 3) = 0.1; // Small translation in x
-    true_transformation(1, 3) = 0.05; // Small translation in y
+    true_transformation(0, 3) = 0.1;   // Small translation in x
+    true_transformation(1, 3) = 0.05;  // Small translation in y
 
     target = source;
     target.Transform(true_transformation);
@@ -179,9 +178,9 @@ TEST(SymmetricICP, RegistrationSymmetricICPConvergence) {
     registration::ICPConvergenceCriteria criteria(1e-6, 1e-6, 30);
 
     registration::RegistrationResult result =
-            registration::RegistrationSymmetricICP(
-                    source, target, 0.5, Eigen::Matrix4d::Identity(),
-                    estimation, criteria);
+            registration::RegistrationSymmetricICP(source, target, 0.5,
+                                                   Eigen::Matrix4d::Identity(),
+                                                   estimation, criteria);
 
     // Check that registration converged to reasonable result
     EXPECT_GT(result.fitness_, 0.5);
diff --git a/cpp/tests/t/pipelines/registration/TransformationEstimation.cpp b/cpp/tests/t/pipelines/registration/TransformationEstimation.cpp
diff --git a/python/test/t/registration/test_registration.py b/python/test/t/registration/test_registration.py
diff --git a/python/test/test_symmetric_icp.py b/python/test/test_symmetric_icp.py
