diff --git a/CHANGELOG.md b/CHANGELOG.md index cdf555b0d11..12f7df3086a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,7 @@ ## Main +- Add degeneracy-aware DCReg registration APIs for legacy CPU ICP, including + `TransformationEstimationPointToPlaneDCReg`, degeneracy diagnostic helpers, + and a standalone-compatible local-plane ICP helper. - Upgrade stdgpu third-party library to commit d7c07d0. - Fix performance for non-contiguous NumPy array conversion in pybind vector converters. This change removes restrictive `py::array::c_style` flags and adds a runtime contiguity check, improving Pandas-to-Open3D conversion speed by up to ~50×. (issue #5250)(PR #7343). - Corrected documentation for Link Open3D in C++ projects (broken links). diff --git a/cpp/open3d/pipelines/CMakeLists.txt b/cpp/open3d/pipelines/CMakeLists.txt index 7173b64b515..e1ca5531ecd 100644 --- a/cpp/open3d/pipelines/CMakeLists.txt +++ b/cpp/open3d/pipelines/CMakeLists.txt @@ -20,6 +20,7 @@ target_sources(pipelines PRIVATE target_sources(pipelines PRIVATE registration/ColoredICP.cpp registration/CorrespondenceChecker.cpp + registration/DCReg.cpp registration/FastGlobalRegistration.cpp registration/Feature.cpp registration/GeneralizedICP.cpp diff --git a/cpp/open3d/pipelines/registration/DCReg.cpp b/cpp/open3d/pipelines/registration/DCReg.cpp new file mode 100644 index 00000000000..02d2ca60351 --- /dev/null +++ b/cpp/open3d/pipelines/registration/DCReg.cpp @@ -0,0 +1,1033 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// Copyright (c) 2018-2024 www.open3d.org +// SPDX-License-Identifier: MIT +// ---------------------------------------------------------------------------- + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "open3d/geometry/KDTreeFlann.h" +#include "open3d/geometry/PointCloud.h" +#include "open3d/pipelines/registration/Registration.h" +#include "open3d/pipelines/registration/TransformationEstimation.h" +#include "open3d/utility/Eigen.h" +#include "open3d/utility/Logging.h" + +namespace open3d { +namespace pipelines { +namespace registration { +namespace { + +constexpr double kMinEigenvalue = 1e-9; +constexpr double kMinPivot = 1e-12; +constexpr int kMinCorrespondences = 10; + +struct DCRegSystem { + Eigen::Matrix6d hessian = Eigen::Matrix6d::Zero(); + Eigen::Vector6d rhs = Eigen::Vector6d::Zero(); +}; + +struct DegeneracyDetection { + bool factorization_ok = false; + Eigen::Vector3d lambda_schur_rot = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + Eigen::Vector3d lambda_schur_trans = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + Eigen::Matrix3d raw_rot_basis = Eigen::Matrix3d::Identity(); + Eigen::Matrix3d raw_trans_basis = Eigen::Matrix3d::Identity(); +}; + +struct DegeneracyCharacterization { + bool factorization_ok = false; + Eigen::Matrix6d preconditioner = Eigen::Matrix6d::Identity(); + double condition_number_rot = std::numeric_limits::quiet_NaN(); + double condition_number_trans = std::numeric_limits::quiet_NaN(); + Eigen::Vector3d aligned_lambda_rot = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + Eigen::Vector3d aligned_lambda_trans = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + Eigen::Vector3d clamped_lambda_rot = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + Eigen::Vector3d clamped_lambda_trans = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + Eigen::Vector3i weak_rot_axes = Eigen::Vector3i::Zero(); + Eigen::Vector3i weak_trans_axes = Eigen::Vector3i::Zero(); + bool is_degenerate = false; +}; + +struct SolverResult { + Eigen::Vector6d update = Eigen::Vector6d::Zero(); + std::string solver_type = "dense"; + bool pcg_converged = false; + int pcg_iteration = 0; +}; + +struct LocalPlaneCorrespondence { + int source_index = -1; + int target_index = -1; + Eigen::Vector3d point_body = Eigen::Vector3d::Zero(); + Eigen::Vector3d normal_world = Eigen::Vector3d::Zero(); + double residual = 0.0; + double weight = 0.0; + double weight_derivative = 0.0; +}; + +struct LocalPlaneCorrespondenceSet { + std::vector correspondences; + double residual_square_sum = 0.0; +}; + +DCRegSystem BuildPointToPlaneSystem(const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + const RobustKernel &kernel) { + // Keep the same residual and Jacobian convention as + // TransformationEstimationPointToPlane; only the linear solver changes. + // In RegistrationICP, `source` is already transformed by the current pose, + // and the returned SE(3) increment is left-multiplied in the target/world + // frame. + auto compute_jacobian_and_residual = [&](int i, Eigen::Vector6d &J_r, + double &r, double &w) { + const Eigen::Vector3d &vs = source.points_[corres[i][0]]; + const Eigen::Vector3d &vt = target.points_[corres[i][1]]; + const Eigen::Vector3d &nt = target.normals_[corres[i][1]]; + r = (vs - vt).dot(nt); + w = kernel.Weight(r); + J_r.block<3, 1>(0, 0) = vs.cross(nt); + J_r.block<3, 1>(3, 0) = nt; + }; + + DCRegSystem system; + Eigen::Vector6d JTr; + double r2; + std::tie(system.hessian, JTr, r2) = + utility::ComputeJTJandJTr( + compute_jacobian_and_residual, (int)corres.size()); + system.rhs = -JTr; + return system; +} + +Eigen::Matrix4d MakeTransform(const Eigen::Matrix3d &rotation, + const Eigen::Vector3d &translation) { + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + transform.block<3, 3>(0, 0) = rotation; + transform.block<3, 1>(0, 3) = translation; + return transform; +} + +Eigen::Matrix3d So3Exp(const Eigen::Vector3d &omega) { + const double theta = omega.norm(); + if (theta < 1e-10) { + return Eigen::Matrix3d::Identity() + utility::SkewMatrix(omega); + } + + const Eigen::Vector3d axis = omega / theta; + const Eigen::Matrix3d axis_hat = utility::SkewMatrix(axis); + return Eigen::Matrix3d::Identity() + std::sin(theta) * axis_hat + + (1.0 - std::cos(theta)) * axis_hat * axis_hat; +} + +bool FitLocalPlaneFromNeighbors(const geometry::PointCloud &target, + const std::vector &neighbor_indices, + double max_plane_thickness, + Eigen::Vector3d &normal_world, + double &plane_offset) { + Eigen::Matrix a_matrix(neighbor_indices.size(), + 3); + const Eigen::VectorXd b_vector = + Eigen::VectorXd::Constant(neighbor_indices.size(), -1.0); + for (int row = 0; row < static_cast(neighbor_indices.size()); ++row) { + a_matrix.row(row) = target.points_[neighbor_indices[row]].transpose(); + } + + const Eigen::Vector3d plane_coefficients = + a_matrix.colPivHouseholderQr().solve(b_vector); + const double coeff_norm = plane_coefficients.norm(); + if (coeff_norm < 1e-6) { + return false; + } + + normal_world = plane_coefficients / coeff_norm; + plane_offset = 1.0 / coeff_norm; + + double max_residual = 0.0; + for (const int index : neighbor_indices) { + max_residual = std::max( + max_residual, std::abs(normal_world.dot(target.points_[index]) + + plane_offset)); + } + return max_residual < max_plane_thickness; +} + +LocalPlaneCorrespondenceSet CollectLocalPlaneCorrespondences( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const geometry::KDTreeFlann &target_kdtree, + double max_correspondence_distance, + const Eigen::Matrix4d &transform, + const DCRegOption &option) { + LocalPlaneCorrespondenceSet result; + if (max_correspondence_distance <= 0.0 || option.local_plane_knn_ <= 0) { + return result; + } + + const double radius2 = + max_correspondence_distance * max_correspondence_distance; + const int knn = option.local_plane_knn_; + result.correspondences.reserve(source.points_.size()); + for (int i = 0; i < static_cast(source.points_.size()); ++i) { + const Eigen::Vector3d point_body = source.points_[i]; + const Eigen::Vector3d point_world = + transform.block<3, 3>(0, 0) * point_body + + transform.block<3, 1>(0, 3); + const Eigen::Vector3d search_point(static_cast(point_world.x()), + static_cast(point_world.y()), + static_cast(point_world.z())); + + std::vector neighbor_indices(knn); + std::vector neighbor_distances(knn); + if (target_kdtree.SearchKNN(search_point, knn, neighbor_indices, + neighbor_distances) != knn || + neighbor_distances.back() >= radius2) { + continue; + } + + Eigen::Vector3d normal_world; + double plane_offset = 0.0; + if (!FitLocalPlaneFromNeighbors(target, neighbor_indices, + option.local_plane_max_thickness_, + normal_world, plane_offset)) { + continue; + } + + const double residual = normal_world.dot(point_world) + plane_offset; + const double weight = + std::max(0.0, 1.0 - option.local_plane_weight_slope_ * + std::abs(residual)); + if (weight <= option.local_plane_min_weight_) { + continue; + } + + LocalPlaneCorrespondence correspondence; + correspondence.source_index = i; + correspondence.target_index = neighbor_indices.front(); + correspondence.point_body = point_body; + correspondence.normal_world = normal_world; + correspondence.residual = residual; + correspondence.weight = weight; + if (option.local_plane_use_weight_derivative_ && weight < 1.0 && + weight > 0.0) { + correspondence.weight_derivative = + -option.local_plane_weight_slope_ * + (residual >= 0.0 ? 1.0 : -1.0); + } + result.residual_square_sum += residual * residual; + result.correspondences.push_back(correspondence); + } + return result; +} + +Eigen::Matrix ComputeLocalFrameSo3Jacobian( + const LocalPlaneCorrespondence &correspondence, + const Eigen::Matrix3d &rotation) { + Eigen::Matrix jacobian; + jacobian.block<1, 3>(0, 0) = -correspondence.normal_world.transpose() * + rotation * + utility::SkewMatrix(correspondence.point_body); + jacobian.block<1, 3>(0, 3) = + correspondence.normal_world.transpose() * rotation; + return jacobian; +} + +DCRegSystem BuildLocalFrameSystem( + const std::vector &correspondences, + const Eigen::Matrix3d &rotation) { + DCRegSystem system; + for (const LocalPlaneCorrespondence &correspondence : correspondences) { + const Eigen::Matrix residual_jacobian = + ComputeLocalFrameSo3Jacobian(correspondence, rotation); + const Eigen::Matrix full_jacobian = + correspondence.weight * residual_jacobian + + correspondence.residual * correspondence.weight_derivative * + residual_jacobian; + const double weighted_residual = + -correspondence.weight * correspondence.residual; + system.hessian.noalias() += full_jacobian.transpose() * full_jacobian; + system.rhs.noalias() += full_jacobian.transpose() * weighted_residual; + } + return system; +} + +SolverResult SolveDefaultUpdate(const DCRegSystem &system) { + SolverResult result; + bool is_success; + Eigen::VectorXd update; + std::tie(is_success, update) = + utility::SolveLinearSystemPSD(system.hessian, system.rhs); + if (is_success && update.size() == 6 && update.allFinite()) { + result.update = update; + return result; + } + result.solver_type = "qr_fallback"; + result.update = system.hessian.colPivHouseholderQr().solve(system.rhs); + return result; +} + +SolverResult SolveRawQRUpdate(const DCRegSystem &system) { + SolverResult result; + result.solver_type = "qr_fallback"; + result.update = system.hessian.colPivHouseholderQr().solve(system.rhs); + return result; +} + +std::tuple SolveRankDeficientMinimumNormUpdate( + const DCRegSystem &system) { + // Exact geometric null spaces, such as cylinder-axis translation, should + // not receive arbitrary updates from an LDLT solve. Use the minimum-norm + // solution when the full Hessian is rank deficient. + const Eigen::Matrix6d hessian = + 0.5 * (system.hessian + system.hessian.transpose()); + const Eigen::SelfAdjointEigenSolver solver(hessian); + if (solver.info() != Eigen::Success) { + return std::make_tuple(false, Eigen::Vector6d::Zero()); + } + + const Eigen::Vector6d eigenvalues = solver.eigenvalues(); + const double lambda_max = eigenvalues.cwiseAbs().maxCoeff(); + if (lambda_max < kMinPivot) { + return std::make_tuple(true, Eigen::Vector6d::Zero()); + } + + const double nullspace_threshold = std::max(lambda_max * 1e-10, kMinPivot); + bool rank_deficient = false; + Eigen::Vector6d inverse_eigenvalues = Eigen::Vector6d::Zero(); + for (int i = 0; i < 6; ++i) { + if (eigenvalues(i) <= nullspace_threshold) { + rank_deficient = true; + continue; + } + inverse_eigenvalues(i) = 1.0 / eigenvalues(i); + } + if (!rank_deficient) { + return std::make_tuple(false, Eigen::Vector6d::Zero()); + } + + const Eigen::Vector6d update = + solver.eigenvectors() * inverse_eigenvalues.asDiagonal() * + solver.eigenvectors().transpose() * system.rhs; + if (!update.allFinite()) { + return std::make_tuple(false, Eigen::Vector6d::Zero()); + } + return std::make_tuple(true, update); +} + +double ComputeConditionNumber(const Eigen::Vector3d &lambda) { + const double lambda_max = lambda.maxCoeff(); + return lambda_max / std::max(lambda.minCoeff(), kMinPivot); +} + +double ComputeFullConditionNumber(const Eigen::Matrix6d &hessian) { + const Eigen::JacobiSVD svd(hessian); + const Eigen::Vector6d singular_values = svd.singularValues(); + return singular_values(0) / std::max(singular_values(5), kMinPivot); +} + +bool ComputeSymmetricEigenDecomposition(const Eigen::Matrix3d &matrix, + Eigen::Vector3d &eigenvalues, + Eigen::Matrix3d &eigenvectors) { + const Eigen::Matrix3d symmetric = 0.5 * (matrix + matrix.transpose()); + const Eigen::SelfAdjointEigenSolver solver(symmetric); + if (solver.info() != Eigen::Success) { + return false; + } + eigenvalues = solver.eigenvalues(); + eigenvectors = solver.eigenvectors(); + return eigenvalues.allFinite() && eigenvectors.allFinite(); +} + +DegeneracyDetection DetectDegeneracy(const Eigen::Matrix6d &hessian) { + DegeneracyDetection detection; + + const Eigen::Matrix3d h_rr = hessian.block<3, 3>(0, 0); + const Eigen::Matrix3d h_rt = hessian.block<3, 3>(0, 3); + const Eigen::Matrix3d h_tr = hessian.block<3, 3>(3, 0); + const Eigen::Matrix3d h_tt = hessian.block<3, 3>(3, 3); + + const Eigen::FullPivLU lu_rr(h_rr); + const Eigen::FullPivLU lu_tt(h_tt); + if (!lu_rr.isInvertible() || !lu_tt.isInvertible()) { + return detection; + } + + // DCReg analyzes clean rotational and translational subspaces via Schur + // complements: S_R = H_RR - H_Rt H_tt^{-1} H_tR and + // S_t = H_tt - H_tR H_RR^{-1} H_Rt. + const Eigen::Matrix3d schur_rot = h_rr - h_rt * lu_tt.solve(h_tr); + const Eigen::Matrix3d schur_trans = h_tt - h_tr * lu_rr.solve(h_rt); + + Eigen::Vector3d lambda_rot; + Eigen::Vector3d lambda_trans; + Eigen::Matrix3d raw_rot_basis; + Eigen::Matrix3d raw_trans_basis; + if (!ComputeSymmetricEigenDecomposition(schur_rot, lambda_rot, + raw_rot_basis) || + !ComputeSymmetricEigenDecomposition(schur_trans, lambda_trans, + raw_trans_basis)) { + return detection; + } + + detection.factorization_ok = true; + detection.lambda_schur_rot = lambda_rot; + detection.lambda_schur_trans = lambda_trans; + detection.raw_rot_basis = raw_rot_basis; + detection.raw_trans_basis = raw_trans_basis; + return detection; +} + +DegeneracyDetection DetectBlockEigenvalueFallback( + const Eigen::Matrix6d &hessian) { + DegeneracyDetection detection; + + const Eigen::Matrix3d h_rr = hessian.block<3, 3>(0, 0); + const Eigen::Matrix3d h_tt = hessian.block<3, 3>(3, 3); + Eigen::Vector3d lambda_rot; + Eigen::Vector3d lambda_trans; + Eigen::Matrix3d raw_rot_basis; + Eigen::Matrix3d raw_trans_basis; + if (!ComputeSymmetricEigenDecomposition(h_rr, lambda_rot, raw_rot_basis) || + !ComputeSymmetricEigenDecomposition(h_tt, lambda_trans, + raw_trans_basis)) { + return detection; + } + + // When Schur complements cannot be formed because a block is singular, the + // block spectra still give interpretable x/y/z weak-axis diagnostics. This + // fallback is diagnostic-only; the ICP update continues to use the + // minimum-norm rank-deficient solve. + detection.factorization_ok = true; + detection.lambda_schur_rot = lambda_rot; + detection.lambda_schur_trans = lambda_trans; + detection.raw_rot_basis = raw_rot_basis; + detection.raw_trans_basis = raw_trans_basis; + return detection; +} + +bool AlignEigenBasisToAxes(const Eigen::Matrix3d &raw_basis, + Eigen::Matrix3d &aligned_basis, + std::array &original_indices) { + // Eigenvectors are sign- and order-ambiguous. Align them to the physical + // x/y/z axes before deciding which weak directions should be clamped. + const std::array refs = { + Eigen::Vector3d::UnitX(), + Eigen::Vector3d::UnitY(), + Eigen::Vector3d::UnitZ(), + }; + aligned_basis.setZero(); + original_indices.fill(-1); + + std::array used = {false, false, false}; + for (int axis = 0; axis < 3; ++axis) { + double best_score = -1.0; + int best_index = -1; + for (int candidate = 0; candidate < 3; ++candidate) { + if (used[candidate]) { + continue; + } + const double score = + std::abs(refs[axis].dot(raw_basis.col(candidate))); + if (score > best_score) { + best_score = score; + best_index = candidate; + } + } + if (best_index < 0) { + return false; + } + used[best_index] = true; + original_indices[axis] = best_index; + Eigen::Vector3d aligned_column = raw_basis.col(best_index); + if (refs[axis].dot(aligned_column) < 0.0) { + aligned_column = -aligned_column; + } + aligned_basis.col(axis) = aligned_column; + } + return true; +} + +Eigen::Vector3i ComputeWeakAxes(const Eigen::Vector3d &aligned_lambda, + const DCRegOption &option) { + Eigen::Vector3i weak_axes = Eigen::Vector3i::Zero(); + const double lambda_max = + std::max(aligned_lambda.maxCoeff(), kMinEigenvalue); + const double threshold = + std::max(option.degeneracy_condition_threshold_, 1.0); + for (int axis = 0; axis < 3; ++axis) { + const double condition = + lambda_max / std::max(aligned_lambda(axis), kMinPivot); + if (condition > threshold) { + weak_axes(axis) = 1; + } + } + return weak_axes; +} + +Eigen::Vector3d ClampWeakEigenvalues(const Eigen::Vector3d &aligned_lambda, + const Eigen::Vector3i &weak_axes, + const DCRegOption &option) { + Eigen::Vector3d clamped = aligned_lambda; + const double lambda_max = + std::max(aligned_lambda.maxCoeff(), kMinEigenvalue); + const double kappa_target = std::max(option.kappa_target_, 1.0); + const double weak_lambda = + std::max(lambda_max / kappa_target, kMinEigenvalue); + + for (int axis = 0; axis < 3; ++axis) { + if (weak_axes(axis)) { + clamped(axis) = weak_lambda; + } + } + return clamped; +} + +DegeneracyCharacterization CharacterizeDegeneracy( + const DegeneracyDetection &detection, const DCRegOption &option) { + DegeneracyCharacterization characterization; + characterization.factorization_ok = detection.factorization_ok; + if (!detection.factorization_ok) { + characterization.is_degenerate = true; + characterization.weak_rot_axes = Eigen::Vector3i::Ones(); + characterization.weak_trans_axes = Eigen::Vector3i::Ones(); + return characterization; + } + + Eigen::Matrix3d aligned_rot_basis; + Eigen::Matrix3d aligned_trans_basis; + std::array rot_indices; + std::array trans_indices; + if (!AlignEigenBasisToAxes(detection.raw_rot_basis, aligned_rot_basis, + rot_indices) || + !AlignEigenBasisToAxes(detection.raw_trans_basis, aligned_trans_basis, + trans_indices)) { + characterization.factorization_ok = false; + return characterization; + } + + for (int axis = 0; axis < 3; ++axis) { + characterization.aligned_lambda_rot(axis) = + detection.lambda_schur_rot(rot_indices[axis]); + characterization.aligned_lambda_trans(axis) = + detection.lambda_schur_trans(trans_indices[axis]); + } + + characterization.condition_number_rot = + ComputeConditionNumber(characterization.aligned_lambda_rot); + characterization.condition_number_trans = + ComputeConditionNumber(characterization.aligned_lambda_trans); + characterization.weak_rot_axes = + ComputeWeakAxes(characterization.aligned_lambda_rot, option); + characterization.weak_trans_axes = + ComputeWeakAxes(characterization.aligned_lambda_trans, option); + characterization.is_degenerate = characterization.weak_rot_axes.any() || + characterization.weak_trans_axes.any(); + characterization.clamped_lambda_rot = + ClampWeakEigenvalues(characterization.aligned_lambda_rot, + characterization.weak_rot_axes, option); + characterization.clamped_lambda_trans = + ClampWeakEigenvalues(characterization.aligned_lambda_trans, + characterization.weak_trans_axes, option); + + characterization.preconditioner.setZero(); + characterization.preconditioner.block<3, 3>(0, 0) = + aligned_rot_basis * + characterization.clamped_lambda_rot.cwiseMax(kMinEigenvalue) + .cwiseInverse() + .asDiagonal() * + aligned_rot_basis.transpose(); + characterization.preconditioner.block<3, 3>(3, 3) = + aligned_trans_basis * + characterization.clamped_lambda_trans.cwiseMax(kMinEigenvalue) + .cwiseInverse() + .asDiagonal() * + aligned_trans_basis.transpose(); + return characterization; +} + +SolverResult SolvePreconditionedUpdate(const DCRegSystem &system, + const DCRegOption &option) { + bool is_rank_deficient = false; + Eigen::Vector6d rank_deficient_update; + std::tie(is_rank_deficient, rank_deficient_update) = + SolveRankDeficientMinimumNormUpdate(system); + if (is_rank_deficient) { + SolverResult result; + result.update = rank_deficient_update; + result.solver_type = "minimum_norm"; + return result; + } + + const DegeneracyDetection detection = DetectDegeneracy(system.hessian); + const DegeneracyCharacterization characterization = + CharacterizeDegeneracy(detection, option); + if (!characterization.factorization_ok || + !characterization.preconditioner.allFinite()) { + return SolveDefaultUpdate(system); + } + + // The original normal equation is preserved; weak eigenvalues are clamped + // only in the preconditioner, not in the Hessian itself. + const Eigen::Matrix6d hessian = + 0.5 * (system.hessian + system.hessian.transpose()); + const double rhs_norm = system.rhs.norm(); + if (rhs_norm < kMinPivot) { + SolverResult result; + result.solver_type = "zero_rhs"; + return result; + } + + Eigen::Vector6d delta = Eigen::Vector6d::Zero(); + Eigen::Vector6d residual = system.rhs; + const Eigen::Vector6d preconditioned_residual = + characterization.preconditioner * residual; + Eigen::Vector6d direction = preconditioned_residual; + double rz_old = residual.dot(preconditioned_residual); + if (!preconditioned_residual.allFinite() || !std::isfinite(rz_old) || + std::abs(rz_old) < kMinPivot) { + return SolveDefaultUpdate(system); + } + + const double tolerance = option.pcg_tolerance_ > 0.0 + ? option.pcg_tolerance_ + : DCRegOption().pcg_tolerance_; + const double target_residual = tolerance * std::max(1.0, rhs_norm); + const int max_iteration = std::max(1, option.pcg_max_iteration_); + int pcg_iteration = 0; + for (int iteration = 0; iteration < max_iteration; ++iteration) { + pcg_iteration = iteration + 1; + const Eigen::Vector6d hessian_direction = hessian * direction; + const double denom = direction.dot(hessian_direction); + if (!std::isfinite(denom) || std::abs(denom) < kMinPivot) { + break; + } + + const double alpha = rz_old / denom; + if (!std::isfinite(alpha)) { + break; + } + + delta += alpha * direction; + residual -= alpha * hessian_direction; + if (!delta.allFinite() || !residual.allFinite()) { + break; + } + if (residual.norm() <= target_residual) { + SolverResult result; + result.update = delta; + result.solver_type = "pcg"; + result.pcg_converged = true; + result.pcg_iteration = pcg_iteration; + return result; + } + + const Eigen::Vector6d z_next = + characterization.preconditioner * residual; + const double rz_new = residual.dot(z_next); + if (!z_next.allFinite() || !std::isfinite(rz_new) || + std::abs(rz_old) < kMinPivot) { + break; + } + + const double beta = rz_new / rz_old; + if (!std::isfinite(beta)) { + break; + } + direction = z_next + beta * direction; + rz_old = rz_new; + } + + SolverResult result = SolveDefaultUpdate(system); + result.pcg_iteration = pcg_iteration; + return result; +} + +SolverResult SolvePreconditionedUpdateDCRegCompatible( + const DCRegSystem &system, const DCRegOption &option) { + const DegeneracyDetection detection = DetectDegeneracy(system.hessian); + const DegeneracyCharacterization characterization = + CharacterizeDegeneracy(detection, option); + if (!characterization.factorization_ok || + !characterization.preconditioner.allFinite()) { + return SolveRawQRUpdate(system); + } + + const Eigen::Matrix6d hessian = + 0.5 * (system.hessian + system.hessian.transpose()); + const double rhs_norm = system.rhs.norm(); + if (rhs_norm < kMinPivot) { + SolverResult result; + result.solver_type = "zero_rhs"; + result.pcg_converged = true; + return result; + } + + Eigen::Vector6d delta = Eigen::Vector6d::Zero(); + Eigen::Vector6d residual = system.rhs; + Eigen::Vector6d preconditioned_residual = + characterization.preconditioner * residual; + if (!preconditioned_residual.allFinite()) { + return SolveRawQRUpdate(system); + } + Eigen::Vector6d direction = preconditioned_residual; + double rz_old = residual.dot(preconditioned_residual); + if (!std::isfinite(rz_old) || std::abs(rz_old) < 1e-20) { + return SolveRawQRUpdate(system); + } + + const double tolerance = option.pcg_tolerance_ > 0.0 + ? option.pcg_tolerance_ + : DCRegOption().pcg_tolerance_; + const double target_residual = tolerance * std::max(1.0, rhs_norm); + const int max_iteration = std::max(option.pcg_max_iteration_, 6); + int pcg_iteration = 0; + for (int iteration = 0; iteration < max_iteration; ++iteration) { + pcg_iteration = iteration + 1; + const Eigen::Vector6d hessian_direction = hessian * direction; + const double denom = direction.dot(hessian_direction); + if (!std::isfinite(denom) || std::abs(denom) < 1e-20) { + break; + } + + const double alpha = rz_old / denom; + if (!std::isfinite(alpha)) { + break; + } + + delta += alpha * direction; + residual -= alpha * hessian_direction; + if (!delta.allFinite() || !residual.allFinite()) { + break; + } + if (residual.norm() <= target_residual) { + SolverResult result; + result.update = delta; + result.solver_type = "pcg"; + result.pcg_converged = true; + result.pcg_iteration = pcg_iteration; + return result; + } + + const Eigen::Vector6d z_next = + characterization.preconditioner * residual; + const double rz_new = residual.dot(z_next); + if (!z_next.allFinite() || !std::isfinite(rz_new) || + std::abs(rz_old) < 1e-20) { + break; + } + + const double beta = rz_new / rz_old; + if (!std::isfinite(beta)) { + break; + } + direction = z_next + beta * direction; + preconditioned_residual = z_next; + rz_old = rz_new; + } + + SolverResult result = SolveRawQRUpdate(system); + result.pcg_iteration = pcg_iteration; + return result; +} + +std::string FormatAxisList(const Eigen::Vector3i &weak_axes) { + constexpr std::array kAxisLabels = {"x", "y", "z"}; + std::string text; + for (int axis = 0; axis < 3; ++axis) { + if (!weak_axes(axis)) { + continue; + } + if (!text.empty()) { + text += ", "; + } + text += kAxisLabels[axis]; + } + return text.empty() ? "none" : text; +} + +std::string FormatDouble(double value) { + if (std::isnan(value)) { + return "nan"; + } + if (std::isinf(value)) { + return value > 0.0 ? "inf" : "-inf"; + } + std::ostringstream stream; + stream << std::setprecision(6) << value; + return stream.str(); +} + +void FillDegeneracyDescription(DCRegDegeneracyAnalysis &analysis) { + analysis.weak_rotation_axes_description_ = + FormatAxisList(analysis.weak_rotation_axes_); + analysis.weak_translation_axes_description_ = + FormatAxisList(analysis.weak_translation_axes_); + + std::ostringstream stream; + if (!analysis.has_correspondence_) { + stream << "No correspondences; DCReg degeneracy was not evaluated."; + } else if (!analysis.has_target_normals_) { + stream << "Target point cloud has no normals; point-to-plane DCReg " + "degeneracy was not evaluated."; + } else if (!analysis.is_degenerate_) { + stream << "No degeneracy detected in the " << analysis.coordinate_frame_ + << " normal equation."; + } else { + stream << "Degenerate in the " << analysis.coordinate_frame_ + << " normal equation: weak rotation axes = " + << analysis.weak_rotation_axes_description_ + << ", weak translation axes = " + << analysis.weak_translation_axes_description_ + << ". condition_number_full = " + << FormatDouble(analysis.condition_number_full_) + << ", condition_number_rotation = " + << FormatDouble(analysis.condition_number_rotation_) + << ", condition_number_translation = " + << FormatDouble(analysis.condition_number_translation_) << ". "; + if (analysis.coordinate_frame_.find("local body frame") != + std::string::npos) { + stream << "This diagnostic uses the standalone-compatible kNN " + "local-plane residual and SO(3) local-frame Jacobian."; + } else { + stream << "This Open3D diagnostic uses the target/world-frame " + "left-multiplied SE(3) point-to-plane Jacobian; it is " + "not the standalone DCReg SO(3) local-frame parking-lot " + "diagnostic."; + } + } + analysis.degeneracy_description_ = stream.str(); +} + +void PopulateAnalysisFromSystem(DCRegDegeneracyAnalysis &analysis, + const DCRegSystem &system, + const DCRegOption &option, + bool use_open3d_rank_deficient_fallback) { + analysis.condition_number_full_ = + ComputeFullConditionNumber(system.hessian); + + Eigen::Vector6d rank_deficient_update; + std::tie(analysis.is_rank_deficient_, rank_deficient_update) = + SolveRankDeficientMinimumNormUpdate(system); + + const DegeneracyDetection schur_detection = + DetectDegeneracy(system.hessian); + DegeneracyDetection diagnostic_detection = schur_detection; + if (use_open3d_rank_deficient_fallback && + !diagnostic_detection.factorization_ok && analysis.is_rank_deficient_) { + diagnostic_detection = DetectBlockEigenvalueFallback(system.hessian); + } + + const DegeneracyCharacterization characterization = + CharacterizeDegeneracy(diagnostic_detection, option); + analysis.schur_factorization_ok_ = schur_detection.factorization_ok; + analysis.condition_number_rotation_ = characterization.condition_number_rot; + analysis.condition_number_translation_ = + characterization.condition_number_trans; + analysis.schur_eigenvalues_rotation_ = + diagnostic_detection.lambda_schur_rot; + analysis.schur_eigenvalues_translation_ = + diagnostic_detection.lambda_schur_trans; + analysis.axis_aligned_eigenvalues_rotation_ = + characterization.aligned_lambda_rot; + analysis.axis_aligned_eigenvalues_translation_ = + characterization.aligned_lambda_trans; + analysis.clamped_eigenvalues_rotation_ = + characterization.clamped_lambda_rot; + analysis.clamped_eigenvalues_translation_ = + characterization.clamped_lambda_trans; + analysis.weak_rotation_axes_ = characterization.weak_rot_axes; + analysis.weak_translation_axes_ = characterization.weak_trans_axes; + analysis.is_degenerate_ = + analysis.is_rank_deficient_ || characterization.is_degenerate; + + const SolverResult solver_result = + use_open3d_rank_deficient_fallback + ? SolvePreconditionedUpdate(system, option) + : SolvePreconditionedUpdateDCRegCompatible(system, option); + analysis.solver_type_ = solver_result.solver_type; + analysis.pcg_converged_ = solver_result.pcg_converged; + analysis.pcg_iteration_ = solver_result.pcg_iteration; +} + +} // namespace + +DCRegDegeneracyAnalysis ComputeDCRegDegeneracyAnalysis( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + const DCRegOption &option, + const RobustKernel &kernel) { + DCRegDegeneracyAnalysis analysis; + analysis.has_correspondence_ = !corres.empty(); + analysis.has_target_normals_ = target.HasNormals(); + if (corres.empty() || !target.HasNormals()) { + FillDegeneracyDescription(analysis); + return analysis; + } + + const DCRegSystem system = + BuildPointToPlaneSystem(source, target, corres, kernel); + PopulateAnalysisFromSystem(analysis, system, option, true); + FillDegeneracyDescription(analysis); + return analysis; +} + +DCRegDegeneracyAnalysis ComputeDCRegDegeneracyAnalysis( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + const DCRegOption &option) { + const L2Loss kernel; + return ComputeDCRegDegeneracyAnalysis(source, target, corres, option, + kernel); +} + +DCRegDegeneracyAnalysis ComputeDCRegLocalDegeneracyAnalysis( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + double max_correspondence_distance, + const Eigen::Matrix4d &transformation, + const DCRegOption &option) { + DCRegDegeneracyAnalysis analysis; + analysis.coordinate_frame_ = + "local body frame (standalone DCReg SO(3) update)"; + analysis.has_target_normals_ = true; + + geometry::KDTreeFlann target_kdtree; + target_kdtree.SetGeometry(target); + const LocalPlaneCorrespondenceSet local_correspondences = + CollectLocalPlaneCorrespondences(source, target, target_kdtree, + max_correspondence_distance, + transformation, option); + analysis.has_correspondence_ = + !local_correspondences.correspondences.empty(); + if (!analysis.has_correspondence_) { + FillDegeneracyDescription(analysis); + return analysis; + } + + const DCRegSystem system = + BuildLocalFrameSystem(local_correspondences.correspondences, + transformation.block<3, 3>(0, 0)); + PopulateAnalysisFromSystem(analysis, system, option, false); + FillDegeneracyDescription(analysis); + return analysis; +} + +RegistrationResult RegistrationICPDCRegLocal( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + double max_correspondence_distance, + const Eigen::Matrix4d &init, + const DCRegOption &option, + const ICPConvergenceCriteria &criteria) { + RegistrationResult result(init); + if (max_correspondence_distance <= 0.0) { + utility::LogError("Invalid max_correspondence_distance."); + } + + geometry::KDTreeFlann target_kdtree; + target_kdtree.SetGeometry(target); + + Eigen::Matrix3d rotation = init.block<3, 3>(0, 0); + Eigen::Vector3d translation = init.block<3, 1>(0, 3); + for (int iteration = 0; iteration < criteria.max_iteration_; ++iteration) { + const Eigen::Matrix4d current_transform = + MakeTransform(rotation, translation); + const LocalPlaneCorrespondenceSet local_correspondences = + CollectLocalPlaneCorrespondences(source, target, target_kdtree, + max_correspondence_distance, + current_transform, option); + if (static_cast(local_correspondences.correspondences.size()) < + kMinCorrespondences) { + result.transformation_ = current_transform; + result.correspondence_set_.clear(); + result.fitness_ = 0.0; + result.inlier_rmse_ = 0.0; + return result; + } + + const DCRegSystem system = BuildLocalFrameSystem( + local_correspondences.correspondences, rotation); + const SolverResult solver_result = + SolvePreconditionedUpdateDCRegCompatible(system, option); + const Eigen::Vector6d &delta = solver_result.update; + if (!delta.allFinite()) { + result.transformation_ = current_transform; + return result; + } + + const Eigen::Matrix3d previous_rotation = rotation; + rotation = rotation * So3Exp(delta.head<3>()); + translation = translation + previous_rotation * delta.tail<3>(); + + result.transformation_ = MakeTransform(rotation, translation); + result.correspondence_set_.clear(); + result.correspondence_set_.reserve( + local_correspondences.correspondences.size()); + for (const LocalPlaneCorrespondence &correspondence : + local_correspondences.correspondences) { + result.correspondence_set_.emplace_back( + correspondence.source_index, correspondence.target_index); + } + result.fitness_ = + static_cast( + local_correspondences.correspondences.size()) / + static_cast(source.points_.size()); + result.inlier_rmse_ = std::sqrt( + local_correspondences.residual_square_sum / + static_cast( + local_correspondences.correspondences.size())); + + if (delta.head<3>().norm() < option.local_frame_convergence_rotation_ && + delta.tail<3>().norm() < + option.local_frame_convergence_translation_) { + return result; + } + } + + return result; +} + +Eigen::Matrix4d +TransformationEstimationPointToPlaneDCReg::ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { + if (corres.empty() || !target.HasNormals()) { + return Eigen::Matrix4d::Identity(); + } + + const DCRegSystem system = + BuildPointToPlaneSystem(source, target, corres, *kernel_); + const Eigen::Vector6d update = + SolvePreconditionedUpdate(system, option_).update; + if (!update.allFinite()) { + return Eigen::Matrix4d::Identity(); + } + // RegistrationICP applies this matrix as update * current_pose, matching + // the legacy point-to-plane estimator's left-multiplied SE(3) convention. + return utility::TransformVector6dToMatrix4d(update); +} + +} // namespace registration +} // namespace pipelines +} // namespace open3d diff --git a/cpp/open3d/pipelines/registration/Registration.h b/cpp/open3d/pipelines/registration/Registration.h index 8bc1cd8a93b..50fc48d5bf1 100644 --- a/cpp/open3d/pipelines/registration/Registration.h +++ b/cpp/open3d/pipelines/registration/Registration.h @@ -158,6 +158,20 @@ RegistrationResult RegistrationICP( TransformationEstimationPointToPoint(false), const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria()); +/// \brief DCReg-compatible ICP registration with kNN local-plane residuals and +/// a local body-frame SO(3) pose update. +/// +/// This function is intentionally separate from RegistrationICP because the +/// standalone DCReg update needs the original source points and current pose +/// state, which are not available inside a TransformationEstimation callback. +RegistrationResult RegistrationICPDCRegLocal( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + double max_correspondence_distance, + const Eigen::Matrix4d &init = Eigen::Matrix4d::Identity(), + const DCRegOption &option = DCRegOption(), + const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria()); + /// \brief Function for global RANSAC registration based on a given set of /// correspondences. /// diff --git a/cpp/open3d/pipelines/registration/TransformationEstimation.h b/cpp/open3d/pipelines/registration/TransformationEstimation.h index 40b3229f6ca..f8c315610c9 100644 --- a/cpp/open3d/pipelines/registration/TransformationEstimation.h +++ b/cpp/open3d/pipelines/registration/TransformationEstimation.h @@ -8,6 +8,7 @@ #pragma once #include +#include #include #include #include @@ -173,6 +174,241 @@ class TransformationEstimationPointToPlane : public TransformationEstimation { const TransformationEstimationType type_ = TransformationEstimationType::PointToPlane; }; +/// \class DCRegOption +/// +/// Options for degeneracy-aware point-to-plane ICP. +/// +/// This estimator adapts the DCReg formulation from Hu et al., +/// "DCReg: Decoupled Characterization for Efficient Degenerate LiDAR +/// Registration", arXiv:2509.06285, https://arxiv.org/abs/2509.06285 . +/// DCReg detects weak rotational and translational directions from the Schur +/// complements of the ICP normal equation and uses the result as a +/// preconditioner. +class DCRegOption { +public: + /// \brief Parameterized Constructor. + /// + /// \param degeneracy_condition_threshold Directions whose Schur condition + /// number exceeds this threshold are treated as weak. + /// \param kappa_target Target condition number after clamping weak Schur + /// eigenvalues. + /// \param pcg_tolerance Relative residual threshold for the PCG solve. + /// \param pcg_max_iteration Maximum number of PCG iterations before + /// falling back to the default dense solve. + DCRegOption(double degeneracy_condition_threshold = 10.0, + double kappa_target = 10.0, + double pcg_tolerance = 1e-6, + int pcg_max_iteration = 10, + int local_plane_knn = 5, + double local_plane_max_thickness = 0.2, + double local_plane_weight_slope = 0.9, + double local_plane_min_weight = 0.1, + bool local_plane_use_weight_derivative = true, + double local_frame_convergence_rotation = 1e-5, + double local_frame_convergence_translation = 1e-3) + : degeneracy_condition_threshold_(degeneracy_condition_threshold), + kappa_target_(kappa_target), + pcg_tolerance_(pcg_tolerance), + pcg_max_iteration_(pcg_max_iteration), + local_plane_knn_(local_plane_knn), + local_plane_max_thickness_(local_plane_max_thickness), + local_plane_weight_slope_(local_plane_weight_slope), + local_plane_min_weight_(local_plane_min_weight), + local_plane_use_weight_derivative_(local_plane_use_weight_derivative), + local_frame_convergence_rotation_(local_frame_convergence_rotation), + local_frame_convergence_translation_( + local_frame_convergence_translation) {} + +public: + /// Schur condition threshold for weak directions. + double degeneracy_condition_threshold_ = 10.0; + /// Target condition number used when clamping weak Schur eigenvalues. + double kappa_target_ = 10.0; + /// Relative residual threshold for the PCG solve. + double pcg_tolerance_ = 1e-6; + /// Maximum number of PCG iterations. + int pcg_max_iteration_ = 10; + /// Number of target neighbors used by the DCReg-local plane fit. + int local_plane_knn_ = 5; + /// Maximum accepted local plane residual on its supporting neighbors. + double local_plane_max_thickness_ = 0.2; + /// Slope for the original DCReg piecewise-linear robust weight. + double local_plane_weight_slope_ = 0.9; + /// Minimum accepted original DCReg robust weight. + double local_plane_min_weight_ = 0.1; + /// Whether to include the original robust-weight derivative term. + bool local_plane_use_weight_derivative_ = true; + /// Local-frame SO(3) rotation-step convergence threshold. + double local_frame_convergence_rotation_ = 1e-5; + /// Local-frame SO(3) translation-step convergence threshold. + double local_frame_convergence_translation_ = 1e-3; +}; + +/// \class DCRegDegeneracyAnalysis +/// +/// Diagnostic summary for the DCReg normal equation at one ICP linearization. +/// Eigenvalue fields with the ``axis_aligned`` prefix follow the physical +/// x/y/z axis order. Weak-axis fields use 1 for weak and 0 for not weak. +/// For the Open3D legacy ICP estimator, these axes are the target/world-frame +/// incremental axes used by the Open3D left-multiplied SE(3) point-to-plane +/// linearization, not the local body-frame axes used by the standalone DCReg +/// SO(3) examples. +/// If a rank-deficient system prevents Schur-complement factorization, the +/// eigenvalue and weak-axis fields are populated by a block-Hessian eigensolver +/// fallback while schur_factorization_ok_ remains false. +class DCRegDegeneracyAnalysis { +public: + /// True if at least one correspondence is available. + bool has_correspondence_ = false; + /// True if the target point cloud has normals. + bool has_target_normals_ = false; + /// True if the full 6x6 normal equation has a numerical null space. + bool is_rank_deficient_ = false; + /// True if rotational and translational Schur complements were computed. + bool schur_factorization_ok_ = false; + /// True if any Schur condition number exceeds the configured threshold or + /// the full normal equation is rank deficient. + bool is_degenerate_ = false; + /// Condition number of the full 6x6 normal equation. + double condition_number_full_ = std::numeric_limits::quiet_NaN(); + /// Condition number of the rotational Schur complement. + double condition_number_rotation_ = + std::numeric_limits::quiet_NaN(); + /// Condition number of the translational Schur complement. + double condition_number_translation_ = + std::numeric_limits::quiet_NaN(); + /// Raw rotational Schur eigenvalues in ascending order, or block-Hessian + /// fallback eigenvalues if schur_factorization_ok_ is false. + Eigen::Vector3d schur_eigenvalues_rotation_ = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + /// Raw translational Schur eigenvalues in ascending order, or block-Hessian + /// fallback eigenvalues if schur_factorization_ok_ is false. + Eigen::Vector3d schur_eigenvalues_translation_ = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + /// Rotational diagnostic eigenvalues aligned to x/y/z axes. + Eigen::Vector3d axis_aligned_eigenvalues_rotation_ = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + /// Translational diagnostic eigenvalues aligned to x/y/z axes. + Eigen::Vector3d axis_aligned_eigenvalues_translation_ = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + /// Clamped rotational eigenvalues used by the preconditioner. + Eigen::Vector3d clamped_eigenvalues_rotation_ = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + /// Clamped translational eigenvalues used by the preconditioner. + Eigen::Vector3d clamped_eigenvalues_translation_ = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + /// Weak rotational x/y/z axes. + Eigen::Vector3i weak_rotation_axes_ = Eigen::Vector3i::Zero(); + /// Weak translational x/y/z axes. + Eigen::Vector3i weak_translation_axes_ = Eigen::Vector3i::Zero(); + /// Coordinate frame used by weak-axis diagnostics. + std::string coordinate_frame_ = + "target/world frame (Open3D left-multiplied SE(3) update)"; + /// Human-readable weak rotational axis list. + std::string weak_rotation_axes_description_ = "none"; + /// Human-readable weak translational axis list. + std::string weak_translation_axes_description_ = "none"; + /// Human-readable degeneracy summary. + std::string degeneracy_description_ = "Degeneracy was not evaluated."; + /// Solver path selected for the normal equation. + std::string solver_type_ = "invalid"; + /// True if the PCG path converged before fallback. + bool pcg_converged_ = false; + /// Number of PCG iterations executed. + int pcg_iteration_ = 0; +}; + +/// \class TransformationEstimationPointToPlaneDCReg +/// +/// Degeneracy-aware point-to-plane transformation estimation for ICP. +/// +/// This is an Open3D-native legacy CPU implementation of the DCReg linear-solve +/// idea. It uses Open3D's point-to-plane residual model and correspondence +/// pipeline, but solves the 6D normal equation with a Schur-based +/// degeneracy-aware preconditioner. The returned increment follows Open3D's +/// legacy ICP convention and is left-multiplied onto the current pose by +/// RegistrationICP. It does not include the standalone DCReg +/// repository's dataset runners, PCL plane fitting path, or experiment logging. +class TransformationEstimationPointToPlaneDCReg + : public TransformationEstimationPointToPlane { +public: + /// \brief Default Constructor. + TransformationEstimationPointToPlaneDCReg() {} + ~TransformationEstimationPointToPlaneDCReg() override {} + + /// \brief Constructor that takes DCReg options. + /// \param option DCReg solver options. + explicit TransformationEstimationPointToPlaneDCReg(DCRegOption option) + : option_(std::move(option)) {} + + /// \brief Constructor that takes a RobustKernel. + /// \param kernel Any of the implemented statistical robust kernels for + /// outlier rejection. + explicit TransformationEstimationPointToPlaneDCReg( + std::shared_ptr kernel) + : TransformationEstimationPointToPlane(std::move(kernel)) {} + + /// \brief Constructor that takes DCReg options and a RobustKernel. + /// \param option DCReg solver options. + /// \param kernel Any of the implemented statistical robust kernels for + /// outlier rejection. + TransformationEstimationPointToPlaneDCReg( + DCRegOption option, std::shared_ptr kernel) + : TransformationEstimationPointToPlane(std::move(kernel)), + option_(std::move(option)) {} + +public: + Eigen::Matrix4d ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const override; + +public: + /// DCReg solver options. + DCRegOption option_; +}; + +/// \brief Compute DCReg degeneracy diagnostics for one point-to-plane ICP +/// linearization. +/// +/// \param source Source point cloud. +/// \param target Target point cloud. It must have normals for a valid analysis. +/// \param corres Correspondence set between source and target point cloud. +/// \param option DCReg solver options. +/// \return DCReg degeneracy and solver diagnostics. +DCRegDegeneracyAnalysis ComputeDCRegDegeneracyAnalysis( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + const DCRegOption &option = DCRegOption()); + +/// \brief Compute DCReg degeneracy diagnostics with a robust kernel. +/// +/// \param source Source point cloud. +/// \param target Target point cloud. It must have normals for a valid analysis. +/// \param corres Correspondence set between source and target point cloud. +/// \param option DCReg solver options. +/// \param kernel Robust kernel used to weight point-to-plane residuals. +/// \return DCReg degeneracy and solver diagnostics. +DCRegDegeneracyAnalysis ComputeDCRegDegeneracyAnalysis( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + const DCRegOption &option, + const RobustKernel &kernel); + +/// \brief Compute DCReg diagnostics with the standalone-compatible local-plane +/// residual and local body-frame SO(3) Jacobian. +/// +/// This helper does not require target normals. It fits a local plane from the +/// target k-nearest neighbors of each transformed source point, matching the +/// standalone DCReg examples. +DCRegDegeneracyAnalysis ComputeDCRegLocalDegeneracyAnalysis( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + double max_correspondence_distance, + const Eigen::Matrix4d &transformation, + const DCRegOption &option = DCRegOption()); } // namespace registration } // namespace pipelines diff --git a/cpp/pybind/pipelines/registration/registration.cpp b/cpp/pybind/pipelines/registration/registration.cpp index 0cc560df72f..08102076ca1 100644 --- a/cpp/pybind/pipelines/registration/registration.cpp +++ b/cpp/pybind/pipelines/registration/registration.cpp @@ -105,6 +105,23 @@ void pybind_registration_declarations(py::module &m) { te_p2l(m_registration, "TransformationEstimationPointToPlane", "Class to estimate a transformation for point to plane " "distance."); + py::class_ dcreg_option( + m_registration, "DCRegOption", + "Options for the DCReg degeneracy-aware point-to-plane ICP " + "estimator."); + py::class_ dcreg_analysis( + m_registration, "DCRegDegeneracyAnalysis", + "Diagnostic summary for the DCReg normal equation at one ICP " + "linearization."); + py::class_, + TransformationEstimationPointToPlane> + te_p2l_dcreg(m_registration, + "TransformationEstimationPointToPlaneDCReg", + "Degeneracy-aware point-to-plane transformation " + "estimation for ICP, based on the DCReg " + "Schur-complement preconditioning idea."); py::class_< TransformationEstimationForColoredICP, PyTransformationEstimation, @@ -306,6 +323,256 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``. .def_readwrite("kernel", &TransformationEstimationPointToPlane::kernel_, "Robust Kernel used in the Optimization"); + auto dcreg_option = static_cast>( + m_registration.attr("DCRegOption")); + py::detail::bind_copy_functions(dcreg_option); + dcreg_option + .def(py::init([](double degeneracy_condition_threshold, + double kappa_target, double pcg_tolerance, + int pcg_max_iteration, int local_plane_knn, + double local_plane_max_thickness, + double local_plane_weight_slope, + double local_plane_min_weight, + bool local_plane_use_weight_derivative, + double local_frame_convergence_rotation, + double local_frame_convergence_translation) { + return new DCRegOption( + degeneracy_condition_threshold, kappa_target, + pcg_tolerance, pcg_max_iteration, local_plane_knn, + local_plane_max_thickness, + local_plane_weight_slope, local_plane_min_weight, + local_plane_use_weight_derivative, + local_frame_convergence_rotation, + local_frame_convergence_translation); + }), + "degeneracy_condition_threshold"_a = 10.0, + "kappa_target"_a = 10.0, "pcg_tolerance"_a = 1e-6, + "pcg_max_iteration"_a = 10, "local_plane_knn"_a = 5, + "local_plane_max_thickness"_a = 0.2, + "local_plane_weight_slope"_a = 0.9, + "local_plane_min_weight"_a = 0.1, + "local_plane_use_weight_derivative"_a = true, + "local_frame_convergence_rotation"_a = 1e-5, + "local_frame_convergence_translation"_a = 1e-3) + .def_readwrite("degeneracy_condition_threshold", + &DCRegOption::degeneracy_condition_threshold_, + "Schur condition threshold for weak directions.") + .def_readwrite( + "kappa_target", &DCRegOption::kappa_target_, + "Target condition number used when clamping weak Schur " + "eigenvalues.") + .def_readwrite("pcg_tolerance", &DCRegOption::pcg_tolerance_, + "Relative residual threshold for the PCG solve.") + .def_readwrite("pcg_max_iteration", + &DCRegOption::pcg_max_iteration_, + "Maximum number of PCG iterations.") + .def_readwrite("local_plane_knn", &DCRegOption::local_plane_knn_, + "Number of target neighbors used by the " + "DCReg-local plane fit.") + .def_readwrite("local_plane_max_thickness", + &DCRegOption::local_plane_max_thickness_, + "Maximum accepted local plane residual on its " + "supporting neighbors.") + .def_readwrite("local_plane_weight_slope", + &DCRegOption::local_plane_weight_slope_, + "Slope for the original DCReg piecewise-linear " + "robust weight.") + .def_readwrite("local_plane_min_weight", + &DCRegOption::local_plane_min_weight_, + "Minimum accepted original DCReg robust weight.") + .def_readwrite("local_plane_use_weight_derivative", + &DCRegOption::local_plane_use_weight_derivative_, + "Whether to include the original robust-weight " + "derivative term.") + .def_readwrite("local_frame_convergence_rotation", + &DCRegOption::local_frame_convergence_rotation_, + "Local-frame SO(3) rotation-step convergence " + "threshold.") + .def_readwrite("local_frame_convergence_translation", + &DCRegOption::local_frame_convergence_translation_, + "Local-frame SO(3) translation-step convergence " + "threshold.") + .def("__repr__", [](const DCRegOption &option) { + return fmt::format( + "DCRegOption(" + "degeneracy_condition_threshold={:e}, " + "kappa_target={:e}, " + "pcg_tolerance={:e}, " + "pcg_max_iteration={:d}, " + "local_plane_knn={:d}, " + "local_plane_max_thickness={:e}, " + "local_plane_weight_slope={:e}, " + "local_plane_min_weight={:e}, " + "local_plane_use_weight_derivative={}, " + "local_frame_convergence_rotation={:e}, " + "local_frame_convergence_translation={:e})", + option.degeneracy_condition_threshold_, + option.kappa_target_, option.pcg_tolerance_, + option.pcg_max_iteration_, option.local_plane_knn_, + option.local_plane_max_thickness_, + option.local_plane_weight_slope_, + option.local_plane_min_weight_, + option.local_plane_use_weight_derivative_ ? "True" + : "False", + option.local_frame_convergence_rotation_, + option.local_frame_convergence_translation_); + }); + + auto dcreg_analysis = static_cast>( + m_registration.attr("DCRegDegeneracyAnalysis")); + py::detail::bind_default_constructor( + dcreg_analysis); + py::detail::bind_copy_functions(dcreg_analysis); + dcreg_analysis + .def_readwrite("has_correspondence", + &DCRegDegeneracyAnalysis::has_correspondence_, + "Whether the input correspondence set is non-empty.") + .def_readwrite("has_target_normals", + &DCRegDegeneracyAnalysis::has_target_normals_, + "Whether the target point cloud has normals.") + .def_readwrite("is_rank_deficient", + &DCRegDegeneracyAnalysis::is_rank_deficient_, + "Whether the 6x6 normal equation has a numerical " + "null space.") + .def_readwrite("schur_factorization_ok", + &DCRegDegeneracyAnalysis::schur_factorization_ok_, + "Whether rotational and translational Schur " + "complements were computed.") + .def_readwrite("is_degenerate", + &DCRegDegeneracyAnalysis::is_degenerate_, + "Whether DCReg detected degeneracy.") + .def_readwrite("condition_number_full", + &DCRegDegeneracyAnalysis::condition_number_full_, + "Condition number of the full 6x6 normal equation.") + .def_readwrite("condition_number_rotation", + &DCRegDegeneracyAnalysis::condition_number_rotation_, + "Condition number of the rotational Schur " + "complement.") + .def_readwrite( + "condition_number_translation", + &DCRegDegeneracyAnalysis::condition_number_translation_, + "Condition number of the translational Schur " + "complement.") + .def_readwrite( + "schur_eigenvalues_rotation", + &DCRegDegeneracyAnalysis::schur_eigenvalues_rotation_, + "Raw rotational Schur eigenvalues, or block-Hessian " + "fallback eigenvalues if Schur factorization failed.") + .def_readwrite( + "schur_eigenvalues_translation", + &DCRegDegeneracyAnalysis::schur_eigenvalues_translation_, + "Raw translational Schur eigenvalues, or block-Hessian " + "fallback eigenvalues if Schur factorization failed.") + .def_readwrite("axis_aligned_eigenvalues_rotation", + &DCRegDegeneracyAnalysis:: + axis_aligned_eigenvalues_rotation_, + "Rotational diagnostic eigenvalues aligned to " + "x/y/z.") + .def_readwrite("axis_aligned_eigenvalues_translation", + &DCRegDegeneracyAnalysis:: + axis_aligned_eigenvalues_translation_, + "Translational diagnostic eigenvalues aligned to " + "x/y/z.") + .def_readwrite( + "clamped_eigenvalues_rotation", + &DCRegDegeneracyAnalysis::clamped_eigenvalues_rotation_, + "Rotational eigenvalues used by the preconditioner.") + .def_readwrite( + "clamped_eigenvalues_translation", + &DCRegDegeneracyAnalysis::clamped_eigenvalues_translation_, + "Translational eigenvalues used by the preconditioner.") + .def_readwrite("weak_rotation_axes", + &DCRegDegeneracyAnalysis::weak_rotation_axes_, + "Weak rotational x/y/z axis flags.") + .def_readwrite("weak_translation_axes", + &DCRegDegeneracyAnalysis::weak_translation_axes_, + "Weak translational x/y/z axis flags.") + .def_readwrite( + "coordinate_frame", + &DCRegDegeneracyAnalysis::coordinate_frame_, + "Coordinate frame used by the weak-axis diagnostics. " + "For Open3D legacy ICP this is the target/world frame of " + "the left-multiplied SE(3) update.") + .def_readwrite( + "weak_rotation_axes_description", + &DCRegDegeneracyAnalysis::weak_rotation_axes_description_, + "Human-readable weak rotational axis list.") + .def_readwrite("weak_translation_axes_description", + &DCRegDegeneracyAnalysis:: + weak_translation_axes_description_, + "Human-readable weak translational axis list.") + .def_readwrite("degeneracy_description", + &DCRegDegeneracyAnalysis::degeneracy_description_, + "Human-readable DCReg degeneracy summary.") + .def_readwrite("solver_type", + &DCRegDegeneracyAnalysis::solver_type_, + "Solver path selected for the normal equation.") + .def_readwrite("pcg_converged", + &DCRegDegeneracyAnalysis::pcg_converged_, + "Whether the PCG path converged before fallback.") + .def_readwrite("pcg_iteration", + &DCRegDegeneracyAnalysis::pcg_iteration_, + "Number of PCG iterations executed.") + .def("__repr__", [](const DCRegDegeneracyAnalysis &analysis) { + return fmt::format( + "DCRegDegeneracyAnalysis(" + "is_degenerate={}, " + "is_rank_deficient={}, " + "condition_number_full={:e}, " + "condition_number_rotation={:e}, " + "condition_number_translation={:e}, " + "solver_type='{}')", + analysis.is_degenerate_ ? "True" : "False", + analysis.is_rank_deficient_ ? "True" : "False", + analysis.condition_number_full_, + analysis.condition_number_rotation_, + analysis.condition_number_translation_, + analysis.solver_type_); + }); + + auto te_p2l_dcreg = static_cast< + py::class_, + TransformationEstimationPointToPlane>>( + m_registration.attr("TransformationEstimationPointToPlaneDCReg")); + py::detail::bind_default_constructor< + TransformationEstimationPointToPlaneDCReg>(te_p2l_dcreg); + py::detail::bind_copy_functions( + te_p2l_dcreg); + te_p2l_dcreg + .def(py::init([](DCRegOption option) { + return new TransformationEstimationPointToPlaneDCReg( + std::move(option)); + }), + "option"_a) + .def(py::init([](std::shared_ptr kernel) { + return new TransformationEstimationPointToPlaneDCReg( + std::move(kernel)); + }), + "kernel"_a) + .def(py::init([](DCRegOption option, + std::shared_ptr kernel) { + return new TransformationEstimationPointToPlaneDCReg( + std::move(option), std::move(kernel)); + }), + "option"_a, "kernel"_a) + .def("__repr__", + [](const TransformationEstimationPointToPlaneDCReg &te) { + return fmt::format( + "TransformationEstimationPointToPlaneDCReg(" + "degeneracy_condition_threshold={:e}, " + "kappa_target={:e}, " + "pcg_tolerance={:e}, " + "pcg_max_iteration={:d})", + te.option_.degeneracy_condition_threshold_, + te.option_.kappa_target_, + te.option_.pcg_tolerance_, + te.option_.pcg_max_iteration_); + }) + .def_readwrite("option", + &TransformationEstimationPointToPlaneDCReg::option_, + "DCReg solver options."); // open3d.registration.TransformationEstimationForColoredICP : auto te_col = static_cast kernel) { + return ComputeDCRegDegeneracyAnalysis(source, target, corres, + option, *kernel); + }, + py::call_guard(), + "Compute DCReg degeneracy diagnostics for one point-to-plane ICP " + "linearization.", + "source"_a, "target"_a, "corres"_a, "option"_a = DCRegOption(), + "kernel"_a = std::make_shared()); + docstring::FunctionDocInject(m_registration, + "compute_dcreg_degeneracy_analysis", + map_shared_argument_docstrings); + + m_registration.def( + "compute_dcreg_local_degeneracy_analysis", + &ComputeDCRegLocalDegeneracyAnalysis, + py::call_guard(), + "Compute DCReg diagnostics using the standalone-compatible kNN " + "local-plane residual and local body-frame SO(3) Jacobian.", + "source"_a, "target"_a, "max_correspondence_distance"_a, + "transformation"_a, "option"_a = DCRegOption()); + docstring::FunctionDocInject(m_registration, + "compute_dcreg_local_degeneracy_analysis", + map_shared_argument_docstrings); + m_registration.def( "registration_icp", &RegistrationICP, py::call_guard(), @@ -669,6 +967,17 @@ must hold true for all edges.)"); docstring::FunctionDocInject(m_registration, "registration_icp", map_shared_argument_docstrings); + m_registration.def( + "registration_icp_dcreg_local", &RegistrationICPDCRegLocal, + py::call_guard(), + "Function for DCReg-compatible ICP registration with kNN " + "local-plane residuals and local body-frame SO(3) updates.", + "source"_a, "target"_a, "max_correspondence_distance"_a, + "init"_a = Eigen::Matrix4d::Identity(), "option"_a = DCRegOption(), + "criteria"_a = ICPConvergenceCriteria()); + docstring::FunctionDocInject(m_registration, "registration_icp_dcreg_local", + 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/tests/pipelines/registration/TransformationEstimation.cpp b/cpp/tests/pipelines/registration/TransformationEstimation.cpp index 96fb8333cb7..30c0ca191a6 100644 --- a/cpp/tests/pipelines/registration/TransformationEstimation.cpp +++ b/cpp/tests/pipelines/registration/TransformationEstimation.cpp @@ -5,10 +5,399 @@ // SPDX-License-Identifier: MIT // ---------------------------------------------------------------------------- +#include "open3d/pipelines/registration/TransformationEstimation.h" + +#include +#include +#include + +#include "open3d/geometry/PointCloud.h" +#include "open3d/pipelines/registration/Registration.h" +#include "open3d/utility/Eigen.h" #include "tests/Tests.h" namespace open3d { namespace tests { +namespace { + +geometry::PointCloud MakeBoxTarget() { + geometry::PointCloud target; + auto add = [&](const Eigen::Vector3d &point, + const Eigen::Vector3d &normal) { + target.points_.push_back(point); + target.normals_.push_back(normal); + }; + + const std::array kCoord = {0.0, 1.0}; + for (double y : kCoord) { + for (double z : kCoord) { + add(Eigen::Vector3d(0.0, y, z), Eigen::Vector3d(-1.0, 0.0, 0.0)); + add(Eigen::Vector3d(1.0, y, z), Eigen::Vector3d(1.0, 0.0, 0.0)); + } + } + for (double x : kCoord) { + for (double z : kCoord) { + add(Eigen::Vector3d(x, 0.0, z), Eigen::Vector3d(0.0, -1.0, 0.0)); + add(Eigen::Vector3d(x, 1.0, z), Eigen::Vector3d(0.0, 1.0, 0.0)); + } + } + for (double x : kCoord) { + for (double y : kCoord) { + add(Eigen::Vector3d(x, y, 0.0), Eigen::Vector3d(0.0, 0.0, -1.0)); + add(Eigen::Vector3d(x, y, 1.0), Eigen::Vector3d(0.0, 0.0, 1.0)); + } + } + return target; +} + +geometry::PointCloud MakeDegeneratePlaneTarget() { + geometry::PointCloud target; + for (int x = 0; x < 3; ++x) { + for (int y = 0; y < 3; ++y) { + target.points_.push_back(Eigen::Vector3d(x, y, 0.0)); + target.normals_.push_back(Eigen::Vector3d(0.0, 0.0, 1.0)); + } + } + return target; +} + +geometry::PointCloud MakeOffsetPlaneTarget() { + geometry::PointCloud target; + for (int x = 0; x < 5; ++x) { + for (int y = 0; y < 5; ++y) { + target.points_.push_back( + Eigen::Vector3d(0.2 * x + 0.5, 0.2 * y + 0.5, 1.0)); + } + } + return target; +} + +geometry::PointCloud MakeCylinderSideTarget() { + geometry::PointCloud target; + constexpr int kThetaCount = 24; + constexpr int kZCount = 7; + constexpr double kPi = 3.14159265358979323846; + constexpr double kZMin = -0.6; + constexpr double kZMax = 0.6; + + for (int theta_index = 0; theta_index < kThetaCount; ++theta_index) { + const double theta = 2.0 * kPi * theta_index / kThetaCount; + const Eigen::Vector3d normal(std::cos(theta), std::sin(theta), 0.0); + for (int z_index = 0; z_index < kZCount; ++z_index) { + const double alpha = static_cast(z_index) / (kZCount - 1); + const double z = kZMin + alpha * (kZMax - kZMin); + target.points_.push_back(Eigen::Vector3d(normal(0), normal(1), z)); + target.normals_.push_back(normal); + } + } + return target; +} + +geometry::PointCloud MakeAsymmetricBoxTarget() { + geometry::PointCloud target; + auto add = [&](const Eigen::Vector3d &point, + const Eigen::Vector3d &normal) { + target.points_.push_back(point); + target.normals_.push_back(normal); + }; + + const std::array kX = {0.0, 0.23, 0.51, 0.88, 1.30}; + const std::array kY = {0.0, 0.17, 0.39, 0.64, 0.80}; + const std::array kZ = {0.0, 0.19, 0.46, 0.73, 1.10}; + for (double y : kY) { + for (double z : kZ) { + add(Eigen::Vector3d(0.0, y, z), Eigen::Vector3d(-1.0, 0.0, 0.0)); + add(Eigen::Vector3d(1.3, y, z), Eigen::Vector3d(1.0, 0.0, 0.0)); + } + } + for (double x : kX) { + for (double z : kZ) { + add(Eigen::Vector3d(x, 0.0, z), Eigen::Vector3d(0.0, -1.0, 0.0)); + add(Eigen::Vector3d(x, 0.8, z), Eigen::Vector3d(0.0, 1.0, 0.0)); + } + } + for (double x : kX) { + for (double y : kY) { + add(Eigen::Vector3d(x, y, 0.0), Eigen::Vector3d(0.0, 0.0, -1.0)); + add(Eigen::Vector3d(x, y, 1.1), Eigen::Vector3d(0.0, 0.0, 1.0)); + } + } + return target; +} + +pipelines::registration::CorrespondenceSet MakeIdentityCorrespondences( + size_t size) { + pipelines::registration::CorrespondenceSet corres; + corres.reserve(size); + for (int i = 0; i < static_cast(size); ++i) { + corres.emplace_back(i, i); + } + return corres; +} + +Eigen::Matrix4d MakeSmallTransform() { + Eigen::Vector6d update; + update << 0.01, -0.02, 0.015, 0.03, -0.02, 0.04; + return utility::TransformVector6dToMatrix4d(update); +} + +Eigen::Matrix4d MakeSmallRegistrationTransform() { + Eigen::Vector6d update; + update << 0.02, -0.015, 0.01, 0.035, -0.025, 0.03; + return utility::TransformVector6dToMatrix4d(update); +} + +} // namespace + +TEST(TransformationEstimation, DCRegOptionConstructor) { + pipelines::registration::DCRegOption option; + EXPECT_DOUBLE_EQ(option.degeneracy_condition_threshold_, 10.0); + EXPECT_DOUBLE_EQ(option.kappa_target_, 10.0); + EXPECT_DOUBLE_EQ(option.pcg_tolerance_, 1e-6); + EXPECT_EQ(option.pcg_max_iteration_, 10); + EXPECT_EQ(option.local_plane_knn_, 5); + EXPECT_DOUBLE_EQ(option.local_plane_max_thickness_, 0.2); + EXPECT_DOUBLE_EQ(option.local_plane_weight_slope_, 0.9); + EXPECT_DOUBLE_EQ(option.local_plane_min_weight_, 0.1); + EXPECT_TRUE(option.local_plane_use_weight_derivative_); + EXPECT_DOUBLE_EQ(option.local_frame_convergence_rotation_, 1e-5); + EXPECT_DOUBLE_EQ(option.local_frame_convergence_translation_, 1e-3); + + pipelines::registration::DCRegOption custom(20.0, 15.0, 1e-8, 6, 7, 0.15, + 0.7, 0.05, false, 1e-4, 2e-3); + EXPECT_DOUBLE_EQ(custom.degeneracy_condition_threshold_, 20.0); + EXPECT_DOUBLE_EQ(custom.kappa_target_, 15.0); + EXPECT_DOUBLE_EQ(custom.pcg_tolerance_, 1e-8); + EXPECT_EQ(custom.pcg_max_iteration_, 6); + EXPECT_EQ(custom.local_plane_knn_, 7); + EXPECT_DOUBLE_EQ(custom.local_plane_max_thickness_, 0.15); + EXPECT_DOUBLE_EQ(custom.local_plane_weight_slope_, 0.7); + EXPECT_DOUBLE_EQ(custom.local_plane_min_weight_, 0.05); + EXPECT_FALSE(custom.local_plane_use_weight_derivative_); + EXPECT_DOUBLE_EQ(custom.local_frame_convergence_rotation_, 1e-4); + EXPECT_DOUBLE_EQ(custom.local_frame_convergence_translation_, 2e-3); +} + +TEST(TransformationEstimation, PointToPlaneDCRegEmptyCorrespondence) { + const geometry::PointCloud target = MakeBoxTarget(); + const geometry::PointCloud source = target; + pipelines::registration::TransformationEstimationPointToPlaneDCReg + estimation; + + const Eigen::Matrix4d update = + estimation.ComputeTransformation(source, target, {}); + EXPECT_TRUE(update.isIdentity(0.0)); +} + +TEST(TransformationEstimation, PointToPlaneDCRegRequiresTargetNormals) { + const geometry::PointCloud source = MakeBoxTarget(); + geometry::PointCloud target = source; + target.normals_.clear(); + pipelines::registration::TransformationEstimationPointToPlaneDCReg + estimation; + + EXPECT_ANY_THROW(estimation.InitializePointCloudsForTransformation( + source, target, 1.0)); + const Eigen::Matrix4d update = estimation.ComputeTransformation( + source, target, MakeIdentityCorrespondences(source.points_.size())); + EXPECT_TRUE(update.isIdentity(0.0)); +} + +TEST(TransformationEstimation, PointToPlaneDCRegMatchesPointToPlane) { + const geometry::PointCloud target = MakeBoxTarget(); + geometry::PointCloud source = target; + source.Transform(MakeSmallTransform().inverse()); + const auto corres = MakeIdentityCorrespondences(source.points_.size()); + + pipelines::registration::TransformationEstimationPointToPlane baseline; + pipelines::registration::TransformationEstimationPointToPlaneDCReg dcreg; + const Eigen::Matrix4d baseline_update = + baseline.ComputeTransformation(source, target, corres); + const Eigen::Matrix4d dcreg_update = + dcreg.ComputeTransformation(source, target, corres); + + EXPECT_TRUE(baseline_update.allFinite()); + EXPECT_TRUE(dcreg_update.allFinite()); + EXPECT_LT((baseline_update - dcreg_update).norm(), 1e-8); +} + +TEST(TransformationEstimation, PointToPlaneDCRegDegeneratePlaneIsFinite) { + const geometry::PointCloud target = MakeDegeneratePlaneTarget(); + geometry::PointCloud source = target; + source.Translate(Eigen::Vector3d(0.5, -0.25, -0.1)); + const auto corres = MakeIdentityCorrespondences(source.points_.size()); + + pipelines::registration::TransformationEstimationPointToPlaneDCReg dcreg; + const Eigen::Matrix4d update = + dcreg.ComputeTransformation(source, target, corres); + + EXPECT_TRUE(update.allFinite()); + const double translation_norm = update.block<3, 1>(0, 3).norm(); + EXPECT_LT(translation_norm, 1.0); + EXPECT_NEAR(update(2, 3), 0.1, 1e-8); +} + +TEST(TransformationEstimation, DCRegDegeneracyAnalysisWellConditioned) { + const geometry::PointCloud target = MakeAsymmetricBoxTarget(); + geometry::PointCloud source = target; + source.Transform(MakeSmallRegistrationTransform().inverse()); + const auto corres = MakeIdentityCorrespondences(source.points_.size()); + + const pipelines::registration::DCRegDegeneracyAnalysis analysis = + pipelines::registration::ComputeDCRegDegeneracyAnalysis( + source, target, corres); + + EXPECT_TRUE(analysis.has_correspondence_); + EXPECT_TRUE(analysis.has_target_normals_); + EXPECT_FALSE(analysis.is_rank_deficient_); + EXPECT_TRUE(analysis.schur_factorization_ok_); + EXPECT_TRUE(std::isfinite(analysis.condition_number_full_)); + EXPECT_TRUE(std::isfinite(analysis.condition_number_rotation_)); + EXPECT_TRUE(std::isfinite(analysis.condition_number_translation_)); + EXPECT_TRUE(analysis.schur_eigenvalues_rotation_.allFinite()); + EXPECT_TRUE(analysis.schur_eigenvalues_translation_.allFinite()); + EXPECT_NE(analysis.degeneracy_description_.find("target/world"), + std::string::npos); + EXPECT_NE(analysis.coordinate_frame_.find("left-multiplied SE(3)"), + std::string::npos); + EXPECT_NE(analysis.solver_type_, "invalid"); +} + +TEST(TransformationEstimation, PointToPlaneDCRegCylinderDegeneracyIsStable) { + const geometry::PointCloud target = MakeCylinderSideTarget(); + geometry::PointCloud source = target; + source.Translate(Eigen::Vector3d(-0.12, 0.05, -0.35)); + const auto corres = MakeIdentityCorrespondences(source.points_.size()); + + pipelines::registration::TransformationEstimationPointToPlaneDCReg dcreg; + const double initial_rmse = dcreg.ComputeRMSE(source, target, corres); + const Eigen::Matrix4d dcreg_update = + dcreg.ComputeTransformation(source, target, corres); + geometry::PointCloud dcreg_aligned = source; + dcreg_aligned.Transform(dcreg_update); + + EXPECT_TRUE(dcreg_update.allFinite()); + EXPECT_GT(initial_rmse, 0.05); + EXPECT_LT(dcreg.ComputeRMSE(dcreg_aligned, target, corres), 1e-8); + EXPECT_NEAR(dcreg_update(0, 3), 0.12, 1e-8); + EXPECT_NEAR(dcreg_update(1, 3), -0.05, 1e-8); + EXPECT_NEAR(dcreg_update(2, 3), 0.0, 1e-8); + EXPECT_LT((dcreg_update.block<3, 3>(0, 0) - Eigen::Matrix3d::Identity()) + .norm(), + 1e-8); +} + +TEST(TransformationEstimation, DCRegDegeneracyAnalysisCylinder) { + const geometry::PointCloud target = MakeCylinderSideTarget(); + geometry::PointCloud source = target; + source.Translate(Eigen::Vector3d(-0.12, 0.05, -0.35)); + const auto corres = MakeIdentityCorrespondences(source.points_.size()); + + const pipelines::registration::DCRegDegeneracyAnalysis analysis = + pipelines::registration::ComputeDCRegDegeneracyAnalysis( + source, target, corres); + + EXPECT_TRUE(analysis.has_correspondence_); + EXPECT_TRUE(analysis.has_target_normals_); + EXPECT_TRUE(analysis.is_rank_deficient_); + EXPECT_TRUE(analysis.is_degenerate_); + EXPECT_FALSE(analysis.schur_factorization_ok_); + EXPECT_TRUE(analysis.schur_eigenvalues_rotation_.allFinite()); + EXPECT_TRUE(analysis.schur_eigenvalues_translation_.allFinite()); + EXPECT_TRUE(analysis.axis_aligned_eigenvalues_rotation_.allFinite()); + EXPECT_TRUE(analysis.axis_aligned_eigenvalues_translation_.allFinite()); + EXPECT_TRUE(std::isfinite(analysis.condition_number_full_)); + EXPECT_TRUE(std::isfinite(analysis.condition_number_rotation_)); + EXPECT_TRUE(std::isfinite(analysis.condition_number_translation_)); + EXPECT_EQ(analysis.weak_rotation_axes_(0), 0); + EXPECT_EQ(analysis.weak_rotation_axes_(1), 0); + EXPECT_EQ(analysis.weak_rotation_axes_(2), 1); + EXPECT_EQ(analysis.weak_translation_axes_(0), 0); + EXPECT_EQ(analysis.weak_translation_axes_(1), 0); + EXPECT_EQ(analysis.weak_translation_axes_(2), 1); + EXPECT_NEAR(analysis.axis_aligned_eigenvalues_translation_(2), 0.0, 1e-10); + EXPECT_EQ(analysis.weak_rotation_axes_description_, "z"); + EXPECT_EQ(analysis.weak_translation_axes_description_, "z"); + EXPECT_NE(analysis.degeneracy_description_.find("target/world"), + std::string::npos); + EXPECT_NE(analysis.coordinate_frame_.find("left-multiplied SE(3)"), + std::string::npos); + EXPECT_EQ(analysis.solver_type_, "minimum_norm"); +} + +TEST(TransformationEstimation, DCRegLocalRegistrationUsesLocalPlane) { + const geometry::PointCloud target = MakeOffsetPlaneTarget(); + geometry::PointCloud source = target; + source.Translate(Eigen::Vector3d(0.0, 0.0, -0.1)); + + pipelines::registration::DCRegOption option; + option.local_frame_convergence_rotation_ = 1e-8; + option.local_frame_convergence_translation_ = 1e-8; + const pipelines::registration::ICPConvergenceCriteria criteria(1e-9, 1e-9, + 10); + const pipelines::registration::RegistrationResult result = + pipelines::registration::RegistrationICPDCRegLocal( + source, target, 0.5, Eigen::Matrix4d::Identity(), option, + criteria); + + EXPECT_TRUE(result.transformation_.allFinite()); + EXPECT_NEAR(result.transformation_(0, 3), 0.0, 1e-8); + EXPECT_NEAR(result.transformation_(1, 3), 0.0, 1e-8); + EXPECT_NEAR(result.transformation_(2, 3), 0.1, 1e-8); + EXPECT_NEAR(result.fitness_, 1.0, 1e-12); + EXPECT_NEAR(result.inlier_rmse_, 0.0, 1e-12); +} + +TEST(TransformationEstimation, DCRegLocalDegeneracyAnalysisUsesLocalFrame) { + const geometry::PointCloud target = MakeOffsetPlaneTarget(); + geometry::PointCloud source = target; + source.Translate(Eigen::Vector3d(0.0, 0.0, -0.1)); + + const pipelines::registration::DCRegDegeneracyAnalysis analysis = + pipelines::registration::ComputeDCRegLocalDegeneracyAnalysis( + source, target, 0.5, Eigen::Matrix4d::Identity()); + + EXPECT_TRUE(analysis.has_correspondence_); + EXPECT_TRUE(analysis.has_target_normals_); + EXPECT_TRUE(analysis.is_degenerate_); + EXPECT_EQ(analysis.weak_rotation_axes_description_, "x, y, z"); + EXPECT_EQ(analysis.weak_translation_axes_description_, "x, y, z"); + EXPECT_NE(analysis.coordinate_frame_.find("local body frame"), + std::string::npos); + EXPECT_NE(analysis.degeneracy_description_.find("local-plane"), + std::string::npos); + EXPECT_EQ(analysis.solver_type_, "qr_fallback"); +} + +TEST(TransformationEstimation, + PointToPlaneDCRegRegistrationICPMatchesBaseline) { + const geometry::PointCloud target = MakeAsymmetricBoxTarget(); + const Eigen::Matrix4d expected = MakeSmallRegistrationTransform(); + geometry::PointCloud source = target; + source.Transform(expected.inverse()); + + const pipelines::registration::ICPConvergenceCriteria criteria(1e-9, 1e-9, + 20); + const pipelines::registration::RegistrationResult baseline = + pipelines::registration::RegistrationICP( + source, target, 0.2, Eigen::Matrix4d::Identity(), + pipelines::registration:: + TransformationEstimationPointToPlane(), + criteria); + const pipelines::registration::RegistrationResult dcreg = + pipelines::registration::RegistrationICP( + source, target, 0.2, Eigen::Matrix4d::Identity(), + pipelines::registration:: + TransformationEstimationPointToPlaneDCReg(), + criteria); + + EXPECT_TRUE(baseline.transformation_.allFinite()); + EXPECT_TRUE(dcreg.transformation_.allFinite()); + EXPECT_NEAR(baseline.fitness_, dcreg.fitness_, 1e-12); + EXPECT_NEAR(baseline.inlier_rmse_, dcreg.inlier_rmse_, 1e-12); + EXPECT_LT((baseline.transformation_ - dcreg.transformation_).norm(), 1e-8); + EXPECT_LT((dcreg.transformation_ - expected).norm(), 1e-8); +} TEST(TransformationEstimation, DISABLED_Constructor) { NotImplemented(); } diff --git a/docs/tutorial/pipelines/dcreg_icp.rst b/docs/tutorial/pipelines/dcreg_icp.rst new file mode 100644 index 00000000000..b97fb03cd3e --- /dev/null +++ b/docs/tutorial/pipelines/dcreg_icp.rst @@ -0,0 +1,239 @@ +Degeneracy-Aware ICP +==================== + +Open3D provides ``TransformationEstimationPointToPlaneDCReg`` for legacy CPU +ICP registration in scenes where point-to-plane ICP may contain weak geometric +directions. The implementation is based on DCReg [Hu2025]_, which detects, +characterizes, and mitigates ill-conditioning through Schur-complement +subspaces and preconditioned conjugate gradient. + +``TransformationEstimationPointToPlaneDCReg`` uses the same correspondences, +target normals, robust kernels, and ICP convergence criteria as +``TransformationEstimationPointToPlane``. The difference is the 6D linear solve: +DCReg detects weak directions from the Schur complements of the normal equation, +builds a block preconditioner, and falls back to a dense solve if the +preconditioned solve is not reliable. + +The pose update follows Open3D's legacy ICP perturbation convention. At each +iteration, ``registration_icp`` has already transformed the source by the +current pose, the estimator solves a target/world-frame point-to-plane update, +and Open3D applies it as ``transformation = update @ transformation``. + +The target point cloud must have normals, just like ordinary point-to-plane +ICP. + +.. code-block:: python + + import copy + import numpy as np + import open3d as o3d + + source = o3d.io.read_point_cloud("source.pcd") + target = o3d.io.read_point_cloud("target.pcd") + target.estimate_normals() + + option = o3d.pipelines.registration.DCRegOption( + degeneracy_condition_threshold=10.0, + kappa_target=10.0, + pcg_tolerance=1e-6, + pcg_max_iteration=10, + ) + estimation = ( + o3d.pipelines.registration.TransformationEstimationPointToPlaneDCReg( + option + ) + ) + + result = o3d.pipelines.registration.registration_icp( + source, + target, + max_correspondence_distance=0.05, + init=np.eye(4), + estimation_method=estimation, + criteria=o3d.pipelines.registration.ICPConvergenceCriteria( + max_iteration=50 + ), + ) + + print(result.transformation) + +``DCRegOption`` exposes the Schur condition threshold, the target condition +number after weak-eigenvalue clamping, and the PCG stopping parameters. The +defaults are intended to be conservative for the same data scale used by +ordinary point-to-plane ICP. + +Standalone-Compatible Local ICP +-------------------------------- + +For reproducing the standalone DCReg examples, Open3D also provides +``registration_icp_dcreg_local``. This is a separate ICP loop because the +standalone path needs the original source point, the current pose state, and +the current rotation for the local-frame Jacobian: + +.. code-block:: python + + option = o3d.pipelines.registration.DCRegOption( + local_plane_knn=5, + local_plane_max_thickness=0.2, + local_plane_weight_slope=0.9, + local_plane_min_weight=0.1, + local_plane_use_weight_derivative=True, + local_frame_convergence_rotation=1e-5, + local_frame_convergence_translation=1e-3, + ) + result = o3d.pipelines.registration.registration_icp_dcreg_local( + source, + target, + max_correspondence_distance=0.5, + init=init, + option=option, + criteria=o3d.pipelines.registration.ICPConvergenceCriteria( + max_iteration=30 + ), + ) + analysis = ( + o3d.pipelines.registration.compute_dcreg_local_degeneracy_analysis( + source, + target, + max_correspondence_distance=0.5, + transformation=result.transformation, + option=option, + ) + ) + + print(result.transformation) + print("DCReg degeneracy description:") + print(analysis.degeneracy_description) + print("weak rotation axes:", analysis.weak_rotation_axes_description) + print("weak translation axes:", analysis.weak_translation_axes_description) + print("condition number rotation:", analysis.condition_number_rotation) + print("condition number translation:", analysis.condition_number_translation) + print("coordinate frame:", analysis.coordinate_frame) + +This compatibility path fits a local plane from each transformed source point's +target k-nearest neighbors and uses the standalone SO(3) update +``R <- R Exp(omega), t <- t + R v``. It does not require target normals. + +Degeneracy Diagnostics +---------------------- + +``registration_icp`` still returns the standard Open3D ``RegistrationResult``. +For DCReg-specific diagnostics, call +``compute_dcreg_degeneracy_analysis`` on a point cloud pair and a +correspondence set. A typical pattern is to evaluate the final ICP +linearization after applying ``result.transformation``: + +.. code-block:: python + + source_aligned = copy.deepcopy(source) + source_aligned.transform(result.transformation) + + analysis = o3d.pipelines.registration.compute_dcreg_degeneracy_analysis( + source_aligned, + target, + result.correspondence_set, + option, + ) + + print(analysis.is_degenerate) + print(analysis.is_rank_deficient) + print(analysis.condition_number_full) + print(analysis.condition_number_rotation) + print(analysis.condition_number_translation) + print(analysis.weak_rotation_axes) + print(analysis.weak_translation_axes) + print(analysis.coordinate_frame) + print(analysis.degeneracy_description) + print(analysis.solver_type) + +For the standalone-compatible local-plane path, use +``compute_dcreg_local_degeneracy_analysis`` with the same source, target, +maximum correspondence distance, transformation, and ``DCRegOption``: + +.. code-block:: python + + analysis = ( + o3d.pipelines.registration.compute_dcreg_local_degeneracy_analysis( + source, + target, + max_correspondence_distance=0.5, + transformation=result.transformation, + option=option, + ) + ) + + print(analysis.degeneracy_description) + print(analysis.weak_rotation_axes_description) + print(analysis.weak_translation_axes_description) + print(analysis.condition_number_rotation) + print(analysis.condition_number_translation) + print(analysis.coordinate_frame) + +The eigenvalue and weak-axis fields use the physical x/y/z axis order after +DCReg aligns the Schur eigenvectors to coordinate axes. For the Open3D legacy +ICP estimator, these axes are the target/world-frame incremental axes used by +Open3D's left-multiplied SE(3) point-to-plane linearization. They are not the +local body-frame translation axes used by the standalone DCReg SO(3) examples. +The weak-axis +arrays use ``1`` for weak and ``0`` for not weak. If the estimator uses a robust +kernel, pass the same kernel to ``compute_dcreg_degeneracy_analysis`` so the +diagnostic normal equation matches the ICP linearization. + +For ``registration_icp_dcreg_local`` and +``compute_dcreg_local_degeneracy_analysis``, the weak translation axes are the +standalone DCReg local body-frame axes. + +For exact rank-deficient cases, the Schur complements may be impossible to form +because one Hessian block is singular. In the Open3D-native point-to-plane +estimator, ``schur_factorization_ok`` remains ``False``, but the diagnostic +function fills the eigenvalue, condition-number, and weak-axis fields from a +block-Hessian eigensolver fallback. This is intended for interpretation only; +the transformation update still uses the minimum-norm rank-deficient solve. The +standalone-compatible local-plane ICP follows the original DCReg behavior and +falls back to the QR solve when Schur/PCG is not reliable. + +Implementation Notes +-------------------- + +This Open3D implementation is intentionally smaller than the original DCReg +research code. It is designed to fit the existing +``open3d.pipelines.registration`` API rather than to reproduce the full +standalone experiment system. + +The main differences are: + +* Open3D owns the ICP loop, correspondence search, robust kernel, point cloud + containers, and ``RegistrationResult``. DCReg is implemented only as a + ``TransformationEstimation`` method. +* The estimator returns Open3D-style left-multiplied SE(3) increments. This + intentionally follows ``TransformationEstimationPointToPlane`` rather than + the standalone DCReg examples' local-frame update convention. +* For ``TransformationEstimationPointToPlaneDCReg``, target normals must + already be available, as in ordinary point-to-plane ICP. +* ``registration_icp_dcreg_local`` is the standalone-compatible exception: it + ports the original kNN local-plane residual, piecewise-linear residual + weighting, and local body-frame SO(3) update for example reproduction. +* Weak-axis diagnostics describe the Open3D point-to-plane normal equation. + They should not be compared axis-by-axis with standalone DCReg parking-lot + logs unless the same normals, correspondences, robust weights, and local + body-frame Jacobian are used. +* The eigenvalue clamping is applied only inside the preconditioner. The + original point-to-plane least-squares objective is not modified. +* Exact rank-deficient systems use a minimum-norm fallback before the Schur/PCG + path. This keeps pure-null motions, such as translation along a cylinder axis, + from receiving arbitrary updates. +* Degeneracy diagnostics are available through + ``compute_dcreg_degeneracy_analysis`` instead of being added to the generic + ``RegistrationResult``. +* Dataset runners, YAML presets, parking-lot-specific preprocessing, detailed + experiment logs, and paper benchmark scripts are not included. +* This first integration targets the legacy CPU API. Tensor and CUDA + registration APIs are not implemented here. + +References +---------- + +.. [Hu2025] Xiangcheng Hu, Xieyuanli Chen, Mingkai Jia, Jin Wu, Ping Tan, + Steven L. Waslander, "DCReg: Decoupled Characterization for Efficient + Degenerate LiDAR Registration," arXiv:2509.06285, 2025. + https://arxiv.org/abs/2509.06285 diff --git a/docs/tutorial/pipelines/index.rst b/docs/tutorial/pipelines/index.rst index a02eaa9fe44..e32d710e1b2 100644 --- a/docs/tutorial/pipelines/index.rst +++ b/docs/tutorial/pipelines/index.rst @@ -4,6 +4,7 @@ Pipelines .. toctree:: icp_registration + dcreg_icp generalized_icp robust_kernels colored_pointcloud_registration diff --git a/python/test/test_registration_dcreg.py b/python/test/test_registration_dcreg.py new file mode 100644 index 00000000000..53e5429ae12 --- /dev/null +++ b/python/test/test_registration_dcreg.py @@ -0,0 +1,332 @@ +# ---------------------------------------------------------------------------- +# - Open3D: www.open3d.org - +# ---------------------------------------------------------------------------- +# Copyright (c) 2018-2024 www.open3d.org +# SPDX-License-Identifier: MIT +# ---------------------------------------------------------------------------- + +import copy + +import numpy as np +import open3d as o3d + + +def _make_box_clouds(): + points = [] + normals = [] + + def add(point, normal): + points.append(point) + normals.append(normal) + + for y in [0.0, 1.0]: + for z in [0.0, 1.0]: + add([0.0, y, z], [-1.0, 0.0, 0.0]) + add([1.0, y, z], [1.0, 0.0, 0.0]) + for x in [0.0, 1.0]: + for z in [0.0, 1.0]: + add([x, 0.0, z], [0.0, -1.0, 0.0]) + add([x, 1.0, z], [0.0, 1.0, 0.0]) + for x in [0.0, 1.0]: + for y in [0.0, 1.0]: + add([x, y, 0.0], [0.0, 0.0, -1.0]) + add([x, y, 1.0], [0.0, 0.0, 1.0]) + + target = o3d.geometry.PointCloud() + target.points = o3d.utility.Vector3dVector(np.asarray(points)) + target.normals = o3d.utility.Vector3dVector(np.asarray(normals)) + + source_to_target = np.eye(4) + source_to_target[:3, 3] = [0.03, -0.02, 0.04] + source = copy.deepcopy(target) + source.transform(np.linalg.inv(source_to_target)) + return source, target + + +def _make_asymmetric_box_clouds(): + points = [] + normals = [] + xs = [0.0, 0.23, 0.51, 0.88, 1.30] + ys = [0.0, 0.17, 0.39, 0.64, 0.80] + zs = [0.0, 0.19, 0.46, 0.73, 1.10] + + def add(point, normal): + points.append(point) + normals.append(normal) + + for y in ys: + for z in zs: + add([0.0, y, z], [-1.0, 0.0, 0.0]) + add([1.30, y, z], [1.0, 0.0, 0.0]) + for x in xs: + for z in zs: + add([x, 0.0, z], [0.0, -1.0, 0.0]) + add([x, 0.80, z], [0.0, 1.0, 0.0]) + for x in xs: + for y in ys: + add([x, y, 0.0], [0.0, 0.0, -1.0]) + add([x, y, 1.10], [0.0, 0.0, 1.0]) + + target = o3d.geometry.PointCloud() + target.points = o3d.utility.Vector3dVector(np.asarray(points)) + target.normals = o3d.utility.Vector3dVector(np.asarray(normals)) + + source_to_target = np.eye(4) + source_to_target[:3, :3] = o3d.geometry.get_rotation_matrix_from_xyz( + [0.02, -0.015, 0.01]) + source_to_target[:3, 3] = [0.035, -0.025, 0.03] + source = copy.deepcopy(target) + source.transform(np.linalg.inv(source_to_target)) + return source, target, source_to_target + + +def _make_cylinder_side_clouds(): + points = [] + normals = [] + for theta in np.linspace(0.0, 2.0 * np.pi, 24, endpoint=False): + normal = [np.cos(theta), np.sin(theta), 0.0] + for z in np.linspace(-0.6, 0.6, 7): + points.append([normal[0], normal[1], z]) + normals.append(normal) + + target = o3d.geometry.PointCloud() + target.points = o3d.utility.Vector3dVector(np.asarray(points)) + target.normals = o3d.utility.Vector3dVector(np.asarray(normals)) + + source = copy.deepcopy(target) + source.translate([-0.12, 0.05, -0.35]) + return source, target + + +def _make_offset_plane_clouds(): + points = [] + for x in range(5): + for y in range(5): + points.append([0.2 * x + 0.5, 0.2 * y + 0.5, 1.0]) + + target = o3d.geometry.PointCloud() + target.points = o3d.utility.Vector3dVector(np.asarray(points)) + + source = copy.deepcopy(target) + source.translate([0.0, 0.0, -0.1]) + return source, target + + +def _identity_correspondences(size): + return o3d.utility.Vector2iVector( + np.asarray([[i, i] for i in range(size)], dtype=np.int32)) + + +def test_dcreg_option_constructor_and_repr(): + option = o3d.pipelines.registration.DCRegOption() + assert option.degeneracy_condition_threshold == 10.0 + assert option.kappa_target == 10.0 + assert option.pcg_tolerance == 1e-6 + assert option.pcg_max_iteration == 10 + assert option.local_plane_knn == 5 + assert option.local_plane_max_thickness == 0.2 + assert option.local_plane_weight_slope == 0.9 + assert option.local_plane_min_weight == 0.1 + assert option.local_plane_use_weight_derivative + assert option.local_frame_convergence_rotation == 1e-5 + assert option.local_frame_convergence_translation == 1e-3 + + custom = o3d.pipelines.registration.DCRegOption(20.0, 15.0, 1e-8, 6, 7, + 0.15, 0.7, 0.05, False, + 1e-4, 2e-3) + assert "DCRegOption" in repr(custom) + assert custom.degeneracy_condition_threshold == 20.0 + assert custom.kappa_target == 15.0 + assert custom.pcg_tolerance == 1e-8 + assert custom.pcg_max_iteration == 6 + assert custom.local_plane_knn == 7 + assert custom.local_plane_max_thickness == 0.15 + assert custom.local_plane_weight_slope == 0.7 + assert custom.local_plane_min_weight == 0.05 + assert not custom.local_plane_use_weight_derivative + assert custom.local_frame_convergence_rotation == 1e-4 + assert custom.local_frame_convergence_translation == 2e-3 + + +def test_dcreg_compute_transformation_matches_point_to_plane(): + source, target = _make_box_clouds() + corres = _identity_correspondences(len(source.points)) + + baseline = o3d.pipelines.registration.TransformationEstimationPointToPlane( + ).compute_transformation(source, target, corres) + dcreg = o3d.pipelines.registration.TransformationEstimationPointToPlaneDCReg( + ).compute_transformation(source, target, corres) + + assert "TransformationEstimationPointToPlaneDCReg" in repr( + o3d.pipelines.registration.TransformationEstimationPointToPlaneDCReg()) + np.testing.assert_allclose(dcreg, baseline, rtol=1e-8, atol=1e-8) + + +def test_dcreg_degeneracy_analysis_on_normal_geometry(): + source, target, _ = _make_asymmetric_box_clouds() + corres = _identity_correspondences(len(source.points)) + + analysis = o3d.pipelines.registration.compute_dcreg_degeneracy_analysis( + source, target, corres) + + assert "DCRegDegeneracyAnalysis" in repr(analysis) + assert analysis.has_correspondence + assert analysis.has_target_normals + assert not analysis.is_rank_deficient + assert analysis.schur_factorization_ok + assert np.isfinite(analysis.condition_number_full) + assert np.isfinite(analysis.condition_number_rotation) + assert np.isfinite(analysis.condition_number_translation) + assert np.isfinite(analysis.schur_eigenvalues_rotation).all() + assert np.isfinite(analysis.schur_eigenvalues_translation).all() + assert "target/world" in analysis.coordinate_frame + assert "left-multiplied SE(3)" in analysis.coordinate_frame + assert "target/world" in analysis.degeneracy_description + assert analysis.solver_type != "invalid" + + +def test_dcreg_cylinder_degeneracy_is_stable(): + source, target = _make_cylinder_side_clouds() + corres = _identity_correspondences(len(source.points)) + + estimation = o3d.pipelines.registration.TransformationEstimationPointToPlane( + ) + initial_rmse = estimation.compute_rmse(source, target, corres) + dcreg = o3d.pipelines.registration.TransformationEstimationPointToPlaneDCReg( + ).compute_transformation(source, target, corres) + dcreg_aligned = copy.deepcopy(source) + dcreg_aligned.transform(dcreg) + + assert np.isfinite(dcreg).all() + assert initial_rmse > 0.05 + np.testing.assert_allclose(estimation.compute_rmse(dcreg_aligned, target, + corres), + 0.0, + atol=1e-8) + np.testing.assert_allclose(dcreg[:2, 3], [0.12, -0.05], atol=1e-8) + np.testing.assert_allclose(dcreg[2, 3], 0.0, atol=1e-8) + np.testing.assert_allclose(dcreg[:3, :3], np.eye(3), atol=1e-8) + + +def test_dcreg_degeneracy_analysis_on_cylinder(): + source, target = _make_cylinder_side_clouds() + corres = _identity_correspondences(len(source.points)) + + analysis = o3d.pipelines.registration.compute_dcreg_degeneracy_analysis( + source, target, corres) + + assert analysis.has_correspondence + assert analysis.has_target_normals + assert analysis.is_rank_deficient + assert analysis.is_degenerate + assert not analysis.schur_factorization_ok + assert np.isfinite(analysis.schur_eigenvalues_rotation).all() + assert np.isfinite(analysis.schur_eigenvalues_translation).all() + assert np.isfinite(analysis.axis_aligned_eigenvalues_rotation).all() + assert np.isfinite(analysis.axis_aligned_eigenvalues_translation).all() + assert np.isfinite(analysis.condition_number_full) + assert np.isfinite(analysis.condition_number_rotation) + assert np.isfinite(analysis.condition_number_translation) + np.testing.assert_array_equal(analysis.weak_rotation_axes, [0, 0, 1]) + np.testing.assert_array_equal(analysis.weak_translation_axes, [0, 0, 1]) + np.testing.assert_allclose(analysis.axis_aligned_eigenvalues_translation[2], + 0.0, + atol=1e-10) + assert analysis.weak_rotation_axes_description == "z" + assert analysis.weak_translation_axes_description == "z" + assert "target/world" in analysis.degeneracy_description + assert "left-multiplied SE(3)" in analysis.coordinate_frame + assert analysis.solver_type == "minimum_norm" + + +def test_dcreg_local_registration_uses_local_plane_without_normals(): + source, target = _make_offset_plane_clouds() + option = o3d.pipelines.registration.DCRegOption( + local_frame_convergence_rotation=1e-8, + local_frame_convergence_translation=1e-8) + criteria = o3d.pipelines.registration.ICPConvergenceCriteria( + relative_fitness=1e-9, relative_rmse=1e-9, max_iteration=10) + + result = o3d.pipelines.registration.registration_icp_dcreg_local( + source, target, 0.5, np.eye(4), option, criteria) + + assert np.isfinite(result.transformation).all() + np.testing.assert_allclose(result.transformation[:3, 3], [0.0, 0.0, 0.1], + atol=1e-8) + np.testing.assert_allclose(result.fitness, 1.0, atol=1e-12) + np.testing.assert_allclose(result.inlier_rmse, 0.0, atol=1e-12) + + +def test_dcreg_local_registration_then_prints_degeneracy_description(capsys): + source, target = _make_offset_plane_clouds() + option = o3d.pipelines.registration.DCRegOption( + local_frame_convergence_rotation=1e-8, + local_frame_convergence_translation=1e-8) + criteria = o3d.pipelines.registration.ICPConvergenceCriteria( + relative_fitness=1e-9, relative_rmse=1e-9, max_iteration=10) + + result = o3d.pipelines.registration.registration_icp_dcreg_local( + source, target, 0.5, np.eye(4), option, criteria) + analysis = o3d.pipelines.registration.compute_dcreg_local_degeneracy_analysis( + source, target, 0.5, result.transformation, option) + + print("DCReg degeneracy description:") + print(analysis.degeneracy_description) + print("weak rotation axes:", analysis.weak_rotation_axes_description) + print("weak translation axes:", analysis.weak_translation_axes_description) + print("condition number rotation:", analysis.condition_number_rotation) + print("condition number translation:", + analysis.condition_number_translation) + print("coordinate frame:", analysis.coordinate_frame) + + captured = capsys.readouterr() + assert "DCReg degeneracy description" in captured.out + assert "local-plane" in captured.out + assert "local body frame" in captured.out + assert "condition number translation" in captured.out + + +def test_dcreg_local_degeneracy_analysis_uses_local_frame(): + source, target = _make_offset_plane_clouds() + + analysis = o3d.pipelines.registration.compute_dcreg_local_degeneracy_analysis( + source, target, 0.5, np.eye(4)) + + assert analysis.has_correspondence + assert analysis.has_target_normals + assert analysis.is_degenerate + assert analysis.weak_rotation_axes_description == "x, y, z" + assert analysis.weak_translation_axes_description == "x, y, z" + assert "local body frame" in analysis.coordinate_frame + assert "local-plane" in analysis.degeneracy_description + assert analysis.solver_type == "qr_fallback" + + +def test_dcreg_registration_icp_matches_point_to_plane_on_normal_geometry(): + source, target, expected = _make_asymmetric_box_clouds() + threshold = 0.2 + criteria = o3d.pipelines.registration.ICPConvergenceCriteria( + relative_fitness=1e-9, relative_rmse=1e-9, max_iteration=20) + + baseline = o3d.pipelines.registration.registration_icp( + source, target, threshold, np.eye(4), + o3d.pipelines.registration.TransformationEstimationPointToPlane(), + criteria) + dcreg = o3d.pipelines.registration.registration_icp( + source, target, threshold, np.eye(4), + o3d.pipelines.registration.TransformationEstimationPointToPlaneDCReg(), + criteria) + + assert np.isfinite(dcreg.transformation).all() + np.testing.assert_allclose(dcreg.fitness, baseline.fitness, atol=1e-12) + np.testing.assert_allclose(dcreg.inlier_rmse, + baseline.inlier_rmse, + atol=1e-12) + np.testing.assert_allclose(dcreg.transformation, + baseline.transformation, + rtol=1e-8, + atol=1e-8) + np.testing.assert_allclose(dcreg.transformation, + expected, + rtol=1e-8, + atol=1e-8)