Add Symmetric ICP Implementation

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,134 @@
+// ----------------------------------------------------------------------------
+// -                        Open3D: www.open3d.org                            -
+// ----------------------------------------------------------------------------
+// Copyright (c) 2018-2024 www.open3d.org
+// SPDX-License-Identifier: MIT
+// ----------------------------------------------------------------------------
+
+#include "open3d/pipelines/registration/SymmetricICP.h"
+
+#include "open3d/geometry/PointCloud.h"
+#include "open3d/utility/Eigen.h"
+#include "open3d/utility/Logging.h"
+
+namespace open3d {
+namespace pipelines {
+namespace registration {
+
+double TransformationEstimationSymmetric::ComputeRMSE(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres) const {
+    if (corres.empty() || !target.HasNormals() || !source.HasNormals()) {
+        return 0.0;
+    }
+    double err = 0.0;
+    for (const auto &c : corres) {
+        const Eigen::Vector3d &vs = source.points_[c[0]];
+        const Eigen::Vector3d &vt = target.points_[c[1]];
+        const Eigen::Vector3d &ns = source.normals_[c[0]];
+        const Eigen::Vector3d &nt = target.normals_[c[1]];
+        Eigen::Vector3d d = vs - vt;
+        double r1 = d.dot(nt);
+        double r2 = d.dot(ns);
+        err += r1 * r1 + r2 * r2;
+    }
+    return std::sqrt(err / (double)corres.size());
+}
+
+Eigen::Matrix4d TransformationEstimationSymmetric::ComputeTransformation(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres) const {
+    if (corres.empty() || !target.HasNormals() || !source.HasNormals()) {
+        return Eigen::Matrix4d::Identity();
+    }
+
+    auto compute_jacobian_and_residual =
+            [&](int i,
+                std::vector<Eigen::Vector6d, utility::Vector6d_allocator> &J_r,
+                std::vector<double> &r, std::vector<double> &w) {
+                const Eigen::Vector3d &vs = source.points_[corres[i][0]];
+                const Eigen::Vector3d &vt = target.points_[corres[i][1]];
+                const Eigen::Vector3d &ns = source.normals_[corres[i][0]];
+                const Eigen::Vector3d &nt = target.normals_[corres[i][1]];
+                const Eigen::Vector3d d = vs - vt;
+
+                // Symmetric ICP always uses exactly 2 jacobians/residuals
+                // Ensure vectors have correct size (only resizes on first call)
+                if (J_r.size() != 2) {
+                    J_r.resize(2);
+                    r.resize(2);
+                    w.resize(2);
+                }
+
+                r[0] = d.dot(nt);
+                w[0] = kernel_->Weight(r[0]);
+                J_r[0].block<3, 1>(0, 0) = vs.cross(nt);
+                J_r[0].block<3, 1>(3, 0) = nt;
+
+                r[1] = d.dot(ns);
+                w[1] = kernel_->Weight(r[1]);
+                J_r[1].block<3, 1>(0, 0) = vs.cross(ns);
+                J_r[1].block<3, 1>(3, 0) = ns;
+            };
+
+    Eigen::Matrix6d JTJ;
+    Eigen::Vector6d JTr;
+    double r2;
+    std::tie(JTJ, JTr, r2) =
+            utility::ComputeJTJandJTr<Eigen::Matrix6d, Eigen::Vector6d>(
+                    compute_jacobian_and_residual, (int)corres.size());
+
+    bool is_success;
+    Eigen::Matrix4d extrinsic;
+    std::tie(is_success, extrinsic) =
+            utility::SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr);
+
+    return is_success ? extrinsic : 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 {
+    if (!target.HasNormals() || !source.HasNormals()) {
+        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.");
+    }
+    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) {
+    // Validate that both point clouds have normals
+    if (!source.HasNormals() || !target.HasNormals()) {
+        utility::LogError(
+                "SymmetricICP requires both source and target to have "
+                "normals.");
+    }
+
+    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,72 @@
+// ----------------------------------------------------------------------------
+// -                        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 Transformation estimation for symmetric point-to-plane ICP.
+class TransformationEstimationSymmetric : public TransformationEstimation {
+public:
+    ~TransformationEstimationSymmetric() override {};
+
+    TransformationEstimationType GetTransformationEstimationType()
+            const override {
+        return type_;
+    };
+    explicit TransformationEstimationSymmetric(
+            std::shared_ptr<RobustKernel> kernel = std::make_shared<L2Loss>())
+        : kernel_(std::move(kernel)) {}
+    double ComputeRMSE(const geometry::PointCloud &source,
+                       const geometry::PointCloud &target,
+                       const CorrespondenceSet &corres) const override;
+    Eigen::Matrix4d ComputeTransformation(
+            const geometry::PointCloud &source,
+            const geometry::PointCloud &target,
+            const CorrespondenceSet &corres) const override;
+
+    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::PointToPlane;
+};
+
+/// \brief Function for symmetric ICP registration using point-to-plane error.
+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/t/pipelines/kernel/Registration.cpp

@@ -51,6 +51,40 @@ core::Tensor ComputePosePointToPlane(const core::Tensor &source_points,
     return pose;
 }
 
+core::Tensor ComputePoseSymmetric(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,
+                                  const registration::RobustKernel &kernel) {
+    const core::Device device = source_points.GetDevice();
+    core::Tensor pose = core::Tensor::Empty({6}, core::Float64, device);
+    float residual = 0;
+    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);
+    } else if (source_points.IsCUDA()) {
+        core::CUDAScopedDevice scoped_device(source_points.GetDevice());
+        CUDA_CALL(ComputePoseSymmetricCUDA, 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.");
+    }
+
+    utility::LogDebug("Symmetric Transform: residual {}, inlier_count {}",
+                      residual, inlier_count);
+
+    return pose;
+}
+
 core::Tensor ComputePoseColoredICP(const core::Tensor &source_points,
                                    const core::Tensor &source_colors,
                                    const core::Tensor &target_points,

cpp/open3d/t/pipelines/kernel/Registration.h

@@ -36,6 +36,32 @@ core::Tensor ComputePosePointToPlane(const core::Tensor &source_positions,
                                      const core::Tensor &correspondence_indices,
                                      const registration::RobustKernel &kernel);
 
+/// \brief Computes pose for symmetric ICP registration method.
+///
+/// Symmetric ICP minimizes the sum of source-to-target and target-to-source
+/// point-to-plane distances, providing more symmetric and robust registration.
+///
+/// \param source_positions source point positions of Float32 or Float64 dtype.
+/// \param target_positions target point positions of same dtype as source point
+/// positions.
+/// \param source_normals source point normals of same dtype as source point
+/// positions.
+/// \param target_normals target point normals of same dtype as source point
+/// positions.
+/// \param correspondence_indices Tensor of type Int64 containing indices of
+/// corresponding target positions, where the value is the target index and the
+/// index of the value itself is the source index. It contains -1 as value at
+/// index with no correspondence.
+/// \param kernel statistical robust kernel for outlier rejection.
+/// \return Pose [alpha beta gamma, tx, ty, tz], a shape {6} tensor of dtype
+/// Float64, where alpha, beta, gamma are the Euler angles in the ZYX order.
+core::Tensor ComputePoseSymmetric(const core::Tensor &source_positions,
+                                  const core::Tensor &target_positions,
+                                  const core::Tensor &source_normals,
+                                  const core::Tensor &target_normals,
+                                  const core::Tensor &correspondence_indices,
+                                  const registration::RobustKernel &kernel);
+
 /// \brief Computes pose for colored-icp registration method.
 ///
 /// \param source_positions source point positions of Float32 or Float64 dtype.