diff --git a/.github/workflows/ubuntu-wheel.yml b/.github/workflows/ubuntu-wheel.yml index 73ce681fd18..c333f565605 100644 --- a/.github/workflows/ubuntu-wheel.yml +++ b/.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 diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml index 765abfd21b8..b68e8814dc2 100644 --- a/.github/workflows/windows.yml +++ b/.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 } diff --git a/3rdparty/webrtc/Dockerfile.webrtc b/3rdparty/webrtc/Dockerfile.webrtc index b935d0e8ef7..8c127c97b6d 100644 --- a/3rdparty/webrtc/Dockerfile.webrtc +++ b/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 diff --git a/cpp/open3d/Open3D.h.in b/cpp/open3d/Open3D.h.in index 5c9a5b5c60a..f008efdbb2b 100644 --- a/cpp/open3d/Open3D.h.in +++ b/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" diff --git a/cpp/open3d/pipelines/CMakeLists.txt b/cpp/open3d/pipelines/CMakeLists.txt index 7173b64b515..59eb09024b5 100644 --- a/cpp/open3d/pipelines/CMakeLists.txt +++ b/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 diff --git a/cpp/open3d/pipelines/registration/SymmetricICP.cpp b/cpp/open3d/pipelines/registration/SymmetricICP.cpp new file mode 100644 index 00000000000..fc23205af6f --- /dev/null +++ b/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 &J_r, + std::vector &r, std::vector &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( + 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> +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 source_initialized_c( + &source, [](const geometry::PointCloud *) {}); + std::shared_ptr 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 diff --git a/cpp/open3d/pipelines/registration/SymmetricICP.h b/cpp/open3d/pipelines/registration/SymmetricICP.h new file mode 100644 index 00000000000..50a3c02ba7f --- /dev/null +++ b/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 kernel = std::make_shared()) + : 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> + 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 kernel_ = std::make_shared(); + +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 diff --git a/cpp/open3d/t/pipelines/kernel/Registration.cpp b/cpp/open3d/t/pipelines/kernel/Registration.cpp index 326671431f3..26b7b06db38 100644 --- a/cpp/open3d/t/pipelines/kernel/Registration.cpp +++ b/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, diff --git a/cpp/open3d/t/pipelines/kernel/Registration.h b/cpp/open3d/t/pipelines/kernel/Registration.h index 3358d30a9ac..727abb87923 100644 --- a/cpp/open3d/t/pipelines/kernel/Registration.h +++ b/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. diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp b/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp index bac5eaf8e3c..fbdd3ce9640 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp +++ b/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp @@ -128,6 +128,112 @@ void ComputePosePointToPlaneCPU(const core::Tensor &source_points, DecodeAndSolve6x6(global_sum, pose, residual, inlier_count); } +template +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 A_1x29(29, 0.0); +#ifdef _WIN32 + std::vector zeros_29(29, 0.0); + A_1x29 = tbb::parallel_reduce( + tbb::blocked_range(0, n), zeros_29, + [&](tbb::blocked_range r, std::vector A_reduction) { + for (int workload_idx = r.begin(); workload_idx < r.end(); + ++workload_idx) { +#else + scalar_t *A_reduction = A_1x29.data(); +#pragma omp parallel for reduction(+ : A_reduction[ : 29]) schedule(static) \ + num_threads(utility::EstimateMaxThreads()) + for (int workload_idx = 0; workload_idx < n; ++workload_idx) { +#endif + scalar_t J_ij[12] = {0}; + scalar_t r1 = 0, r2 = 0; + const bool valid = GetJacobianSymmetric( + workload_idx, source_points_ptr, target_points_ptr, + source_normals_ptr, target_normals_ptr, + correspondence_indices, J_ij, r1, r2); + + if (valid) { + const scalar_t w1 = GetWeightFromRobustKernel(r1); + const scalar_t w2 = GetWeightFromRobustKernel(r2); + + // Accumulate JtJ and Jtr for both terms + int i = 0; + for (int j = 0; j < 6; ++j) { + for (int k = 0; k <= j; ++k) { + // Contribution from first term (source to + // target) + A_reduction[i] += J_ij[j] * w1 * J_ij[k]; + // Contribution from second term (target to + // source) + A_reduction[i] += + J_ij[j + 6] * w2 * J_ij[k + 6]; + ++i; + } + // Jtr contributions + A_reduction[21 + j] += + J_ij[j] * w1 * r1 + J_ij[j + 6] * w2 * r2; + } + A_reduction[27] += r1 * r1 + r2 * r2; + A_reduction[28] += 1; + } + } +#ifdef _WIN32 + return A_reduction; + }, + [&](std::vector a, std::vector b) { + std::vector result(29); + for (int j = 0; j < 29; ++j) { + result[j] = a[j] + b[j]; + } + return result; + }); +#endif + + for (int i = 0; i < 29; ++i) { + global_sum[i] = A_1x29[i]; + } +} + +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) { + int n = source_points.GetLength(); + + core::Tensor global_sum = core::Tensor::Zeros({29}, dtype, device); + + DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() { + scalar_t *global_sum_ptr = global_sum.GetDataPtr(); + + DISPATCH_ROBUST_KERNEL_FUNCTION( + kernel.type_, scalar_t, kernel.scaling_parameter_, + kernel.shape_parameter_, [&]() { + ComputePoseSymmetricKernelCPU( + source_points.GetDataPtr(), + target_points.GetDataPtr(), + source_normals.GetDataPtr(), + target_normals.GetDataPtr(), + correspondence_indices.GetDataPtr(), n, + global_sum_ptr, GetWeightFromRobustKernel); + }); + }); + + DecodeAndSolve6x6(global_sum, pose, residual, inlier_count); +} + template static void ComputePoseColoredICPKernelCPU( const scalar_t *source_points_ptr, diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu index 1fe76815a2f..c1a022458c4 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu +++ b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu @@ -229,6 +229,105 @@ void ComputePoseColoredICPCUDA(const core::Tensor &source_points, DecodeAndSolve6x6(global_sum, pose, residual, inlier_count); } +template +__global__ void ComputePoseSymmetricKernelCUDA( + 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, + func_t GetWeightFromRobustKernel) { + typedef utility::MiniVec ReduceVec; + // Create shared memory. + typedef cub::BlockReduce BlockReduce; + __shared__ typename BlockReduce::TempStorage temp_storage; + ReduceVec local_sum(static_cast(0)); + + const int workload_idx = threadIdx.x + blockIdx.x * blockDim.x; + if (workload_idx < n) { + scalar_t J_ij[12] = {0}; // 6 for each term in symmetric ICP + scalar_t r1 = 0, r2 = 0; + const bool valid = GetJacobianSymmetric( + workload_idx, source_points_ptr, target_points_ptr, + source_normals_ptr, target_normals_ptr, correspondence_indices, + J_ij, r1, r2); + + if (valid) { + const scalar_t w1 = GetWeightFromRobustKernel(r1); + const scalar_t w2 = GetWeightFromRobustKernel(r2); + + // Accumulate JtJ and Jtr for both terms + int i = 0; + for (int j = 0; j < 6; ++j) { + for (int k = 0; k <= j; ++k) { + // Contribution from first term (source to target) + local_sum[i] += J_ij[j] * w1 * J_ij[k]; + // Contribution from second term (target to source) + local_sum[i] += J_ij[j + 6] * w2 * J_ij[k + 6]; + ++i; + } + // Jtr contributions + local_sum[21 + j] += J_ij[j] * w1 * r1 + J_ij[j + 6] * w2 * r2; + } + local_sum[27] += r1 * r1 + r2 * r2; + local_sum[28] += 1; + } + } + + // Reduction. + auto result = BlockReduce(temp_storage).Sum(local_sum); + + // Add result to global_sum. + if (threadIdx.x == 0) { +#pragma unroll + for (int i = 0; i < kReduceDim; ++i) { + atomicAdd(&global_sum[i], result[i]); + } + } +} + +void ComputePoseSymmetricCUDA(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) { + core::CUDAScopedDevice scoped_device(source_points.GetDevice()); + int n = source_points.GetLength(); + + core::Tensor global_sum = core::Tensor::Zeros({29}, dtype, device); + const dim3 blocks((n + kThread1DUnit - 1) / kThread1DUnit); + const dim3 threads(kThread1DUnit); + + DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() { + scalar_t *global_sum_ptr = global_sum.GetDataPtr(); + + DISPATCH_ROBUST_KERNEL_FUNCTION( + kernel.type_, scalar_t, kernel.scaling_parameter_, + kernel.shape_parameter_, [&]() { + ComputePoseSymmetricKernelCUDA<<>>( + source_points.GetDataPtr(), + target_points.GetDataPtr(), + source_normals.GetDataPtr(), + target_normals.GetDataPtr(), + correspondence_indices.GetDataPtr(), n, + global_sum_ptr, GetWeightFromRobustKernel); + }); + }); + + core::cuda::Synchronize(); + + DecodeAndSolve6x6(global_sum, pose, residual, inlier_count); +} + template __global__ void ComputePoseDopplerICPKernelCUDA( const scalar_t *source_points_ptr, diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h index 3f08ca4c811..94f4f6f4de5 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h +++ b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h @@ -37,6 +37,18 @@ void ComputePosePointToPlaneCPU(const core::Tensor &source_points, const core::Device &device, const registration::RobustKernel &kernel); +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); + void ComputePoseColoredICPCPU(const core::Tensor &source_points, const core::Tensor &source_colors, const core::Tensor &target_points, @@ -87,6 +99,18 @@ void ComputePosePointToPlaneCUDA(const core::Tensor &source_points, const core::Device &device, const registration::RobustKernel &kernel); +void ComputePoseSymmetricCUDA(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); + void ComputePoseColoredICPCUDA(const core::Tensor &source_points, const core::Tensor &source_colors, const core::Tensor &target_points, @@ -203,6 +227,82 @@ template bool GetJacobianPointToPlane(int64_t workload_idx, double *J_ij, double &r); +template +OPEN3D_HOST_DEVICE inline bool GetJacobianSymmetric( + int64_t workload_idx, + 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, + scalar_t *J_ij, + scalar_t &r1, + scalar_t &r2) { + if (correspondence_indices[workload_idx] == -1) { + return false; + } + + const int64_t target_idx = 3 * correspondence_indices[workload_idx]; + const int64_t source_idx = 3 * workload_idx; + + const scalar_t &sx = source_points_ptr[source_idx + 0]; + const scalar_t &sy = source_points_ptr[source_idx + 1]; + const scalar_t &sz = source_points_ptr[source_idx + 2]; + const scalar_t &tx = target_points_ptr[target_idx + 0]; + const scalar_t &ty = target_points_ptr[target_idx + 1]; + const scalar_t &tz = target_points_ptr[target_idx + 2]; + const scalar_t &nx_s = source_normals_ptr[source_idx + 0]; + const scalar_t &ny_s = source_normals_ptr[source_idx + 1]; + const scalar_t &nz_s = source_normals_ptr[source_idx + 2]; + const scalar_t &nx_t = target_normals_ptr[target_idx + 0]; + const scalar_t &ny_t = target_normals_ptr[target_idx + 1]; + const scalar_t &nz_t = target_normals_ptr[target_idx + 2]; + + // Symmetric ICP: minimize both source-to-target and target-to-source + // distances + r1 = (sx - tx) * nx_t + (sy - ty) * ny_t + (sz - tz) * nz_t; + r2 = (sx - tx) * nx_s + (sy - ty) * ny_s + (sz - tz) * nz_s; + + // For symmetric ICP, we compute Jacobians for both terms + // First term (source to target plane) + J_ij[0] = nz_t * sy - ny_t * sz; + J_ij[1] = nx_t * sz - nz_t * sx; + J_ij[2] = ny_t * sx - nx_t * sy; + J_ij[3] = nx_t; + J_ij[4] = ny_t; + J_ij[5] = nz_t; + + // Second term (target to source plane) + J_ij[6] = nz_s * sy - ny_s * sz; + J_ij[7] = nx_s * sz - nz_s * sx; + J_ij[8] = ny_s * sx - nx_s * sy; + J_ij[9] = nx_s; + J_ij[10] = ny_s; + J_ij[11] = nz_s; + + return true; +} + +template bool GetJacobianSymmetric(int64_t workload_idx, + const float *source_points_ptr, + const float *target_points_ptr, + const float *source_normals_ptr, + const float *target_normals_ptr, + const int64_t *correspondence_indices, + float *J_ij, + float &r1, + float &r2); + +template bool GetJacobianSymmetric(int64_t workload_idx, + const double *source_points_ptr, + const double *target_points_ptr, + const double *source_normals_ptr, + const double *target_normals_ptr, + const int64_t *correspondence_indices, + double *J_ij, + double &r1, + double &r2); + template OPEN3D_HOST_DEVICE inline bool GetJacobianColoredICP( const int64_t workload_idx, diff --git a/cpp/open3d/t/pipelines/registration/Registration.cpp b/cpp/open3d/t/pipelines/registration/Registration.cpp index 87b9d181ea7..95eef44c353 100644 --- a/cpp/open3d/t/pipelines/registration/Registration.cpp +++ b/cpp/open3d/t/pipelines/registration/Registration.cpp @@ -105,6 +105,17 @@ ICP(const geometry::PointCloud &source, estimation, callback_after_iteration); } +RegistrationResult SymmetricICP( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + double max_correspondence_distance, + const core::Tensor &init_source_to_target, + const TransformationEstimationSymmetric &estimation, + const ICPConvergenceCriteria &criteria) { + return ICP(source, target, max_correspondence_distance, + init_source_to_target, estimation, criteria); +} + static void AssertInputMultiScaleICP( const geometry::PointCloud &source, const geometry::PointCloud &target, diff --git a/cpp/open3d/t/pipelines/registration/Registration.h b/cpp/open3d/t/pipelines/registration/Registration.h index ed933b60132..e4f54e04a83 100644 --- a/cpp/open3d/t/pipelines/registration/Registration.h +++ b/cpp/open3d/t/pipelines/registration/Registration.h @@ -143,6 +143,17 @@ ICP(const geometry::PointCloud &source, const std::function &)> &callback_after_iteration = nullptr); +/// \brief Wrapper for symmetric ICP registration using symmetric objective. +RegistrationResult SymmetricICP( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + double max_correspondence_distance, + const core::Tensor &init_source_to_target = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + const TransformationEstimationSymmetric &estimation = + TransformationEstimationSymmetric(), + const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria()); + /// \brief Functions for Multi-Scale ICP registration. /// It will run ICP on different voxel level, from coarse to dense. /// The vector of ICPConvergenceCriteria(relative fitness, relative rmse, diff --git a/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp b/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp index 922cc31ef70..9580c611767 100644 --- a/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp +++ b/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp @@ -10,6 +10,7 @@ #include "open3d/core/TensorCheck.h" #include "open3d/t/pipelines/kernel/Registration.h" #include "open3d/t/pipelines/kernel/TransformationConverter.h" +#include "open3d/t/pipelines/registration/RobustKernelImpl.h" namespace open3d { namespace t { @@ -164,6 +165,105 @@ core::Tensor TransformationEstimationPointToPlane::ComputeTransformation( return pipelines::kernel::PoseToTransformation(pose); } +double TransformationEstimationSymmetric::ComputeRMSE( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences) const { + if (!target.HasPointPositions() || !source.HasPointPositions()) { + utility::LogError("Source and/or Target pointcloud is empty."); + } + if (!target.HasPointNormals() || !source.HasPointNormals()) { + utility::LogError( + "SymmetricICP requires both source and target to have " + "normals."); + } + + core::AssertTensorDtype(target.GetPointPositions(), + source.GetPointPositions().GetDtype()); + core::AssertTensorDevice(target.GetPointPositions(), source.GetDevice()); + + AssertValidCorrespondences(correspondences, source.GetPointPositions()); + + core::Tensor valid = correspondences.Ne(-1).Reshape({-1}); + core::Tensor neighbour_indices = + correspondences.IndexGet({valid}).Reshape({-1}); + + // Check if there are any valid correspondences + if (neighbour_indices.GetLength() == 0) { + return 0.0; + } + + core::Tensor source_points_indexed = + source.GetPointPositions().IndexGet({valid}); + core::Tensor target_points_indexed = + target.GetPointPositions().IndexGet({neighbour_indices}); + core::Tensor source_normals_indexed = + source.GetPointNormals().IndexGet({valid}); + core::Tensor target_normals_indexed = + target.GetPointNormals().IndexGet({neighbour_indices}); + + // Compute residuals for both point-to-plane terms + 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(); + return std::sqrt(error / + static_cast(neighbour_indices.GetLength())); +} + +core::Tensor TransformationEstimationSymmetric::ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const { + if (!target.HasPointPositions() || !source.HasPointPositions()) { + utility::LogError("Source and/or Target pointcloud is empty."); + } + if (!target.HasPointNormals() || !source.HasPointNormals()) { + utility::LogError( + "SymmetricICP requires both source and target to have " + "normals."); + } + + const core::Device device = source.GetPointPositions().GetDevice(); + core::AssertTensorDevice(target.GetPointPositions(), device); + + const core::Dtype dtype = source.GetPointPositions().GetDtype(); + core::AssertTensorDtypes(source.GetPointPositions(), + {core::Float64, core::Float32}); + core::AssertTensorDtype(target.GetPointPositions(), + source.GetPointPositions().GetDtype()); + core::AssertTensorDtype(target.GetPointNormals(), + source.GetPointPositions().GetDtype()); + core::AssertTensorDtype(source.GetPointNormals(), + source.GetPointPositions().GetDtype()); + + AssertValidCorrespondences(correspondences, source.GetPointPositions()); + + // Check if there are any valid correspondences + core::Tensor valid = correspondences.Ne(-1); + if (valid.Any().Item() == false) { + // Return identity transformation if no valid correspondences + return core::Tensor::Eye(4, dtype, device); + } + + // Get pose {6} of type Float64. + core::Tensor pose = pipelines::kernel::ComputePoseSymmetric( + source.GetPointPositions(), target.GetPointPositions(), + source.GetPointNormals(), target.GetPointNormals(), correspondences, + this->kernel_); + + // Get rigid transformation tensor of {4, 4} of type Float64 on CPU:0 + // device, from pose {6}. + (void)current_transform; + (void)iteration; + return pipelines::kernel::PoseToTransformation(pose); +} + double TransformationEstimationForColoredICP::ComputeRMSE( const geometry::PointCloud &source, const geometry::PointCloud &target, diff --git a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h index 73227014b22..347afe0e0b7 100644 --- a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h +++ b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h @@ -215,6 +215,40 @@ class TransformationEstimationPointToPlane : public TransformationEstimation { TransformationEstimationType::PointToPlane; }; +/// \class TransformationEstimationSymmetric +/// +/// Class to estimate a transformation matrix tensor of shape {4, 4}, dtype +/// Float64, on CPU or CUDA device for symmetric point-to-plane distance. +class TransformationEstimationSymmetric : public TransformationEstimation { +public: + explicit TransformationEstimationSymmetric( + const RobustKernel &kernel = + RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0)) + : kernel_(kernel) {} + ~TransformationEstimationSymmetric() override {} + +public: + TransformationEstimationType GetTransformationEstimationType() + const override { + return TransformationEstimationType::PointToPlane; + }; + + double ComputeRMSE(const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences) const override; + + core::Tensor ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences, + const core::Tensor ¤t_transform = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + const std::size_t iteration = 0) const override; + +public: + RobustKernel kernel_ = RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0); +}; + /// \class TransformationEstimationForColoredICP /// /// This is implementation of following paper diff --git a/cpp/pybind/pipelines/registration/registration.cpp b/cpp/pybind/pipelines/registration/registration.cpp index 0cc560df72f..113286c5db5 100644 --- a/cpp/pybind/pipelines/registration/registration.cpp +++ b/cpp/pybind/pipelines/registration/registration.cpp @@ -17,6 +17,7 @@ #include "open3d/pipelines/registration/Feature.h" #include "open3d/pipelines/registration/GeneralizedICP.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" @@ -105,6 +106,12 @@ void pybind_registration_declarations(py::module &m) { te_p2l(m_registration, "TransformationEstimationPointToPlane", "Class to estimate a transformation for point to plane " "distance."); + py::class_, + TransformationEstimation> + te_sym(m_registration, "TransformationEstimationSymmetric", + "Class to estimate a transformation for symmetric " + "point to plane distance."); py::class_< TransformationEstimationForColoredICP, PyTransformationEstimation, @@ -307,6 +314,27 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``. &TransformationEstimationPointToPlane::kernel_, "Robust Kernel used in the Optimization"); + auto te_sym = static_cast, + TransformationEstimation>>( + m_registration.attr("TransformationEstimationSymmetric")); + py::detail::bind_default_constructor( + te_sym); + py::detail::bind_copy_functions(te_sym); + te_sym.def(py::init([](std::shared_ptr kernel) { + return new TransformationEstimationSymmetric( + std::move(kernel)); + }), + "kernel"_a) + .def("__repr__", + [](const TransformationEstimationSymmetric &te) { + return std::string("TransformationEstimationSymmetric"); + }) + .def_readwrite("kernel", + &TransformationEstimationSymmetric::kernel_, + "Robust Kernel used in the Optimization"); + // open3d.registration.TransformationEstimationForColoredICP : auto te_col = static_cast(), + "Function for symmetric ICP registration", "source"_a, "target"_a, + "max_correspondence_distance"_a, + "init"_a = Eigen::Matrix4d::Identity(), + "estimation_method"_a = TransformationEstimationSymmetric(), + "criteria"_a = ICPConvergenceCriteria()); + docstring::FunctionDocInject(m_registration, "registration_symmetric_icp", + map_shared_argument_docstrings); + m_registration.def("registration_colored_icp", &RegistrationColoredICP, py::call_guard(), "Function for Colored ICP registration", "source"_a, diff --git a/cpp/pybind/t/pipelines/registration/registration.cpp b/cpp/pybind/t/pipelines/registration/registration.cpp index 0d3e54eddaf..c15ec607d20 100644 --- a/cpp/pybind/t/pipelines/registration/registration.cpp +++ b/cpp/pybind/t/pipelines/registration/registration.cpp @@ -120,6 +120,12 @@ void pybind_registration_declarations(py::module &m) { te_p2l(m_registration, "TransformationEstimationPointToPlane", "Class to estimate a transformation for point to " "plane distance."); + py::class_, + TransformationEstimation> + te_sym(m_registration, "TransformationEstimationSymmetric", + "Class to estimate a transformation for symmetric " + "point to plane distance."); py::class_< TransformationEstimationForColoredICP, PyTransformationEstimation, @@ -291,6 +297,28 @@ void pybind_registration_definitions(py::module &m) { &TransformationEstimationPointToPlane::kernel_, "Robust Kernel used in the Optimization"); + // open3d.t.pipelines.registration.TransformationEstimationSymmetric + // TransformationEstimation + auto te_sym = static_cast, + TransformationEstimation>>( + m_registration.attr("TransformationEstimationSymmetric")); + py::detail::bind_default_constructor( + te_sym); + py::detail::bind_copy_functions(te_sym); + te_sym.def(py::init([](const RobustKernel &kernel) { + return new TransformationEstimationSymmetric(kernel); + }), + "kernel"_a) + .def("__repr__", + [](const TransformationEstimationSymmetric &te) { + return std::string("TransformationEstimationSymmetric"); + }) + .def_readwrite("kernel", + &TransformationEstimationSymmetric::kernel_, + "Robust Kernel used in the Optimization"); + // open3d.t.pipelines.registration.TransformationEstimationForColoredICP // TransformationEstimation auto te_col = static_cast(), + "Function for symmetric ICP registration", "source"_a, "target"_a, + "max_correspondence_distance"_a, + "init_source_to_target"_a = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + "estimation_method"_a = TransformationEstimationSymmetric(), + "criteria"_a = ICPConvergenceCriteria()); + docstring::FunctionDocInject(m_registration, "registration_symmetric_icp", + map_shared_argument_docstrings); + m_registration.def( "multi_scale_icp", &MultiScaleICP, py::call_guard(), diff --git a/cpp/tests/pipelines/CMakeLists.txt b/cpp/tests/pipelines/CMakeLists.txt index 486759c8d12..d38aecfdf5d 100644 --- a/cpp/tests/pipelines/CMakeLists.txt +++ b/cpp/tests/pipelines/CMakeLists.txt @@ -19,5 +19,6 @@ target_sources(tests PRIVATE registration/GlobalOptimizationConvergenceCriteria.cpp registration/PoseGraph.cpp registration/Registration.cpp + registration/SymmetricICP.cpp registration/TransformationEstimation.cpp ) diff --git a/cpp/tests/pipelines/registration/SymmetricICP.cpp b/cpp/tests/pipelines/registration/SymmetricICP.cpp new file mode 100644 index 00000000000..5c8307022a2 --- /dev/null +++ b/cpp/tests/pipelines/registration/SymmetricICP.cpp @@ -0,0 +1,187 @@ +// ---------------------------------------------------------------------------- +// - 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/pipelines/registration/Registration.h" +#include "open3d/utility/Eigen.h" +#include "open3d/utility/Random.h" +#include "tests/Tests.h" + +namespace open3d { +namespace tests { + +using namespace open3d::pipelines; + +TEST(SymmetricICP, TransformationEstimationSymmetricConstructor) { + registration::TransformationEstimationSymmetric estimation; + EXPECT_EQ(estimation.GetTransformationEstimationType(), + registration::TransformationEstimationType::PointToPlane); +} + +TEST(SymmetricICP, TransformationEstimationSymmetricComputeRMSE) { + geometry::PointCloud source; + geometry::PointCloud target; + + source.points_ = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}}; + source.normals_ = {{0, 0, 1}, {0, 0, 1}, {0, 0, 1}}; + + target.points_ = {{0.1, 0.1, 0.1}, {1.1, 0.1, 0.1}, {0.1, 1.1, 0.1}}; + target.normals_ = {{0, 0, 1}, {0, 0, 1}, {0, 0, 1}}; + + registration::CorrespondenceSet corres = {{0, 0}, {1, 1}, {2, 2}}; + registration::TransformationEstimationSymmetric estimation; + + double rmse = estimation.ComputeRMSE(source, target, corres); + EXPECT_GT(rmse, 0.0); +} + +TEST(SymmetricICP, TransformationEstimationSymmetricComputeRMSEEmptyCorres) { + geometry::PointCloud source; + geometry::PointCloud target; + registration::CorrespondenceSet corres; + registration::TransformationEstimationSymmetric estimation; + + double rmse = estimation.ComputeRMSE(source, target, corres); + EXPECT_EQ(rmse, 0.0); +} + +TEST(SymmetricICP, TransformationEstimationSymmetricComputeRMSENoNormals) { + geometry::PointCloud source; + geometry::PointCloud target; + + source.points_ = {{0, 0, 0}, {1, 0, 0}}; + target.points_ = {{0.1, 0.1, 0.1}, {1.1, 0.1, 0.1}}; + + registration::CorrespondenceSet corres = {{0, 0}, {1, 1}}; + registration::TransformationEstimationSymmetric estimation; + + double rmse = estimation.ComputeRMSE(source, target, corres); + EXPECT_EQ(rmse, 0.0); +} + +TEST(SymmetricICP, TransformationEstimationSymmetricComputeTransformation) { + geometry::PointCloud source; + geometry::PointCloud target; + + // Create test point clouds with normals + source.points_ = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {1, 1, 0}}; + source.normals_ = {{0, 0, 1}, {0, 0, 1}, {0, 0, 1}, {0, 0, 1}}; + + target.points_ = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {1, 1, 0}}; + target.normals_ = {{0, 0, 1}, {0, 0, 1}, {0, 0, 1}, {0, 0, 1}}; + + registration::CorrespondenceSet corres = {{0, 0}, {1, 1}, {2, 2}, {3, 3}}; + registration::TransformationEstimationSymmetric estimation; + + Eigen::Matrix4d transformation = + estimation.ComputeTransformation(source, target, corres); + + // Should be close to identity for perfect correspondence + EXPECT_TRUE(transformation.isApprox(Eigen::Matrix4d::Identity(), 1e-3)); +} + +TEST(SymmetricICP, + TransformationEstimationSymmetricComputeTransformationEmptyCorres) { + geometry::PointCloud source; + geometry::PointCloud target; + registration::CorrespondenceSet corres; + registration::TransformationEstimationSymmetric estimation; + + Eigen::Matrix4d transformation = + estimation.ComputeTransformation(source, target, corres); + + EXPECT_TRUE(transformation.isApprox(Eigen::Matrix4d::Identity())); +} + +TEST(SymmetricICP, + TransformationEstimationSymmetricComputeTransformationNoNormals) { + geometry::PointCloud source; + geometry::PointCloud target; + + source.points_ = {{0, 0, 0}, {1, 0, 0}}; + target.points_ = {{0.1, 0.1, 0.1}, {1.1, 0.1, 0.1}}; + // No normals + + registration::CorrespondenceSet corres = {{0, 0}, {1, 1}}; + registration::TransformationEstimationSymmetric estimation; + + Eigen::Matrix4d transformation = + estimation.ComputeTransformation(source, target, corres); + + EXPECT_TRUE(transformation.isApprox(Eigen::Matrix4d::Identity())); +} + +TEST(SymmetricICP, RegistrationSymmetricICP) { + geometry::PointCloud source; + geometry::PointCloud target; + + // Create test point clouds with normals + source.points_ = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}}; + 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.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); + + EXPECT_GT(result.correspondence_set_.size(), 0); + EXPECT_GT(result.fitness_, 0.0); + EXPECT_GE(result.inlier_rmse_, 0.0); +} + +TEST(SymmetricICP, RegistrationSymmetricICPConvergence) { + utility::random::Seed(42); + + // Create a more complex test case + geometry::PointCloud source; + geometry::PointCloud target; + + // Generate random points with normals + const int num_points = 50; + utility::random::UniformRealGenerator rand_gen(0.0, 10.0); + for (int i = 0; i < num_points; ++i) { + double x = rand_gen(); + double y = rand_gen(); + double z = rand_gen(); + + source.points_.push_back({x, y, z}); + 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 + + target = source; + target.Transform(true_transformation); + + registration::TransformationEstimationSymmetric estimation; + registration::ICPConvergenceCriteria criteria(1e-6, 1e-6, 30); + + registration::RegistrationResult result = + registration::RegistrationSymmetricICP(source, target, 0.5, + Eigen::Matrix4d::Identity(), + estimation, criteria); + + // Check that registration converged to reasonable result + EXPECT_GT(result.fitness_, 0.5); + EXPECT_LT(result.inlier_rmse_, 1.0); +} + +} // namespace tests +} // namespace open3d diff --git a/cpp/tests/t/pipelines/registration/TransformationEstimation.cpp b/cpp/tests/t/pipelines/registration/TransformationEstimation.cpp index 2daca846d0a..74efdbba8c4 100644 --- a/cpp/tests/t/pipelines/registration/TransformationEstimation.cpp +++ b/cpp/tests/t/pipelines/registration/TransformationEstimation.cpp @@ -171,5 +171,276 @@ TEST_P(TransformationEstimationPermuteDevices, } } +TEST_P(TransformationEstimationPermuteDevices, ComputeRMSESymmetric) { + core::Device device = GetParam(); + + for (auto dtype : {core::Float32, core::Float64}) { + t::geometry::PointCloud source_pcd(device), target_pcd(device); + core::Tensor corres; + std::tie(source_pcd, target_pcd, corres) = + GetTestPointCloudsAndCorrespondences(dtype, device); + + // Add normals to source point cloud (required for symmetric ICP) + core::Tensor source_normals = + core::Tensor::Init({{-0.1, -0.2, -0.8}, + {0.2, -0.1, -0.7}, + {0.05, -0.4, -0.6}, + {-0.01, -0.3, -0.5}, + {0.3, -0.1, -0.7}, + {0.06, -0.4, -0.4}, + {0.2, 0.2, -0.8}, + {0.4, 0.1, -0.5}, + {0.1, 0.1, -0.7}, + {0.2, 0.3, -0.9}, + {0.3, -0.2, -0.2}, + {0.1, 0.1, -0.6}, + {0.05, -0.4, -0.4}, + {0.1, 0.2, -0.7}}, + device); + source_pcd.SetPointNormals(source_normals.To(device, dtype)); + + t::pipelines::registration::TransformationEstimationSymmetric + estimation_symmetric; + double symmetric_rmse = estimation_symmetric.ComputeRMSE( + source_pcd, target_pcd, corres); + + // Symmetric RMSE should be positive and finite + EXPECT_GT(symmetric_rmse, 0.0); + EXPECT_TRUE(std::isfinite(symmetric_rmse)); + } +} + +TEST_P(TransformationEstimationPermuteDevices, ComputeTransformationSymmetric) { + core::Device device = GetParam(); + + for (auto dtype : {core::Float32, core::Float64}) { + t::geometry::PointCloud source_pcd(device), target_pcd(device); + core::Tensor corres; + std::tie(source_pcd, target_pcd, corres) = + GetTestPointCloudsAndCorrespondences(dtype, device); + + // Add normals to source point cloud (required for symmetric ICP) + core::Tensor source_normals = + core::Tensor::Init({{-0.1, -0.2, -0.8}, + {0.2, -0.1, -0.7}, + {0.05, -0.4, -0.6}, + {-0.01, -0.3, -0.5}, + {0.3, -0.1, -0.7}, + {0.06, -0.4, -0.4}, + {0.2, 0.2, -0.8}, + {0.4, 0.1, -0.5}, + {0.1, 0.1, -0.7}, + {0.2, 0.3, -0.9}, + {0.3, -0.2, -0.2}, + {0.1, 0.1, -0.6}, + {0.05, -0.4, -0.4}, + {0.1, 0.2, -0.7}}, + device); + source_pcd.SetPointNormals(source_normals.To(device, dtype)); + + t::pipelines::registration::TransformationEstimationSymmetric + estimation_symmetric; + + // Compute initial RMSE + double initial_rmse = estimation_symmetric.ComputeRMSE( + source_pcd, target_pcd, corres); + (void)initial_rmse; // Suppress unused variable warning + + // Get transform + core::Tensor symmetric_transform = + estimation_symmetric.ComputeTransformation(source_pcd, + target_pcd, corres); + + // Verify transformation is 4x4 matrix + EXPECT_EQ(symmetric_transform.GetShape(), core::SizeVector({4, 4})); + EXPECT_EQ(symmetric_transform.GetDtype(), core::Float64); + + // Apply transform + t::geometry::PointCloud source_transformed_symmetric = + source_pcd.Clone(); + source_transformed_symmetric.Transform(symmetric_transform); + double final_rmse = estimation_symmetric.ComputeRMSE( + source_transformed_symmetric, target_pcd, corres); + + // Final RMSE should be finite and potentially lower than initial + EXPECT_TRUE(std::isfinite(final_rmse)); + EXPECT_GE(final_rmse, 0.0); + } +} + +TEST_P(TransformationEstimationPermuteDevices, SymmetricICPDeviceConsistency) { + core::Device device = GetParam(); + + for (auto dtype : {core::Float32, core::Float64}) { + // Create simple test data for device consistency testing + t::geometry::PointCloud source_pcd(device), target_pcd(device); + core::Tensor corres; + std::tie(source_pcd, target_pcd, corres) = + GetTestPointCloudsAndCorrespondences(dtype, device); + + // Add normals to source + core::Tensor source_normals = + core::Tensor::Init({{-0.1, -0.2, -0.8}, + {0.2, -0.1, -0.7}, + {0.05, -0.4, -0.6}, + {-0.01, -0.3, -0.5}, + {0.3, -0.1, -0.7}, + {0.06, -0.4, -0.4}, + {0.2, 0.2, -0.8}, + {0.4, 0.1, -0.5}, + {0.1, 0.1, -0.7}, + {0.2, 0.3, -0.9}, + {0.3, -0.2, -0.2}, + {0.1, 0.1, -0.6}, + {0.05, -0.4, -0.4}, + {0.1, 0.2, -0.7}}, + device); + source_pcd.SetPointNormals(source_normals.To(device, dtype)); + + t::pipelines::registration::TransformationEstimationSymmetric + estimation; + + // Test RMSE computation + double rmse = estimation.ComputeRMSE(source_pcd, target_pcd, corres); + EXPECT_GT(rmse, 0.0); + EXPECT_TRUE(std::isfinite(rmse)); + + // Test transformation computation + core::Tensor transform = estimation.ComputeTransformation( + source_pcd, target_pcd, corres); + EXPECT_EQ(transform.GetShape(), core::SizeVector({4, 4})); + EXPECT_EQ(transform.GetDevice(), device); + EXPECT_TRUE(transform.AllClose(transform)); // Check for NaN/Inf + } +} + +TEST_P(TransformationEstimationPermuteDevices, + SymmetricICPCPUvsGPUConsistency) { + core::Device device = GetParam(); + + // Skip CUDA consistency test if device is CPU + if (device.GetType() == core::Device::DeviceType::CPU) { + GTEST_SKIP() << "Skipping CPU vs GPU consistency test for CPU device"; + } + +#ifdef BUILD_CUDA_MODULE + for (auto dtype : {core::Float32, core::Float64}) { + // Create identical test data for both CPU and GPU + auto [source_cpu, target_cpu, corres_cpu] = + GetTestPointCloudsAndCorrespondences(dtype, + core::Device("CPU:0")); + + // Add normals to source + core::Tensor source_normals_cpu = + core::Tensor::Init({{-0.1, -0.2, -0.8}, + {0.2, -0.1, -0.7}, + {0.05, -0.4, -0.6}, + {-0.01, -0.3, -0.5}, + {0.3, -0.1, -0.7}, + {0.06, -0.4, -0.4}, + {0.2, 0.2, -0.8}, + {0.4, 0.1, -0.5}, + {0.1, 0.1, -0.7}, + {0.2, 0.3, -0.9}, + {0.3, -0.2, -0.2}, + {0.1, 0.1, -0.6}, + {0.05, -0.4, -0.4}, + {0.1, 0.2, -0.7}}, + core::Device("CPU:0")); + source_cpu.SetPointNormals( + source_normals_cpu.To(core::Device("CPU:0"), dtype)); + + // Copy to GPU + auto source_gpu = source_cpu.To(device); + auto target_gpu = target_cpu.To(device); + auto corres_gpu = corres_cpu.To(device); + + // Test on both devices + t::pipelines::registration::TransformationEstimationSymmetric + estimation; + + // Compute on CPU + double rmse_cpu = + estimation.ComputeRMSE(source_cpu, target_cpu, corres_cpu); + core::Tensor transform_cpu = estimation.ComputeTransformation( + source_cpu, target_cpu, corres_cpu); + + // Compute on GPU + double rmse_gpu = + estimation.ComputeRMSE(source_gpu, target_gpu, corres_gpu); + core::Tensor transform_gpu = estimation.ComputeTransformation( + source_gpu, target_gpu, corres_gpu); + + // Compare results - they should be very close + EXPECT_NEAR(rmse_cpu, rmse_gpu, 1e-6); + EXPECT_TRUE(transform_cpu.AllClose( + transform_gpu.To(core::Device("CPU:0")), 1e-6, 1e-6)); + + utility::LogInfo("CPU RMSE: {}, GPU RMSE: {}", rmse_cpu, rmse_gpu); + } +#else + GTEST_SKIP() << "CUDA not available, skipping CPU vs GPU consistency test"; +#endif +} + +TEST_P(TransformationEstimationPermuteDevices, SymmetricICPRobustKernels) { + core::Device device = GetParam(); + + for (auto dtype : {core::Float32, core::Float64}) { + t::geometry::PointCloud source_pcd(device), target_pcd(device); + core::Tensor corres; + std::tie(source_pcd, target_pcd, corres) = + GetTestPointCloudsAndCorrespondences(dtype, device); + + // Add normals to source + core::Tensor source_normals = + core::Tensor::Init({{-0.1, -0.2, -0.8}, + {0.2, -0.1, -0.7}, + {0.05, -0.4, -0.6}, + {-0.01, -0.3, -0.5}, + {0.3, -0.1, -0.7}, + {0.06, -0.4, -0.4}, + {0.2, 0.2, -0.8}, + {0.4, 0.1, -0.5}, + {0.1, 0.1, -0.7}, + {0.2, 0.3, -0.9}, + {0.3, -0.2, -0.2}, + {0.1, 0.1, -0.6}, + {0.05, -0.4, -0.4}, + {0.1, 0.2, -0.7}}, + device); + source_pcd.SetPointNormals(source_normals.To(device, dtype)); + + // Test different robust kernels + std::vector kernels = { + t::pipelines::registration::RobustKernel( + t::pipelines::registration::RobustKernelMethod::L2Loss, + 1.0, 1.0), + t::pipelines::registration::RobustKernel( + t::pipelines::registration::RobustKernelMethod::L1Loss, + 1.0, 1.0), + t::pipelines::registration::RobustKernel( + t::pipelines::registration::RobustKernelMethod:: + HuberLoss, + 1.0, 1.0)}; + + for (const auto& kernel : kernels) { + t::pipelines::registration::TransformationEstimationSymmetric + estimation(kernel); + + double rmse = + estimation.ComputeRMSE(source_pcd, target_pcd, corres); + core::Tensor transform = estimation.ComputeTransformation( + source_pcd, target_pcd, corres); + + // All kernels should produce valid results + EXPECT_GT(rmse, 0.0); + EXPECT_TRUE(std::isfinite(rmse)); + EXPECT_EQ(transform.GetShape(), core::SizeVector({4, 4})); + EXPECT_TRUE(transform.AllClose(transform)); // Check for NaN/Inf + } + } +} + } // namespace tests } // namespace open3d diff --git a/docker/Dockerfile.ci b/docker/Dockerfile.ci index 18432c8cbc6..cbad785399e 100755 --- a/docker/Dockerfile.ci +++ b/docker/Dockerfile.ci @@ -165,7 +165,7 @@ RUN source util/ci_utils.sh \ RUN mkdir -p /etc/apt/keyrings \ && curl -fsSL https://deb.nodesource.com/gpgkey/nodesource-repo.gpg.key \ | gpg --dearmor -o /etc/apt/keyrings/nodesource.gpg \ - && echo "deb [signed-by=/etc/apt/keyrings/nodesource.gpg] https://deb.nodesource.com/node_16.x nodistro main" \ + && echo "deb [signed-by=/etc/apt/keyrings/nodesource.gpg] https://deb.nodesource.com/node_18.x nodistro main" \ | tee /etc/apt/sources.list.d/nodesource.list \ && apt-get update \ && apt-get install -y nodejs \ diff --git a/docker/Dockerfile.wheel b/docker/Dockerfile.wheel index e685faaf79f..08a3ae5a583 100644 --- a/docker/Dockerfile.wheel +++ b/docker/Dockerfile.wheel @@ -1,5 +1,5 @@ # FROM must be called before other ARGS except for ARG BASE_IMAGE -ARG BASE_IMAGE=nvidia/cuda:12.1.0-cudnn8-devel-ubuntu20.04 +ARG BASE_IMAGE=nvidia/cuda:12.6.3-cudnn-devel-ubuntu22.04 FROM ${BASE_IMAGE} # Customizable build arguments from cuda.yml @@ -119,7 +119,7 @@ RUN source /root/Open3D/util/ci_utils.sh \ && install_python_dependencies with-jupyter # Open3D Jupyter dependencies -RUN curl -fsSL https://deb.nodesource.com/setup_16.x | bash - \ +RUN curl -fsSL https://deb.nodesource.com/setup_18.x | bash - \ && apt-get install -y nodejs \ && rm -rf /var/lib/apt/lists/* \ && node --version diff --git a/python/test/t/registration/test_registration.py b/python/test/t/registration/test_registration.py index 4cbea101c7f..78ec3551bce 100644 --- a/python/test/t/registration/test_registration.py +++ b/python/test/t/registration/test_registration.py @@ -202,6 +202,48 @@ def test_icp_point_to_plane(device): reg_p2plane_legacy.fitness, 0.001) +@pytest.mark.parametrize("device", list_devices()) +def test_icp_symmetric(device): + + supported_dtypes = [o3c.float32, o3c.float64] + for dtype in supported_dtypes: + source_t, target_t = get_pcds(dtype, device) + + # Symmetric ICP requires normals for both source and target + source_t.estimate_normals() + target_t.estimate_normals() + + source_legacy = source_t.to_legacy() + target_legacy = target_t.to_legacy() + + max_correspondence_distance = 3.0 + + init_trans_legacy = np.array([[0.862, 0.011, -0.507, 0.5], + [-0.139, 0.967, -0.215, 0.7], + [0.487, 0.255, 0.835, -1.4], + [0.0, 0.0, 0.0, 1.0]]) + init_trans_t = o3c.Tensor(init_trans_legacy, + dtype=o3c.float64, + device=device) + + reg_sym_t = o3d.t.pipelines.registration.registration_symmetric_icp( + source_t, target_t, max_correspondence_distance, init_trans_t, + o3d.t.pipelines.registration.TransformationEstimationSymmetric(), + o3d.t.pipelines.registration.ICPConvergenceCriteria( + max_iteration=2)) + + reg_sym_legacy = o3d.pipelines.registration.registration_symmetric_icp( + source_legacy, target_legacy, max_correspondence_distance, + init_trans_legacy, + o3d.pipelines.registration.TransformationEstimationSymmetric(), + o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=2)) + + np.testing.assert_allclose(reg_sym_t.inlier_rmse, + reg_sym_legacy.inlier_rmse, 0.001) + np.testing.assert_allclose(reg_sym_t.fitness, reg_sym_legacy.fitness, + 0.001) + + @pytest.mark.parametrize("device", list_devices()) def test_get_information_matrix(device): diff --git a/python/test/test_symmetric_icp.py b/python/test/test_symmetric_icp.py new file mode 100644 index 00000000000..a1fc97f93e7 --- /dev/null +++ b/python/test/test_symmetric_icp.py @@ -0,0 +1,281 @@ +# ---------------------------------------------------------------------------- +# - Open3D: www.open3d.org - +# ---------------------------------------------------------------------------- +# Copyright (c) 2018-2024 www.open3d.org +# SPDX-License-Identifier: MIT +# ---------------------------------------------------------------------------- + +import open3d as o3d +import numpy as np + + +class TestSymmetricICP: + + def test_transformation_estimation_symmetric_constructor(self): + """Test TransformationEstimationSymmetric constructor.""" + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + assert estimation is not None + + def test_transformation_estimation_symmetric_with_kernel(self): + """Test TransformationEstimationSymmetric with robust kernel.""" + kernel = o3d.pipelines.registration.HuberLoss(0.1) + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + kernel) + assert estimation is not None + assert estimation.kernel is not None + + def test_transformation_estimation_symmetric_compute_rmse(self): + """Test compute_rmse method.""" + # Create simple test point clouds with normals + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + + source.points = o3d.utility.Vector3dVector([[0, 0, 0], [1, 0, 0], + [0, 1, 0]]) + source.normals = o3d.utility.Vector3dVector([[0, 0, 1], [0, 0, 1], + [0, 0, 1]]) + + target.points = o3d.utility.Vector3dVector([[0.1, 0.1, 0.1], + [1.1, 0.1, 0.1], + [0.1, 1.1, 0.1]]) + target.normals = o3d.utility.Vector3dVector([[0, 0, 1], [0, 0, 1], + [0, 0, 1]]) + + corres = o3d.utility.Vector2iVector([[0, 0], [1, 1], [2, 2]]) + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + + rmse = estimation.compute_rmse(source, target, corres) + assert rmse > 0.0 + + def test_transformation_estimation_symmetric_compute_rmse_empty_corres( + self): + """Test compute_rmse with empty correspondences.""" + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + corres = o3d.utility.Vector2iVector() + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + + rmse = estimation.compute_rmse(source, target, corres) + assert rmse == 0.0 + + def test_transformation_estimation_symmetric_compute_rmse_no_normals(self): + """Test compute_rmse without normals.""" + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + + source.points = o3d.utility.Vector3dVector([[0, 0, 0], [1, 0, 0]]) + target.points = o3d.utility.Vector3dVector([[0.1, 0.1, 0.1], + [1.1, 0.1, 0.1]]) + # No normals + + corres = o3d.utility.Vector2iVector([[0, 0], [1, 1]]) + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + + rmse = estimation.compute_rmse(source, target, corres) + assert rmse == 0.0 + + def test_transformation_estimation_symmetric_compute_transformation(self): + """Test compute_transformation method.""" + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + + # Create test point clouds with normals + source.points = o3d.utility.Vector3dVector([[0, 0, 0], [1, 0, 0], + [0, 1, 0], [1, 1, 0]]) + source.normals = o3d.utility.Vector3dVector([[0, 0, 1], [0, 0, 1], + [0, 0, 1], [0, 0, 1]]) + + target.points = o3d.utility.Vector3dVector([[0, 0, 0], [1, 0, 0], + [0, 1, 0], [1, 1, 0]]) + target.normals = o3d.utility.Vector3dVector([[0, 0, 1], [0, 0, 1], + [0, 0, 1], [0, 0, 1]]) + + corres = o3d.utility.Vector2iVector([[0, 0], [1, 1], [2, 2], [3, 3]]) + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + + transformation = estimation.compute_transformation( + source, target, corres) + + # Should be close to identity for perfect correspondence + expected = np.eye(4) + np.testing.assert_allclose(transformation, expected, atol=1e-3) + + def test_transformation_estimation_symmetric_compute_transformation_empty_corres( + self): + """Test compute_transformation with empty correspondences.""" + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + corres = o3d.utility.Vector2iVector() + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + + transformation = estimation.compute_transformation( + source, target, corres) + + expected = np.eye(4) + np.testing.assert_allclose(transformation, expected) + + def test_transformation_estimation_symmetric_compute_transformation_no_normals( + self): + """Test compute_transformation without normals.""" + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + + source.points = o3d.utility.Vector3dVector([[0, 0, 0], [1, 0, 0]]) + target.points = o3d.utility.Vector3dVector([[0.1, 0.1, 0.1], + [1.1, 0.1, 0.1]]) + # No normals + + corres = o3d.utility.Vector2iVector([[0, 0], [1, 1]]) + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + + transformation = estimation.compute_transformation( + source, target, corres) + + expected = np.eye(4) + np.testing.assert_allclose(transformation, expected) + + def test_registration_symmetric_icp(self): + """Test registration_symmetric_icp function.""" + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + + # Create test point clouds with normals + source.points = o3d.utility.Vector3dVector([[0, 0, 0], [1, 0, 0], + [0, 1, 0]]) + source.normals = o3d.utility.Vector3dVector([[0, 0, 1], [0, 0, 1], + [0, 0, 1]]) + + # Target is slightly translated + target.points = o3d.utility.Vector3dVector([[0.05, 0.05, 0.05], + [1.05, 0.05, 0.05], + [0.05, 1.05, 0.05]]) + target.normals = o3d.utility.Vector3dVector([[0, 0, 1], [0, 0, 1], + [0, 0, 1]]) + + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + criteria = o3d.pipelines.registration.ICPConvergenceCriteria() + + result = o3d.pipelines.registration.registration_symmetric_icp( + source, target, 0.1, np.eye(4), estimation, criteria) + + assert len(result.correspondence_set) > 0 + assert result.fitness > 0.0 + assert result.inlier_rmse >= 0.0 + + # Verify the transformation matrix + # With all normals pointing in Z direction, symmetric ICP can only + # constrain motion along Z axis. X and Y translations are not observable. + # So we only check that Z translation is recovered correctly. + computed_translation = result.transformation[:3, 3] + expected_z_translation = 0.05 + np.testing.assert_allclose( + computed_translation[2], expected_z_translation, + atol=1e-2) # Allow some tolerance due to iterative nature + + # Rotation should be close to identity + expected_rotation = np.eye(3) + computed_rotation = result.transformation[:3, :3] + np.testing.assert_allclose(computed_rotation, + expected_rotation, + atol=1e-2) + + def test_registration_symmetric_icp_with_robust_kernel(self): + """Test registration_symmetric_icp with robust kernel.""" + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + + # Create test point clouds with normals + source.points = o3d.utility.Vector3dVector([[0, 0, 0], [1, 0, 0], + [0, 1, 0], [1, 1, 0]]) + source.normals = o3d.utility.Vector3dVector([[0, 0, 1], [0, 0, 1], + [0, 0, 1], [0, 0, 1]]) + + target.points = o3d.utility.Vector3dVector([[0.02, 0.02, 0.02], + [1.02, 0.02, 0.02], + [0.02, 1.02, 0.02], + [1.02, 1.02, 0.02]]) + target.normals = o3d.utility.Vector3dVector([[0, 0, 1], [0, 0, 1], + [0, 0, 1], [0, 0, 1]]) + + kernel = o3d.pipelines.registration.HuberLoss(0.1) + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + kernel) + criteria = o3d.pipelines.registration.ICPConvergenceCriteria( + 1e-6, 1e-6, 30) + + result = o3d.pipelines.registration.registration_symmetric_icp( + source, target, 0.1, np.eye(4), estimation, criteria) + + assert len(result.correspondence_set) > 0 + assert result.fitness > 0.0 + + def test_registration_symmetric_icp_convergence(self): + """Test registration_symmetric_icp convergence with known transformation.""" + np.random.seed(42) + + # Create a more complex test case + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + + # Generate random points with normals + num_points = 50 + points = np.random.rand(num_points, 3) * 10.0 + normals = np.zeros_like(points) + normals[:, 2] = 1.0 # Simple normal for testing + + source.points = o3d.utility.Vector3dVector(points) + source.normals = o3d.utility.Vector3dVector(normals) + + # Create target by transforming source with known transformation + true_transformation = np.eye(4) + true_transformation[0, 3] = 0.1 # Small translation in x + true_transformation[1, 3] = 0.05 # Small translation in y + + import copy + + target = copy.deepcopy(source) + target.transform(true_transformation) + + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + criteria = o3d.pipelines.registration.ICPConvergenceCriteria( + 1e-6, 1e-6, 30) + + result = o3d.pipelines.registration.registration_symmetric_icp( + source, target, 0.5, np.eye(4), estimation, criteria) + + # Check that registration converged to reasonable result + assert result.fitness > 0.5 + assert result.inlier_rmse < 1.0 + + def test_registration_symmetric_icp_requires_normals(self): + """Test that symmetric ICP requires normals.""" + import pytest + + source = o3d.geometry.PointCloud() + target = o3d.geometry.PointCloud() + + source.points = o3d.utility.Vector3dVector([[0, 0, 0], [1, 0, 0]]) + target.points = o3d.utility.Vector3dVector([[0.1, 0.1, 0.1], + [1.1, 0.1, 0.1]]) + # No normals set - should raise an error + + estimation = o3d.pipelines.registration.TransformationEstimationSymmetric( + ) + criteria = o3d.pipelines.registration.ICPConvergenceCriteria() + + # This should raise a RuntimeError about missing normals + with pytest.raises( + RuntimeError, + match= + "SymmetricICP requires both source and target to have normals"): + o3d.pipelines.registration.registration_symmetric_icp( + source, target, 0.1, np.eye(4), estimation, criteria)