Add Symmetric ICP Implementation

.github/workflows/ubuntu-wheel.yml

@@ -46,7 +46,7 @@ jobs:
     env:
       DEVELOPER_BUILD: ${{ github.event.inputs.developer_build || 'ON' }}
       PYTHON_VERSION: ${{ matrix.python_version }}
-      CCACHE_TAR_NAME: open3d-ubuntu-2004-cuda-ci-ccache
+      CCACHE_TAR_NAME: open3d-ubuntu-2204-cuda-ci-ccache
       OPEN3D_CPU_RENDERING: true
     steps:
       - name: Checkout source code
@@ -90,6 +90,14 @@ jobs:
             ${{ env.PIP_PKG_NAME }}
             ${{ env.PIP_CPU_PKG_NAME }}
           if-no-files-found: error
+      - name: Update devel release
+        if: ${{ github.ref == 'refs/heads/main' }}
+        env:
+          GH_TOKEN: ${{ github.token }}
+        run: |
+          gh release upload main-devel ${GITHUB_WORKSPACE}/${{ env.PIP_PKG_NAME }} \
+          ${GITHUB_WORKSPACE}/${{ env.PIP_CPU_PKG_NAME }} --clobber
+          gh release view main-devel
       - name: GCloud CLI auth
         if: ${{ github.ref == 'refs/heads/main' }}
         uses: 'google-github-actions/auth@v2'
@@ -106,14 +114,6 @@ jobs:
         if: ${{ github.ref == 'refs/heads/main' }}
         run: |
           gsutil cp ${GITHUB_WORKSPACE}/${{ env.CCACHE_TAR_NAME }}.tar.xz gs://open3d-ci-cache/
-      - name: Update devel release
-        if: ${{ github.ref == 'refs/heads/main' }}
-        env:
-          GH_TOKEN: ${{ github.token }}
-        run: |
-          gh release upload main-devel ${GITHUB_WORKSPACE}/${{ env.PIP_PKG_NAME }} \
-          ${GITHUB_WORKSPACE}/${{ env.PIP_CPU_PKG_NAME }} --clobber
-          gh release view main-devel
 
   test-wheel-cpu:
     name: Test wheel CPU

.github/workflows/windows.yml

@@ -98,7 +98,7 @@ jobs:
       - name: Set up Python version
         uses: actions/setup-python@v5
         with:
-          python-version: '3.10'
+          python-version: '3.12'
 
       - name: Checkout Open3D-ML
         uses: actions/checkout@v4
@@ -111,7 +111,7 @@ jobs:
         run: |
           if ( '${{ matrix.BUILD_CUDA_MODULE }}' -eq 'ON' ) {
             # python -m pip install -r open3d_ml/requirements-torch-cuda.txt
-            python -m pip install torch torchvision --index-url https://download.pytorch.org/whl/cu124
+            python -m pip install torch torchvision --index-url https://download.pytorch.org/whl/cu126
           } else {
             python -m pip install -r open3d_ml/requirements-torch.txt
           }

3rdparty/webrtc/Dockerfile.webrtc

@@ -13,7 +13,7 @@
 #   - docker run --rm --entrypoint cat open3d-webrtc:abi1 \
 #       webrtc_60e6748_cxx-abi-1.tar.gz > webrtc_60e6748_cxx-abi-1.tar.gz
 
-FROM ubuntu:20.04
+FROM ubuntu:22.04
 
 ARG SUDO=command
 COPY 3rdparty/webrtc 3rdparty/webrtc

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,14 @@ core::Tensor ComputePosePointToPlane(const core::Tensor &source_positions,
                                      const core::Tensor &correspondence_indices,
                                      const registration::RobustKernel &kernel);
 
+/// \brief Computes pose for symmetric ICP registration.
+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.