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..8aeb3a5934d 100644 --- a/cpp/open3d/t/pipelines/kernel/Registration.h +++ b/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. 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..028613b0a48 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,100 @@ template bool GetJacobianPointToPlane(int64_t workload_idx, double *J_ij, double &r); +/// \brief Computes Jacobian and residuals for symmetric ICP. +/// +/// Symmetric ICP minimizes both source-to-target and target-to-source +/// point-to-plane distances. This function computes two Jacobians and two +/// residuals for a single correspondence pair. +/// +/// \param workload_idx Index of the correspondence to process. +/// \param source_points_ptr Pointer to source point positions (3N elements). +/// \param target_points_ptr Pointer to target point positions (3N elements). +/// \param source_normals_ptr Pointer to source point normals (3N elements). +/// \param target_normals_ptr Pointer to target point normals (3N elements). +/// \param correspondence_indices Pointer to correspondence indices. +/// \param J_ij Output array for Jacobians (must have space for 12 elements: +/// elements 0-5 for Jacobian using target normal, +/// elements 6-11 for Jacobian using source normal). +/// \param r1 Output residual for source-to-target point-to-plane distance. +/// \param r2 Output residual for target-to-source point-to-plane distance. +/// \return true if correspondence is valid, false if correspondence is -1. +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/docs/jupyter/pipelines/icp_registration.ipynb b/docs/jupyter/pipelines/icp_registration.ipynb index fcc75148ea8..17aae323a60 100644 --- a/docs/jupyter/pipelines/icp_registration.ipynb +++ b/docs/jupyter/pipelines/icp_registration.ipynb @@ -234,6 +234,23 @@ "\n", "" ] + }, + { + "cell_type": "markdown", + "source": "## Symmetric ICP\nSymmetric ICP uses a symmetric point-to-plane objective function that measures distances in both directions:\n\n\\begin{equation}\nE(\\mathbf{T}) = \\sum_{(\\mathbf{p},\\mathbf{q})\\in\\mathcal{K}}\\left[\\big((\\mathbf{p} - \\mathbf{T}\\mathbf{q})\\cdot\\mathbf{n}_{\\mathbf{p}}\\big)^{2} + \\big((\\mathbf{p} - \\mathbf{T}\\mathbf{q})\\cdot\\mathbf{n}_{\\mathbf{q}}\\big)^{2}\\right]\n\\end{equation}\n\nwhere $\\mathbf{n}_{\\mathbf{p}}$ is the normal at target point $\\mathbf{p}$ and $\\mathbf{n}_{\\mathbf{q}}$ is the normal at source point $\\mathbf{q}$. \n\nThis symmetric formulation can provide more robust registration in certain scenarios compared to standard point-to-plane ICP, particularly when both point clouds have reliable normal estimates.\n\nThe class `TransformationEstimationSymmetric` implements functions to compute the residuals and Jacobian matrices of the symmetric ICP objective. The function `registration_symmetric_icp` takes it as a parameter and runs symmetric ICP to obtain the results.", + "metadata": {} + }, + { + "cell_type": "code", + "source": "print(\"Apply symmetric ICP\")\nreg_sym = o3d.pipelines.registration.registration_symmetric_icp(\n source, target, threshold, trans_init,\n o3d.pipelines.registration.TransformationEstimationSymmetric())\nprint(reg_sym)\nprint(\"Transformation is:\")\nprint(reg_sym.transformation)\ndraw_registration_result(source, target, reg_sym.transformation)", + "metadata": {}, + "execution_count": null, + "outputs": [] + }, + { + "cell_type": "markdown", + "source": "Symmetric ICP achieves tight alignment comparable to point-to-plane ICP (a `fitness` score around 0.62 and an `inlier_rmse` around 0.0065).\n\n
\n\n**Note:**\n\nSymmetric ICP requires both source and target point clouds to have normals. The symmetric objective function uses normals from both point clouds to constrain the transformation, which can provide more robust alignment when both sets of normals are reliable.\n\n
", + "metadata": {} } ], "metadata": { @@ -258,4 +275,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/docs/jupyter/t_pipelines/t_icp_registration.ipynb b/docs/jupyter/t_pipelines/t_icp_registration.ipynb index cb22159bc7f..b1312edac3c 100644 --- a/docs/jupyter/t_pipelines/t_icp_registration.ipynb +++ b/docs/jupyter/t_pipelines/t_icp_registration.ipynb @@ -1068,6 +1068,71 @@ "---" ] }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Symmetric ICP Registration\n", + "\n", + "Symmetric ICP uses a symmetric point-to-plane distance metric that considers normals from both the source and target point clouds. The objective function is:\n", + "\n", + "\\begin{equation}\n", + "E(\\mathbf{T}) = \\sum_{(\\mathbf{p},\\mathbf{q})\\in\\mathcal{K}}\\left[\\big((\\mathbf{p} - \\mathbf{T}\\mathbf{q})\\cdot\\mathbf{n}_{\\mathbf{p}}\\big)^{2} + \\big((\\mathbf{p} - \\mathbf{T}\\mathbf{q})\\cdot\\mathbf{n}_{\\mathbf{q}}\\big)^{2}\\right]\n", + "\\end{equation}\n", + "\n", + "This bidirectional formulation can provide more robust and accurate alignment when both point clouds have reliable normal estimates." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "estimation = treg.TransformationEstimationSymmetric()\n", + "\n", + "criteria = treg.ICPConvergenceCriteria(relative_fitness=0.0000001,\n", + " relative_rmse=0.0000001,\n", + " max_iteration=30)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "print(\"Apply Symmetric ICP\")\n", + "s = time.time()\n", + "\n", + "reg_symmetric = treg.icp(source, target, max_correspondence_distance,\n", + " init_source_to_target, estimation, criteria,\n", + " voxel_size)\n", + "\n", + "icp_time = time.time() - s\n", + "print(\"Time taken by Symmetric ICP: \", icp_time)\n", + "print(\"Fitness: \", reg_symmetric.fitness)\n", + "print(\"Inlier RMSE: \", reg_symmetric.inlier_rmse)\n", + "\n", + "draw_registration_result(source, target, reg_symmetric.transformation)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Symmetric ICP provides comparable or better alignment than standard point-to-plane ICP by leveraging normal information from both point clouds. The symmetric objective helps constrain the transformation more effectively, particularly useful when dealing with partial overlaps or noisy data.\n", + "\n", + "**Note:** Symmetric ICP requires both source and target point clouds to have normals computed." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "---" + ] + }, { "cell_type": "markdown", "metadata": { @@ -1285,4 +1350,4 @@ }, "nbformat": 4, "nbformat_minor": 4 -} +} \ No newline at end of file diff --git a/examples/python/pipelines/icp_registration.py b/examples/python/pipelines/icp_registration.py index 6badc4f5bd5..588317ca141 100644 --- a/examples/python/pipelines/icp_registration.py +++ b/examples/python/pipelines/icp_registration.py @@ -42,6 +42,17 @@ def point_to_plane_icp(source, target, threshold, trans_init): draw_registration_result(source, target, reg_p2l.transformation) +def symmetric_icp(source, target, threshold, trans_init): + print("Apply symmetric ICP") + reg_sym = o3d.pipelines.registration.registration_symmetric_icp( + source, target, threshold, trans_init, + o3d.pipelines.registration.TransformationEstimationSymmetric()) + print(reg_sym) + print("Transformation is:") + print(reg_sym.transformation, "\n") + draw_registration_result(source, target, reg_sym.transformation) + + if __name__ == "__main__": pcd_data = o3d.data.DemoICPPointClouds() source = o3d.io.read_point_cloud(pcd_data.paths[0]) @@ -59,3 +70,4 @@ def point_to_plane_icp(source, target, threshold, trans_init): point_to_point_icp(source, target, threshold, trans_init) point_to_plane_icp(source, target, threshold, trans_init) + symmetric_icp(source, target, threshold, trans_init) 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..a1c54d7a4e7 --- /dev/null +++ b/python/test/test_symmetric_icp.py @@ -0,0 +1,280 @@ +# ---------------------------------------------------------------------------- +# - Open3D: www.open3d.org - +# ---------------------------------------------------------------------------- +# Copyright (c) 2018-2024 www.open3d.org +# SPDX-License-Identifier: MIT +# ---------------------------------------------------------------------------- + +import copy + +import numpy as np +import pytest + +import open3d as o3d + + +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() + + # 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 + + 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.""" + 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)