Implements the Symmetric Iterative Closest Point (ICP) algorithm for point cloud registration. (#7276)

This change introduces a new method for aligning point clouds that considers the symmetry between source and target, leading to potentially more accurate and robust registration results, especially when dealing with noisy or incomplete data.

Includes C++ and Python implementations with corresponding tests.

Adds CUDA and SYCL implementation for the Symmetric ICP registration algorithm, enabling it to run on GPUs.

---------

Co-authored-by: sewon.jeon <sewon.jeon@connecteve.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

Author: 3 people

CHANGELOG.md

@@ -1,4 +1,5 @@
 ## Main
+-   Add symmetric ICP registration to the legacy and Tensor pipelines (PR #7276).
 -   Replace OpenMP with oneAPI TBB for all CPU parallelism; Open3D no longer depends on OpenMP. This removes the `libomp` / `libgomp` runtime dependency and the thread oversubscription and crashes caused by loading multiple OpenMP runtimes in one process (e.g. alongside PyTorch in Python). The `WITH_OPENMP` CMake option is removed, oneTBB >= 2021.4.0 is required, and `OMP_NUM_THREADS` is replaced by `open3d.utility.set_max_threads()` (C++: `utility::SetMaxThreads()` or a `tbb::task_arena`). `utility::OMPProgressBar` is removed in favor of the thread-safe `utility::ProgressBar`; `utility::GetThreadNum()` and `utility::InParallel()` are removed (PR #6626) (issues #6196, #6544, #6750)
 -   Add point cloud smoothing algorithms: Moving Least Squares (MLS), Laplacian, Taubin, and bilateral smoothing. These methods provide flexible noise reduction for point clouds with different preservation characteristics (PR #7419).
 -   Add vcpkg support for easier dependency management (PR #7386)

cpp/open3d/Open3D.h.in

@@ -64,6 +64,7 @@
 #include "open3d/pipelines/registration/GeneralizedICP.h"
 #include "open3d/pipelines/registration/GlobalOptimization.h"
 #include "open3d/pipelines/registration/Registration.h"
+#include "open3d/pipelines/registration/SymmetricICP.h"
 #include "open3d/pipelines/registration/TransformationEstimation.h"
 #include "open3d/t/geometry/Geometry.h"
 #include "open3d/t/geometry/Image.h"

cpp/open3d/pipelines/CMakeLists.txt

@@ -23,6 +23,7 @@ target_sources(pipelines PRIVATE
     registration/FastGlobalRegistration.cpp
     registration/Feature.cpp
     registration/GeneralizedICP.cpp
+    registration/SymmetricICP.cpp
     registration/GlobalOptimization.cpp
     registration/PoseGraph.cpp
     registration/Registration.cpp

cpp/open3d/pipelines/registration/SymmetricICP.cpp

@@ -0,0 +1,208 @@
+// ----------------------------------------------------------------------------
+// -                        Open3D: www.open3d.org                            -
+// ----------------------------------------------------------------------------
+// Copyright (c) 2018-2024 www.open3d.org
+// SPDX-License-Identifier: MIT
+// ----------------------------------------------------------------------------
+
+#include "open3d/pipelines/registration/SymmetricICP.h"
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_reduce.h>
+
+#include <cmath>
+#include <cstddef>
+
+#include "open3d/geometry/PointCloud.h"
+#include "open3d/pipelines/registration/SymmetricICPImpl.h"
+#include "open3d/utility/Eigen.h"
+#include "open3d/utility/Logging.h"
+
+namespace open3d {
+namespace pipelines {
+namespace registration {
+namespace {
+
+void ValidateSymmetricICPNormals(const geometry::PointCloud &source,
+                                 const geometry::PointCloud &target) {
+    if (!source.HasNormals() || !target.HasNormals()) {
+        utility::LogError(
+                "SymmetricICP requires both source and target to have "
+                "normals.");
+    }
+}
+
+void ValidateSymmetricICPCorrespondences(const geometry::PointCloud &source,
+                                         const geometry::PointCloud &target,
+                                         const CorrespondenceSet &corres) {
+    for (const Eigen::Vector2i &correspondence : corres) {
+        if (correspondence[0] < 0 || correspondence[1] < 0 ||
+            static_cast<std::size_t>(correspondence[0]) >=
+                    source.points_.size() ||
+            static_cast<std::size_t>(correspondence[1]) >=
+                    target.points_.size()) {
+            utility::LogError(
+                    "SymmetricICP correspondence ({}, {}) is out of range for "
+                    "source size {} and target size {}.",
+                    correspondence[0], correspondence[1], source.points_.size(),
+                    target.points_.size());
+        }
+    }
+}
+
+Eigen::Vector3d GetSymmetricNormal(const Eigen::Vector3d &source_normal,
+                                   const Eigen::Vector3d &target_normal) {
+    if (source_normal.dot(target_normal) < 0.0) {
+        return target_normal - source_normal;
+    }
+    return target_normal + source_normal;
+}
+
+struct CorrespondenceSums {
+    Eigen::Vector3d source = Eigen::Vector3d::Zero();
+    Eigen::Vector3d target = Eigen::Vector3d::Zero();
+};
+
+struct NormalEquations {
+    Eigen::Matrix6d JTJ = Eigen::Matrix6d::Zero();
+    Eigen::Vector6d JTr = Eigen::Vector6d::Zero();
+};
+
+}  // namespace
+
+double TransformationEstimationSymmetric::ComputeRMSE(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres) const {
+    ValidateSymmetricICPNormals(source, target);
+    ValidateSymmetricICPCorrespondences(source, target, corres);
+    if (corres.empty()) {
+        return 0.0;
+    }
+
+    double err = 0.0;
+    for (const auto &c : corres) {
+        const Eigen::Vector3d &source_point = source.points_[c[0]];
+        const Eigen::Vector3d &target_point = target.points_[c[1]];
+        const Eigen::Vector3d normal = GetSymmetricNormal(
+                source.normals_[c[0]], target.normals_[c[1]]);
+        const double residual = (source_point - target_point).dot(normal);
+        err += residual * residual;
+    }
+    return std::sqrt(err / static_cast<double>(corres.size()));
+}
+
+Eigen::Matrix4d TransformationEstimationSymmetric::ComputeTransformation(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres) const {
+    ValidateSymmetricICPNormals(source, target);
+    ValidateSymmetricICPCorrespondences(source, target, corres);
+    if (corres.empty()) {
+        return Eigen::Matrix4d::Identity();
+    }
+
+    const CorrespondenceSums sums = tbb::parallel_reduce(
+            tbb::blocked_range<std::size_t>(0, corres.size()),
+            CorrespondenceSums(),
+            [&](const tbb::blocked_range<std::size_t> &range,
+                CorrespondenceSums local) {
+                for (std::size_t i = range.begin(); i != range.end(); ++i) {
+                    local.source += source.points_[corres[i][0]];
+                    local.target += target.points_[corres[i][1]];
+                }
+                return local;
+            },
+            [](CorrespondenceSums lhs, const CorrespondenceSums &rhs) {
+                lhs.source += rhs.source;
+                lhs.target += rhs.target;
+                return lhs;
+            });
+    const double inverse_count = 1.0 / static_cast<double>(corres.size());
+    const Eigen::Vector3d source_mean = sums.source * inverse_count;
+    const Eigen::Vector3d target_mean = sums.target * inverse_count;
+
+    // Centering the correspondences decouples the symmetric rotation and
+    // translation system while the robust weight uses the raw residual.
+    const NormalEquations equations = tbb::parallel_reduce(
+            tbb::blocked_range<std::size_t>(0, corres.size()),
+            NormalEquations(),
+            [&](const tbb::blocked_range<std::size_t> &range,
+                NormalEquations local) {
+                for (std::size_t i = range.begin(); i != range.end(); ++i) {
+                    const Eigen::Vector3d &source_point =
+                            source.points_[corres[i][0]];
+                    const Eigen::Vector3d &target_point =
+                            target.points_[corres[i][1]];
+                    const Eigen::Vector3d normal =
+                            GetSymmetricNormal(source.normals_[corres[i][0]],
+                                               target.normals_[corres[i][1]]);
+                    const Eigen::Vector3d source_centered =
+                            source_point - source_mean;
+                    const Eigen::Vector3d target_centered =
+                            target_point - target_mean;
+                    const double raw_residual =
+                            (source_point - target_point).dot(normal);
+                    const double residual =
+                            (source_centered - target_centered).dot(normal);
+
+                    Eigen::Vector6d jacobian;
+                    jacobian.head<3>() =
+                            (source_centered + target_centered).cross(normal);
+                    jacobian.tail<3>() = normal;
+
+                    const double weight = kernel_->Weight(raw_residual);
+                    local.JTJ.noalias() +=
+                            weight * jacobian * jacobian.transpose();
+                    local.JTr.noalias() += weight * jacobian * residual;
+                }
+                return local;
+            },
+            [](NormalEquations lhs, const NormalEquations &rhs) {
+                lhs.JTJ += rhs.JTJ;
+                lhs.JTr += rhs.JTr;
+                return lhs;
+            });
+
+    bool is_success = false;
+    Eigen::Vector6d pose;
+    std::tie(is_success, pose) =
+            utility::SolveLinearSystemPSD(equations.JTJ, -equations.JTr);
+    return is_success ? TransformSymmetricPoseToMatrix4d(pose, source_mean,
+                                                         target_mean)
+                      : Eigen::Matrix4d::Identity();
+}
+
+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 {
+    ValidateSymmetricICPNormals(source, target);
+    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.");
+    }
+    return std::make_tuple(source_initialized_c, target_initialized_c);
+}
+
+RegistrationResult RegistrationSymmetricICP(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        double max_correspondence_distance,
+        const Eigen::Matrix4d &init,
+        const TransformationEstimationSymmetric &estimation,
+        const ICPConvergenceCriteria &criteria) {
+    return RegistrationICP(source, target, max_correspondence_distance, init,
+                           estimation, criteria);
+}
+
+}  // namespace registration
+}  // namespace pipelines
+}  // namespace open3d

cpp/open3d/pipelines/registration/SymmetricICP.h

@@ -0,0 +1,114 @@
+// ----------------------------------------------------------------------------
+// -                        Open3D: www.open3d.org                            -
+// ----------------------------------------------------------------------------
+// Copyright (c) 2018-2024 www.open3d.org
+// SPDX-License-Identifier: MIT
+// ----------------------------------------------------------------------------
+
+#pragma once
+
+#include "open3d/pipelines/registration/Registration.h"
+#include "open3d/pipelines/registration/RobustKernel.h"
+#include "open3d/pipelines/registration/TransformationEstimation.h"
+
+namespace open3d {
+
+namespace geometry {
+class PointCloud;
+}
+
+namespace pipelines {
+namespace registration {
+
+class RegistrationResult;
+
+/// \brief Estimates a source-to-target transformation with symmetric ICP.
+///
+/// For each correspondence, \f$p\f$ is the source point and \f$q\f$ is the
+/// target point in the current aligned frame, with corresponding normals
+/// \f$n_p\f$ and \f$n_q\f$. After aligning the normal directions, the
+/// objective uses the single residual \f$(p-q)^T(n_p+n_q)\f$.
+class TransformationEstimationSymmetric : public TransformationEstimation {
+public:
+    ~TransformationEstimationSymmetric() override = default;
+
+    TransformationEstimationType GetTransformationEstimationType()
+            const override {
+        return type_;
+    };
+    /// \brief Constructs a symmetric transformation estimator.
+    /// \param kernel Robust kernel applied to the symmetric residual.
+    explicit TransformationEstimationSymmetric(
+            std::shared_ptr<RobustKernel> kernel = std::make_shared<L2Loss>())
+        : kernel_(std::move(kernel)) {}
+
+    /// \brief Computes the symmetric point-to-plane RMSE.
+    /// \param source Source point cloud in the current aligned frame.
+    /// \param target Target point cloud in the current aligned frame.
+    /// \param corres Source-to-target correspondence indices.
+    /// \return The symmetric point-to-plane RMSE.
+    /// \throw std::runtime_error If either point cloud lacks normals or a
+    /// correspondence index is out of range.
+    double ComputeRMSE(const geometry::PointCloud &source,
+                       const geometry::PointCloud &target,
+                       const CorrespondenceSet &corres) const override;
+
+    /// \brief Estimates a source-to-target transformation update.
+    /// \param source Source point cloud in the current aligned frame.
+    /// \param target Target point cloud in the current aligned frame.
+    /// \param corres Source-to-target correspondence indices.
+    /// \return The source-to-target transformation update.
+    /// \throw std::runtime_error If either point cloud lacks normals or a
+    /// correspondence index is out of range.
+    Eigen::Matrix4d ComputeTransformation(
+            const geometry::PointCloud &source,
+            const geometry::PointCloud &target,
+            const CorrespondenceSet &corres) const override;
+
+    /// \brief Validates and initializes point clouds for symmetric ICP.
+    /// \param source Source point cloud.
+    /// \param target Target point cloud.
+    /// \param max_correspondence_distance Maximum correspondence distance.
+    /// \return The initialized source and target point clouds.
+    /// \throw std::runtime_error If either point cloud lacks normals.
+    std::tuple<std::shared_ptr<const geometry::PointCloud>,
+               std::shared_ptr<const geometry::PointCloud>>
+    InitializePointCloudsForTransformation(
+            const geometry::PointCloud &source,
+            const geometry::PointCloud &target,
+            double max_correspondence_distance) const override;
+
+    /// shared_ptr to an Abstract RobustKernel that could mutate at runtime.
+    std::shared_ptr<RobustKernel> kernel_ = std::make_shared<L2Loss>();
+
+private:
+    const TransformationEstimationType type_ =
+            TransformationEstimationType::SymmetricICP;
+};
+
+/// \brief Registers source to target with symmetric point-to-plane ICP.
+///
+/// For each correspondence in the current aligned frame, \f$p\f$ and
+/// \f$n_p\f$ denote the source point and normal, while \f$q\f$ and \f$n_q\f$
+/// denote the target point and normal. After aligning normal directions, the
+/// objective uses the single residual \f$(p-q)^T(n_p+n_q)\f$.
+/// \param source Source point cloud with normals.
+/// \param target Target point cloud with normals.
+/// \param max_correspondence_distance Maximum correspondence distance.
+/// \param init Initial source-to-target transformation.
+/// \param estimation Symmetric transformation estimator.
+/// \param criteria ICP convergence criteria.
+/// \return The registration result with a source-to-target transformation.
+/// \throw std::runtime_error If either point cloud lacks normals.
+RegistrationResult RegistrationSymmetricICP(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        double max_correspondence_distance,
+        const Eigen::Matrix4d &init = Eigen::Matrix4d::Identity(),
+        const TransformationEstimationSymmetric &estimation =
+                TransformationEstimationSymmetric(),
+        const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria());
+
+}  // namespace registration
+}  // namespace pipelines
+}  // namespace open3d

cpp/open3d/pipelines/registration/SymmetricICPImpl.h

@@ -0,0 +1,47 @@
+// ----------------------------------------------------------------------------
+// -                        Open3D: www.open3d.org                            -
+// ----------------------------------------------------------------------------
+// Copyright (c) 2018-2024 www.open3d.org
+// SPDX-License-Identifier: MIT
+// ----------------------------------------------------------------------------
+
+#pragma once
+
+#include <Eigen/Geometry>
+#include <cmath>
+
+#include "open3d/utility/Eigen.h"
+
+namespace open3d {
+namespace pipelines {
+namespace registration {
+
+inline Eigen::Matrix4d TransformSymmetricPoseToMatrix4d(
+        const Eigen::Vector6d &pose,
+        const Eigen::Vector3d &source_mean,
+        const Eigen::Vector3d &target_mean) {
+    const Eigen::Vector3d g = pose.head<3>();
+    const double g_norm = g.norm();
+    const double theta = std::atan(g_norm);
+
+    Eigen::Matrix3d half_rotation = Eigen::Matrix3d::Identity();
+    if (g_norm > 0.0) {
+        half_rotation = Eigen::AngleAxisd(theta, g / g_norm).toRotationMatrix();
+    }
+
+    // Symmetric ICP solves for a half-angle pose about correspondence means.
+    // Equation 11 converts it to the full rigid transformation.
+    const Eigen::Matrix3d rotation = half_rotation * half_rotation;
+    const Eigen::Vector3d translation =
+            target_mean + half_rotation * (pose.tail<3>() * std::cos(theta)) -
+            rotation * source_mean;
+
+    Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
+    transformation.block<3, 3>(0, 0) = rotation;
+    transformation.block<3, 1>(0, 3) = translation;
+    return transformation;
+}
+
+}  // namespace registration
+}  // namespace pipelines
+}  // namespace open3d