diff --git a/CHANGELOG.md b/CHANGELOG.md index 01c50d9ea44..3d1b119fb0f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,7 @@ - Fix macOS arm64 builds, add CI runner for macOS arm64 (PR #6695) - Fix KDTreeFlann possibly using a dangling pointer instead of internal storage and simplified its members (PR #6734) - Fix RANSAC early stop if no inliers in a specific iteration (PR #6789) +- Add 3D Normal Distributions Transform registration with C++ and Python APIs. - Fix segmentation fault (infinite recursion) of DetectPlanarPatches if multiple points have same coordinates (PR #6794) - `TriangleMesh`'s `+=` operator appends UVs regardless of the presence of existing features (PR #6728) - Fix build with fmt v10.2.0 (#6783) diff --git a/cpp/open3d/Open3D.h.in b/cpp/open3d/Open3D.h.in index 5c9a5b5c60a..23fc26200e1 100644 --- a/cpp/open3d/Open3D.h.in +++ b/cpp/open3d/Open3D.h.in @@ -63,6 +63,7 @@ #include "open3d/pipelines/registration/Feature.h" #include "open3d/pipelines/registration/GeneralizedICP.h" #include "open3d/pipelines/registration/GlobalOptimization.h" +#include "open3d/pipelines/registration/NormalDistributionsTransform.h" #include "open3d/pipelines/registration/Registration.h" #include "open3d/pipelines/registration/TransformationEstimation.h" #include "open3d/t/geometry/Geometry.h" diff --git a/cpp/open3d/pipelines/CMakeLists.txt b/cpp/open3d/pipelines/CMakeLists.txt index 7173b64b515..b414292cdb0 100644 --- a/cpp/open3d/pipelines/CMakeLists.txt +++ b/cpp/open3d/pipelines/CMakeLists.txt @@ -24,6 +24,7 @@ target_sources(pipelines PRIVATE registration/Feature.cpp registration/GeneralizedICP.cpp registration/GlobalOptimization.cpp + registration/NormalDistributionsTransform.cpp registration/PoseGraph.cpp registration/Registration.cpp registration/RobustKernel.cpp diff --git a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp new file mode 100644 index 00000000000..8b78d544b3a --- /dev/null +++ b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp @@ -0,0 +1,453 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// Copyright (c) 2018-2024 www.open3d.org +// SPDX-License-Identifier: MIT +// ---------------------------------------------------------------------------- + +#include "open3d/pipelines/registration/NormalDistributionsTransform.h" + +// This implementation follows the same 3D NDT registration formulation used in +// https://github.com/gaoxiang12/slam_in_autonomous_driving/blob/master/src/ch7/ndt_3d.cc: +// target voxel Gaussian modeling, center/six-neighbor voxel residuals, +// covariance eigenvalue regularization, Mahalanobis outlier rejection, and +// Gauss-Newton SE(3) updates adapted to Open3D's registration API. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "open3d/geometry/PointCloud.h" +#include "open3d/utility/Eigen.h" +#include "open3d/utility/Helper.h" +#include "open3d/utility/Logging.h" + +namespace open3d { +namespace pipelines { +namespace registration { + +namespace { + +struct VoxelKey { + std::int64_t x; + std::int64_t y; + std::int64_t z; + + bool operator==(const VoxelKey &other) const { + return x == other.x && y == other.y && z == other.z; + } +}; + +struct VoxelKeyHash { + size_t operator()(const VoxelKey &key) const { + size_t seed = 0; + utility::hash_combine(seed, key.x); + utility::hash_combine(seed, key.y); + utility::hash_combine(seed, key.z); + return seed; + } +}; + +struct VoxelGaussian { + int count = 0; + int representative_index = -1; + Eigen::Vector3d mean = Eigen::Vector3d::Zero(); + Eigen::Matrix3d information = Eigen::Matrix3d::Zero(); +}; + +using VoxelMap = std::unordered_map; + +struct NDTLinearSystem { + Eigen::Matrix6d JTJ = Eigen::Matrix6d::Zero(); + Eigen::Vector6d JTr = Eigen::Vector6d::Zero(); + double residual2 = 0.0; + int residual_count = 0; + + double MeanObjective() const { + return residual2 / static_cast(residual_count); + } +}; + +VoxelKey GetVoxelKey(const Eigen::Vector3d &point, double inv_voxel_size) { + if (!point.allFinite()) { + utility::LogError("Point coordinates must be finite."); + } + const Eigen::Vector3d scaled = point * inv_voxel_size; + if (!scaled.allFinite()) { + utility::LogError("Scaled point coordinates must be finite."); + } + const Eigen::Vector3d rounded = scaled.array().round(); + if (!rounded.allFinite()) { + utility::LogError("Rounded voxel coordinates must be finite."); + } + + const double min_key = std::nextafter( + static_cast(std::numeric_limits::min()), + std::numeric_limits::infinity()); + const double max_key = std::nextafter( + static_cast(std::numeric_limits::max()), + -std::numeric_limits::infinity()); + if ((rounded.array() < min_key).any() || + (rounded.array() > max_key).any()) { + utility::LogError("Voxel coordinates exceed the supported range."); + } + + return VoxelKey{static_cast(rounded.x()), + static_cast(rounded.y()), + static_cast(rounded.z())}; +} + +std::vector GetNeighborOffsets(int neighbor_search_type) { + std::vector offsets{{0, 0, 0}}; + if (neighbor_search_type == 1) { + offsets.push_back({-1, 0, 0}); + offsets.push_back({1, 0, 0}); + offsets.push_back({0, -1, 0}); + offsets.push_back({0, 1, 0}); + offsets.push_back({0, 0, -1}); + offsets.push_back({0, 0, 1}); + } + return offsets; +} + +void ValidateNDTOption(const NormalDistributionsTransformOption &option) { + if (!std::isfinite(option.voxel_size_) || option.voxel_size_ <= 0.0) { + utility::LogError("voxel_size must be positive."); + } + if (option.min_points_per_voxel_ < 4) { + utility::LogError("min_points_per_voxel must be at least 4."); + } + if (!std::isfinite(option.covariance_regularization_) || + option.covariance_regularization_ <= 0.0 || + option.covariance_regularization_ >= 1.0) { + utility::LogError( + "covariance_regularization must be in the range (0, 1)."); + } + if (!std::isfinite(option.transformation_epsilon_) || + option.transformation_epsilon_ <= 0.0) { + utility::LogError("transformation_epsilon must be positive."); + } + if (!std::isfinite(option.relative_objective_) || + option.relative_objective_ <= 0.0) { + utility::LogError("relative_objective must be positive."); + } + if (option.max_iteration_ <= 0) { + utility::LogError("max_iteration must be positive."); + } + if (!std::isfinite(option.outlier_threshold_) || + option.outlier_threshold_ <= 0.0) { + utility::LogError("outlier_threshold must be positive."); + } + if (option.neighbor_search_type_ != 0 && + option.neighbor_search_type_ != 1) { + utility::LogError("neighbor_search_type must be 0 or 1."); + } +} + +VoxelMap BuildVoxelGaussians(const geometry::PointCloud &target, + const NormalDistributionsTransformOption &option) { + const double inv_voxel_size = 1.0 / option.voxel_size_; + std::unordered_map, VoxelKeyHash> voxel_indices; + for (int i = 0; i < static_cast(target.points_.size()); ++i) { + voxel_indices[GetVoxelKey(target.points_[i], inv_voxel_size)].push_back( + i); + } + + VoxelMap voxel_map; + for (const auto &item : voxel_indices) { + const auto &indices = item.second; + if (static_cast(indices.size()) < option.min_points_per_voxel_) { + continue; + } + + VoxelGaussian gaussian; + gaussian.count = static_cast(indices.size()); + for (const int idx : indices) { + gaussian.mean += target.points_[idx]; + } + gaussian.mean /= static_cast(indices.size()); + + Eigen::Matrix3d covariance = Eigen::Matrix3d::Zero(); + double representative_distance2 = std::numeric_limits::max(); + for (const int idx : indices) { + const Eigen::Vector3d centered = + target.points_[idx] - gaussian.mean; + covariance += centered * centered.transpose(); + const double distance2 = centered.squaredNorm(); + if (distance2 < representative_distance2) { + representative_distance2 = distance2; + gaussian.representative_index = idx; + } + } + covariance /= static_cast(indices.size() - 1); + + Eigen::SelfAdjointEigenSolver solver(covariance); + if (solver.info() != Eigen::Success) { + continue; + } + Eigen::Vector3d eigenvalues = solver.eigenvalues(); + const double max_eigenvalue = eigenvalues.maxCoeff(); + if (max_eigenvalue <= 0.0) { + continue; + } + const double min_eigenvalue = + max_eigenvalue * option.covariance_regularization_; + for (int i = 0; i < 3; ++i) { + eigenvalues[i] = std::max(eigenvalues[i], min_eigenvalue); + } + gaussian.information = solver.eigenvectors() * + eigenvalues.cwiseInverse().asDiagonal() * + solver.eigenvectors().transpose(); + voxel_map.emplace(item.first, gaussian); + } + return voxel_map; +} + +NDTLinearSystem ComputeNDTLinearSystem( + const geometry::PointCloud &source_transformed, + const VoxelMap &voxel_map, + const NormalDistributionsTransformOption &option) { + NDTLinearSystem system; + const double inv_voxel_size = 1.0 / option.voxel_size_; + const auto offsets = GetNeighborOffsets(option.neighbor_search_type_); + for (const Eigen::Vector3d &point : source_transformed.points_) { + const VoxelKey key = GetVoxelKey(point, inv_voxel_size); + for (const auto &offset : offsets) { + const VoxelKey neighbor{key.x + offset.x, key.y + offset.y, + key.z + offset.z}; + const auto voxel_itr = voxel_map.find(neighbor); + if (voxel_itr == voxel_map.end()) { + continue; + } + + const Eigen::Vector3d diff = point - voxel_itr->second.mean; + const Eigen::Matrix3d &information = voxel_itr->second.information; + const double distance = diff.transpose() * information * diff; + if (!std::isfinite(distance) || + distance > option.outlier_threshold_) { + continue; + } + + Eigen::Matrix jacobian; + jacobian.block<3, 3>(0, 0) = -utility::SkewMatrix(point); + jacobian.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); + + system.JTJ += jacobian.transpose() * information * jacobian; + system.JTr += jacobian.transpose() * information * diff; + system.residual2 += distance; + ++system.residual_count; + } + } + return system; +} + +RegistrationResult EvaluateNDTResult( + const geometry::PointCloud &source_transformed, + const geometry::PointCloud &target, + const Eigen::Matrix4d &transformation, + const VoxelMap &voxel_map, + const NormalDistributionsTransformOption &option) { + RegistrationResult result(transformation); + if (source_transformed.points_.empty()) { + return result; + } + + const double inv_voxel_size = 1.0 / option.voxel_size_; + const auto offsets = GetNeighborOffsets(option.neighbor_search_type_); + double euclidean_error2 = 0.0; + for (int i = 0; i < static_cast(source_transformed.points_.size()); + ++i) { + const Eigen::Vector3d &point = source_transformed.points_[i]; + const VoxelKey key = GetVoxelKey(point, inv_voxel_size); + + bool has_inlier = false; + double best_residual2 = option.outlier_threshold_; + double best_euclidean_error2 = 0.0; + int best_target_index = -1; + for (int j = 0; j < static_cast(offsets.size()); ++j) { + const VoxelKey neighbor{key.x + offsets[j].x, key.y + offsets[j].y, + key.z + offsets[j].z}; + const auto voxel_itr = voxel_map.find(neighbor); + if (voxel_itr == voxel_map.end()) { + continue; + } + const Eigen::Vector3d diff = point - voxel_itr->second.mean; + const double distance = + diff.transpose() * voxel_itr->second.information * diff; + if (std::isfinite(distance) && distance <= best_residual2) { + has_inlier = true; + best_residual2 = distance; + best_target_index = voxel_itr->second.representative_index; + best_euclidean_error2 = + (point - target.points_[best_target_index]) + .squaredNorm(); + } + } + + if (has_inlier) { + result.correspondence_set_.push_back( + Eigen::Vector2i(i, best_target_index)); + euclidean_error2 += best_euclidean_error2; + } + } + + if (!result.correspondence_set_.empty()) { + const double correspondence_count = + static_cast(result.correspondence_set_.size()); + result.fitness_ = + correspondence_count / + static_cast(source_transformed.points_.size()); + result.inlier_rmse_ = + std::sqrt(euclidean_error2 / correspondence_count); + } + return result; +} + +} // namespace + +NormalDistributionsTransformOption::NormalDistributionsTransformOption( + double voxel_size, + int min_points_per_voxel, + double covariance_regularization, + double transformation_epsilon, + double relative_objective, + int max_iteration, + double outlier_threshold, + int neighbor_search_type) + : voxel_size_(voxel_size), + min_points_per_voxel_(min_points_per_voxel), + covariance_regularization_(covariance_regularization), + transformation_epsilon_(transformation_epsilon), + relative_objective_(relative_objective), + max_iteration_(max_iteration), + outlier_threshold_(outlier_threshold), + neighbor_search_type_(neighbor_search_type) { + ValidateNDTOption(*this); +} + +RegistrationResult RegistrationNDT( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const NormalDistributionsTransformOption &option, + const Eigen::Matrix4d &init) { + ValidateNDTOption(option); + if (source.IsEmpty() || target.IsEmpty()) { + return RegistrationResult(init); + } + + const VoxelMap voxel_map = BuildVoxelGaussians(target, option); + if (voxel_map.empty()) { + utility::LogError( + "No target NDT voxels were created. Increase voxel_size or " + "decrease min_points_per_voxel."); + } + + Eigen::Matrix4d transformation = init; + geometry::PointCloud pcd = source; + if (!init.isIdentity()) { + pcd.Transform(init); + } + + RegistrationResult result = + EvaluateNDTResult(pcd, target, transformation, voxel_map, option); + double previous_objective = std::numeric_limits::infinity(); + + for (int i = 0; i < option.max_iteration_; ++i) { + const NDTLinearSystem system = + ComputeNDTLinearSystem(pcd, voxel_map, option); + + if (system.residual_count < 6) { + utility::LogWarning( + "NDT iteration {:d}: too few effective residuals ({:d}).", + i, system.residual_count); + break; + } + + const double objective = system.MeanObjective(); + utility::LogDebug( + "NDT Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}, " + "mean Mahalanobis objective {:.4f}", + i, result.fitness_, result.inlier_rmse_, objective); + if (i > 0) { + const double relative_objective_change = + std::abs(previous_objective - objective) / + std::max(std::abs(previous_objective), + std::numeric_limits::epsilon()); + if (relative_objective_change < option.relative_objective_) { + break; + } + } + previous_objective = objective; + + if (!system.JTJ.allFinite() || !system.JTr.allFinite()) { + utility::LogWarning( + "NDT iteration {:d}: linear system is non-finite.", i); + break; + } + Eigen::SelfAdjointEigenSolver hessian_solver( + system.JTJ, Eigen::EigenvaluesOnly); + if (hessian_solver.info() != Eigen::Success || + !hessian_solver.eigenvalues().allFinite()) { + utility::LogWarning( + "NDT iteration {:d}: Hessian eigenvalue decomposition " + "failed.", + i); + break; + } + constexpr double kMaxHessianConditionNumber = 1e12; + const double min_eigenvalue = hessian_solver.eigenvalues().minCoeff(); + const double max_eigenvalue = hessian_solver.eigenvalues().maxCoeff(); + if (max_eigenvalue <= 0.0 || + min_eigenvalue <= max_eigenvalue / kMaxHessianConditionNumber) { + utility::LogWarning( + "NDT iteration {:d}: Hessian is rank-deficient or " + "ill-conditioned.", + i); + break; + } + + bool is_success = false; + Eigen::Vector6d update_vector; + std::tie(is_success, update_vector) = + utility::SolveLinearSystemPSD(system.JTJ, -system.JTr); + if (!is_success || !update_vector.allFinite()) { + utility::LogWarning( + "NDT iteration {:d}: linear solve failed or produced a " + "non-finite update.", + i); + break; + } + const Eigen::Matrix4d update = + utility::TransformVector6dToMatrix4d(update_vector); + const Eigen::Matrix4d candidate_transformation = + update * transformation; + if (!update.allFinite() || !candidate_transformation.allFinite()) { + utility::LogWarning( + "NDT iteration {:d}: transformation update is " + "non-finite.", + i); + break; + } + + transformation = candidate_transformation; + pcd.Transform(update); + + result = EvaluateNDTResult(pcd, target, transformation, voxel_map, + option); + + if (update_vector.norm() < option.transformation_epsilon_) { + break; + } + } + + return result; +} + +} // namespace registration +} // namespace pipelines +} // namespace open3d diff --git a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h new file mode 100644 index 00000000000..71d570f7ea1 --- /dev/null +++ b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h @@ -0,0 +1,97 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// Copyright (c) 2018-2024 www.open3d.org +// SPDX-License-Identifier: MIT +// ---------------------------------------------------------------------------- + +#pragma once + +#include + +#include "open3d/pipelines/registration/Registration.h" + +namespace open3d { + +namespace geometry { +class PointCloud; +} + +namespace pipelines { +namespace registration { + +class RegistrationResult; + +/// \class NormalDistributionsTransformOption +/// +/// \brief Class that defines options for 3D Normal Distributions Transform +/// registration. +class NormalDistributionsTransformOption { +public: + /// \brief Parameterized Constructor. + /// + /// \param voxel_size Target voxel size used to build the Gaussian model. + /// \param min_points_per_voxel Minimum number of target points needed for a + /// voxel Gaussian. + /// \param covariance_regularization Minimum eigenvalue ratio used to keep + /// voxel covariance inverses well-conditioned. + /// \param transformation_epsilon Stop optimization when the update vector + /// norm is lower than this value. + /// \param relative_objective Stop optimization when the relative change in + /// mean Mahalanobis objective is lower than this value. + /// \param max_iteration Maximum number of Gauss-Newton iterations. + /// \param outlier_threshold Maximum squared Mahalanobis distance accepted + /// for a point-to-voxel residual. + /// \param neighbor_search_type 0 uses only the rounded center voxel. 1 + /// also uses the six face-adjacent voxels. + NormalDistributionsTransformOption(double voxel_size = 1.0, + int min_points_per_voxel = 6, + double covariance_regularization = 1e-3, + double transformation_epsilon = 1e-6, + double relative_objective = 1e-6, + int max_iteration = 30, + double outlier_threshold = 9.0, + int neighbor_search_type = 1); + + ~NormalDistributionsTransformOption() {} + +public: + /// Target voxel size used to build the Gaussian model. + double voxel_size_; + /// Minimum number of target points needed for a voxel Gaussian. + int min_points_per_voxel_; + /// Minimum eigenvalue ratio for regularizing voxel covariance inverses. + double covariance_regularization_; + /// Stop threshold for the Gauss-Newton update vector norm. + double transformation_epsilon_; + /// Stop threshold for relative mean Mahalanobis objective change. + double relative_objective_; + /// Maximum number of Gauss-Newton iterations. + int max_iteration_; + /// Maximum squared Mahalanobis distance accepted for a residual. + double outlier_threshold_; + /// 0 uses the rounded center voxel; 1 also uses the six face-adjacent + /// voxels. + int neighbor_search_type_; +}; + +/// \brief Function for 3D Normal Distributions Transform registration. +/// +/// This implementation builds a voxelized Gaussian model from the target point +/// cloud and optimizes the rigid source-to-target transformation with +/// Gauss-Newton iterations. +/// +/// \param source The source point cloud. +/// \param target The target point cloud. +/// \param option NDT voxel model and optimization options. +/// \param init Initial transformation estimation. +RegistrationResult RegistrationNDT( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const NormalDistributionsTransformOption &option = + NormalDistributionsTransformOption(), + const Eigen::Matrix4d &init = Eigen::Matrix4d::Identity()); + +} // namespace registration +} // namespace pipelines +} // namespace open3d diff --git a/cpp/pybind/pipelines/registration/registration.cpp b/cpp/pybind/pipelines/registration/registration.cpp index 71bfd20f60d..a9c6ae31f83 100644 --- a/cpp/pybind/pipelines/registration/registration.cpp +++ b/cpp/pybind/pipelines/registration/registration.cpp @@ -16,6 +16,7 @@ #include "open3d/pipelines/registration/FastGlobalRegistration.h" #include "open3d/pipelines/registration/Feature.h" #include "open3d/pipelines/registration/GeneralizedICP.h" +#include "open3d/pipelines/registration/NormalDistributionsTransform.h" #include "open3d/pipelines/registration/RobustKernel.h" #include "open3d/pipelines/registration/TransformationEstimation.h" #include "open3d/utility/Logging.h" @@ -118,6 +119,9 @@ void pybind_registration_declarations(py::module &m) { TransformationEstimation> te_gicp(m_registration, "TransformationEstimationForGeneralizedICP", "Class to estimate a transformation for Generalized ICP."); + py::class_ ndt_option( + m_registration, "NormalDistributionsTransformOption", + "Options for 3D Normal Distributions Transform registration."); py::class_> cc(m_registration, "CorrespondenceChecker", @@ -407,6 +411,88 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``. &TransformationEstimationForGeneralizedICP::kernel_, "Robust Kernel used in the Optimization"); + // open3d.registration.NormalDistributionsTransformOption: + auto ndt_option = + static_cast>( + m_registration.attr("NormalDistributionsTransformOption")); + py::detail::bind_copy_functions( + ndt_option); + ndt_option + .def(py::init([](double voxel_size, int min_points_per_voxel, + double covariance_regularization, + double transformation_epsilon, + double relative_objective, int max_iteration, + double outlier_threshold, + int neighbor_search_type) { + return new NormalDistributionsTransformOption( + voxel_size, min_points_per_voxel, + covariance_regularization, transformation_epsilon, + relative_objective, max_iteration, + outlier_threshold, neighbor_search_type); + }), + "voxel_size"_a = 1.0, "min_points_per_voxel"_a = 6, + "covariance_regularization"_a = 1e-3, + "transformation_epsilon"_a = 1e-6, + "relative_objective"_a = 1e-6, "max_iteration"_a = 30, + "outlier_threshold"_a = 9.0, "neighbor_search_type"_a = 1) + .def_readwrite("voxel_size", + &NormalDistributionsTransformOption::voxel_size_, + "Target voxel size used to build the Gaussian " + "model.") + .def_readwrite( + "min_points_per_voxel", + &NormalDistributionsTransformOption::min_points_per_voxel_, + "Minimum number of target points needed for a voxel " + "Gaussian.") + .def_readwrite("covariance_regularization", + &NormalDistributionsTransformOption:: + covariance_regularization_, + "Minimum eigenvalue ratio used to regularize voxel " + "covariances.") + .def_readwrite( + "transformation_epsilon", + &NormalDistributionsTransformOption:: + transformation_epsilon_, + "Stop optimization when the update vector norm is lower " + "than this value.") + .def_readwrite( + "relative_objective", + &NormalDistributionsTransformOption::relative_objective_, + "Stop optimization when the relative change in mean " + "Mahalanobis objective is lower than this value.") + .def_readwrite("max_iteration", + &NormalDistributionsTransformOption::max_iteration_, + "Maximum number of Gauss-Newton iterations.") + .def_readwrite( + "outlier_threshold", + &NormalDistributionsTransformOption::outlier_threshold_, + "Maximum squared Mahalanobis distance accepted for a " + "point-to-voxel residual.") + .def_readwrite( + "neighbor_search_type", + &NormalDistributionsTransformOption::neighbor_search_type_, + "0 uses the rounded center voxel; 1 also uses the six " + "face-adjacent voxels.") + .def("__repr__", + [](const NormalDistributionsTransformOption &option) { + return fmt::format( + "NormalDistributionsTransformOption(" + "voxel_size={}, " + "min_points_per_voxel={}, " + "covariance_regularization={}, " + "transformation_epsilon={}, " + "relative_objective={}, " + "max_iteration={}, " + "outlier_threshold={}, " + "neighbor_search_type={})", + option.voxel_size_, option.min_points_per_voxel_, + option.covariance_regularization_, + option.transformation_epsilon_, + option.relative_objective_, option.max_iteration_, + option.outlier_threshold_, + option.neighbor_search_type_); + }); + // open3d.registration.CorrespondenceChecker auto cc = static_cast< py::class_(), + "Function for 3D Normal Distributions Transform " + "registration", + "source"_a, "target"_a, + "option"_a = NormalDistributionsTransformOption(), + "init"_a = Eigen::Matrix4d::Identity()); + docstring::FunctionDocInject(m_registration, "registration_ndt", + map_shared_argument_docstrings); + m_registration.def( "registration_ransac_based_on_correspondence", &RegistrationRANSACBasedOnCorrespondence, diff --git a/cpp/tests/pipelines/CMakeLists.txt b/cpp/tests/pipelines/CMakeLists.txt index 486759c8d12..39827dcf86c 100644 --- a/cpp/tests/pipelines/CMakeLists.txt +++ b/cpp/tests/pipelines/CMakeLists.txt @@ -17,6 +17,7 @@ target_sources(tests PRIVATE registration/Feature.cpp registration/GlobalOptimization.cpp registration/GlobalOptimizationConvergenceCriteria.cpp + registration/NormalDistributionsTransform.cpp registration/PoseGraph.cpp registration/Registration.cpp registration/TransformationEstimation.cpp diff --git a/cpp/tests/pipelines/registration/NormalDistributionsTransform.cpp b/cpp/tests/pipelines/registration/NormalDistributionsTransform.cpp new file mode 100644 index 00000000000..3351d1601b1 --- /dev/null +++ b/cpp/tests/pipelines/registration/NormalDistributionsTransform.cpp @@ -0,0 +1,331 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// Copyright (c) 2018-2024 www.open3d.org +// SPDX-License-Identifier: MIT +// ---------------------------------------------------------------------------- + +#include "open3d/pipelines/registration/NormalDistributionsTransform.h" + +#include +#include +#include + +#include "open3d/geometry/PointCloud.h" +#include "tests/Tests.h" + +namespace open3d { +namespace tests { + +namespace { + +geometry::PointCloud MakeStructuredPointCloud() { + geometry::PointCloud pcd; + const std::array centers = { + Eigen::Vector3d(-1.2, -0.8, -0.4), Eigen::Vector3d(-0.2, 0.7, 0.3), + Eigen::Vector3d(0.9, -0.1, 0.8), Eigen::Vector3d(1.5, 1.0, -0.2), + Eigen::Vector3d(-1.5, 1.1, 0.9)}; + + for (int cluster = 0; cluster < static_cast(centers.size()); + ++cluster) { + const double angle = 0.35 * static_cast(cluster); + const double c = std::cos(angle); + const double s = std::sin(angle); + Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity(); + rotation.block<2, 2>(0, 0) << c, -s, s, c; + + for (int i = 0; i < 160; ++i) { + const double a = (static_cast(i % 16) - 7.5) / 7.5; + const double b = (static_cast((i / 16) % 10) - 4.5) / 4.5; + const double h = std::sin(0.37 * static_cast(i) + + static_cast(cluster)); + const Eigen::Vector3d offset(0.20 * a + 0.04 * b, + 0.13 * b + 0.03 * h, + 0.08 * h + 0.025 * a * b); + pcd.points_.push_back(centers[cluster] + rotation * offset); + } + } + return pcd; +} + +geometry::PointCloud MakeRoundIndexedPointCloud() { + geometry::PointCloud pcd; + pcd.points_ = { + Eigen::Vector3d(0.60, 0.08, 0.12), + Eigen::Vector3d(0.75, 0.32, 0.18), + Eigen::Vector3d(0.90, 0.16, 0.42), + Eigen::Vector3d(1.10, 0.38, 0.08), + Eigen::Vector3d(1.25, 0.10, 0.34), + Eigen::Vector3d(1.40, 0.28, 0.26), + }; + return pcd; +} + +Eigen::Matrix4d MakeSmallInitialTransformation() { + Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity(); + transformation.block<3, 3>(0, 0) = + Eigen::AngleAxisd(0.01, Eigen::Vector3d::UnitZ()) + .toRotationMatrix(); + transformation.block<3, 1>(0, 3) = Eigen::Vector3d(0.01, -0.01, 0.01); + return transformation; +} + +Eigen::Matrix4d MakeTransformation() { + Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity(); + transformation.block<3, 3>(0, 0) = + Eigen::AngleAxisd(0.08, Eigen::Vector3d::UnitZ()) + .toRotationMatrix(); + transformation.block<3, 1>(0, 3) = Eigen::Vector3d(0.24, -0.17, 0.11); + return transformation; +} + +} // namespace + +TEST(NormalDistributionsTransform, OptionRejectsInvalidValues) { + using pipelines::registration::NormalDistributionsTransformOption; + + EXPECT_THROW(NormalDistributionsTransformOption(-1.0), std::runtime_error); + EXPECT_THROW(NormalDistributionsTransformOption(1.0, 2), + std::runtime_error); + EXPECT_THROW(NormalDistributionsTransformOption(1.0, 4, 0.01, 0.01, 0.0), + std::runtime_error); + EXPECT_THROW(NormalDistributionsTransformOption(1.0, 4, 0.01, 0.01, 1e-6, + 30, 0.0), + std::runtime_error); + EXPECT_THROW(NormalDistributionsTransformOption(1.0, 4, 0.01, 0.01, 1e-6, + 30, 9.0, 2), + std::runtime_error); +} + +TEST(NormalDistributionsTransform, RegistrationRejectsMutatedInvalidOption) { + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + const geometry::PointCloud pcd = MakeStructuredPointCloud(); + NormalDistributionsTransformOption option; + option.voxel_size_ = 0.0; + + EXPECT_THROW(RegistrationNDT(pcd, pcd, option), std::runtime_error); + + option = NormalDistributionsTransformOption(); + option.relative_objective_ = std::numeric_limits::quiet_NaN(); + EXPECT_THROW(RegistrationNDT(pcd, pcd, option), std::runtime_error); + + option = NormalDistributionsTransformOption(); + option.outlier_threshold_ = std::numeric_limits::infinity(); + EXPECT_THROW(RegistrationNDT(pcd, pcd, option), std::runtime_error); +} + +TEST(NormalDistributionsTransform, VoxelIndexUsesRound) { + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + const geometry::PointCloud pcd = MakeRoundIndexedPointCloud(); + const Eigen::Matrix4d init = MakeSmallInitialTransformation(); + const auto result = + RegistrationNDT(pcd, pcd, + NormalDistributionsTransformOption( + 1.0, 4, 1e-3, 1e-6, 1e-6, 1, 9.0, 0), + init); + + EXPECT_FALSE(result.transformation_.isApprox(init, 1e-12)); + EXPECT_LT((result.transformation_ - Eigen::Matrix4d::Identity()).norm(), + (init - Eigen::Matrix4d::Identity()).norm()); +} + +TEST(NormalDistributionsTransform, InvalidTargetCoordinatesThrow) { + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + const geometry::PointCloud source = MakeStructuredPointCloud(); + const std::array invalid_coordinates = { + std::numeric_limits::quiet_NaN(), + std::numeric_limits::infinity(), 1e30}; + for (const double coordinate : invalid_coordinates) { + geometry::PointCloud target = source; + target.points_.emplace_back(coordinate, 0.0, 0.0); + EXPECT_THROW(RegistrationNDT(source, target, + NormalDistributionsTransformOption()), + std::runtime_error) + << "coordinate: " << coordinate; + } +} + +TEST(NormalDistributionsTransform, InvalidSourceCoordinatesThrow) { + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + const geometry::PointCloud target = MakeStructuredPointCloud(); + const std::array invalid_coordinates = { + std::numeric_limits::quiet_NaN(), + std::numeric_limits::infinity(), 1e30}; + for (const double coordinate : invalid_coordinates) { + geometry::PointCloud source = target; + source.points_.emplace_back(coordinate, 0.0, 0.0); + EXPECT_THROW(RegistrationNDT(source, target, + NormalDistributionsTransformOption()), + std::runtime_error) + << "coordinate: " << coordinate; + } +} + +TEST(NormalDistributionsTransform, VoxelIndexSupportsCoordinatesAboveInt32) { + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + geometry::PointCloud target; + const std::array offsets = { + Eigen::Vector3d(0.125, 0.0, 0.0), + Eigen::Vector3d(-0.125, 0.0, 0.0), + Eigen::Vector3d(0.0, 0.125, 0.0), + Eigen::Vector3d(0.0, -0.125, 0.0), + Eigen::Vector3d(0.0, 0.0, 0.125), + Eigen::Vector3d(0.0, 0.0, -0.125)}; + for (const double center_x : {3e9, -3e9}) { + for (const Eigen::Vector3d &offset : offsets) { + target.points_.push_back(Eigen::Vector3d(center_x, 0.0, 0.0) + + offset); + } + } + + geometry::PointCloud source; + source.points_.assign(6, Eigen::Vector3d(3e9, 0.0, 0.0)); + + const auto result = + RegistrationNDT(source, target, + NormalDistributionsTransformOption( + 1.0, 6, 1e-3, 1e-6, 1e-6, 1, 0.1, 0)); + + EXPECT_EQ(result.correspondence_set_.size(), source.points_.size()); + EXPECT_DOUBLE_EQ(result.fitness_, 1.0); +} + +TEST(NormalDistributionsTransform, ResultRMSEMatchesCorrespondenceSet) { + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + const geometry::PointCloud pcd = MakeRoundIndexedPointCloud(); + const Eigen::Matrix4d init = MakeSmallInitialTransformation(); + const auto result = + RegistrationNDT(pcd, pcd, + NormalDistributionsTransformOption( + 1.0, 4, 1e-3, 1e-6, 1e-6, 1, 9.0, 0), + init); + + EXPECT_FALSE(result.transformation_.isApprox(init, 1e-12)); + EXPECT_LT((result.transformation_ - Eigen::Matrix4d::Identity()).norm(), + (init - Eigen::Matrix4d::Identity()).norm()); + ASSERT_EQ(result.correspondence_set_.size(), pcd.points_.size()); + + geometry::PointCloud source_transformed = pcd; + source_transformed.Transform(result.transformation_); + double error2 = 0.0; + for (const Eigen::Vector2i &correspondence : result.correspondence_set_) { + error2 += (source_transformed.points_[correspondence[0]] - + pcd.points_[correspondence[1]]) + .squaredNorm(); + } + const double expected_rmse = + std::sqrt(error2 / result.correspondence_set_.size()); + EXPECT_NEAR(result.inlier_rmse_, expected_rmse, 1e-12); +} + +TEST(NormalDistributionsTransform, RegistrationNDTRecoversKnownTransform) { + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + geometry::PointCloud source = MakeStructuredPointCloud(); + geometry::PointCloud target = source; + const Eigen::Matrix4d expected = MakeTransformation(); + target.Transform(expected); + + const auto result = + RegistrationNDT(source, target, + NormalDistributionsTransformOption( + 0.8, 4, 1e-3, 0.01, 1e-7, 40, 9.0, 1), + Eigen::Matrix4d::Identity()); + + EXPECT_GT(result.fitness_, 0.80); + EXPECT_LT(result.inlier_rmse_, 0.16); + EXPECT_TRUE(result.transformation_.isApprox(expected, 5e-2)) + << "expected:\n" + << expected << "\nactual:\n" + << result.transformation_; +} + +TEST(NormalDistributionsTransform, RegistrationNDTImprovesInitialAlignment) { + using pipelines::registration::EvaluateRegistration; + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + geometry::PointCloud source = MakeStructuredPointCloud(); + geometry::PointCloud target = source; + const Eigen::Matrix4d expected = MakeTransformation(); + target.Transform(expected); + + const auto initial = EvaluateRegistration(source, target, 0.35); + const auto result = + RegistrationNDT(source, target, + NormalDistributionsTransformOption( + 0.8, 4, 1e-3, 0.01, 1e-7, 40, 9.0, 1), + Eigen::Matrix4d::Identity()); + const auto refined = + EvaluateRegistration(source, target, 0.35, result.transformation_); + + EXPECT_GT(refined.fitness_, initial.fitness_); + EXPECT_LT(refined.inlier_rmse_, initial.inlier_rmse_); +} + +TEST(NormalDistributionsTransform, + RankDeficientHessianPreservesInitialTransformation) { + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + geometry::PointCloud source; + source.points_.assign(10, Eigen::Vector3d::Zero()); + + geometry::PointCloud target; + target.points_ = { + Eigen::Vector3d(0.35, 0.25, 0.25), + Eigen::Vector3d(0.25, 0.35, 0.25), + Eigen::Vector3d(0.25, 0.25, 0.35), + Eigen::Vector3d(0.15, 0.15, 0.15), + }; + + Eigen::Matrix4d init = Eigen::Matrix4d::Identity(); + init.block<3, 1>(0, 3) = Eigen::Vector3d(0.2, 0.2, 0.2); + const auto result = + RegistrationNDT(source, target, + NormalDistributionsTransformOption( + 1.0, 4, 1e-3, 1e-6, 1e-6, 1, 9.0, 0), + init); + + EXPECT_TRUE(result.transformation_.isApprox(init, 1e-12)) + << "expected:\n" + << init << "\nactual:\n" + << result.transformation_; +} + +TEST(NormalDistributionsTransform, RelativeObjectiveStopsBeforeSecondUpdate) { + using pipelines::registration::NormalDistributionsTransformOption; + using pipelines::registration::RegistrationNDT; + + geometry::PointCloud source = MakeStructuredPointCloud(); + geometry::PointCloud target = source; + target.Transform(MakeTransformation()); + + NormalDistributionsTransformOption one_iteration_option(0.8, 4, 1e-3, 1e-15, + 1e-12, 1, 9.0, 1); + NormalDistributionsTransformOption converged_option(0.8, 4, 1e-3, 1e-15, + 1e9, 40, 9.0, 1); + EXPECT_DOUBLE_EQ(converged_option.relative_objective_, 1e9); + + const auto one_iteration = + RegistrationNDT(source, target, one_iteration_option); + const auto converged = RegistrationNDT(source, target, converged_option); + EXPECT_TRUE(converged.transformation_.isApprox( + one_iteration.transformation_, 1e-12)); +} + +} // namespace tests +} // namespace open3d diff --git a/docs/tutorial/pipelines/index.rst b/docs/tutorial/pipelines/index.rst index a02eaa9fe44..f384fb145cf 100644 --- a/docs/tutorial/pipelines/index.rst +++ b/docs/tutorial/pipelines/index.rst @@ -4,6 +4,7 @@ Pipelines .. toctree:: icp_registration + ndt_registration generalized_icp robust_kernels colored_pointcloud_registration diff --git a/docs/tutorial/pipelines/ndt_registration.rst b/docs/tutorial/pipelines/ndt_registration.rst new file mode 100644 index 00000000000..5930691d3f0 --- /dev/null +++ b/docs/tutorial/pipelines/ndt_registration.rst @@ -0,0 +1,26 @@ +Normal Distributions Transform +============================== + +Normal Distributions Transform (NDT) registration aligns a source point cloud to +a target point cloud represented as a voxel grid of local Gaussian +distributions. The method can be useful when a smooth target distribution is +preferred over point-to-point nearest-neighbor correspondences. + +This implementation follows the Normal Distributions Transform introduced by +Biber and Straßer [BiberAndStrasser2003]_ and the 3D NDT formulation described +by Gao [Gao2023]_. + +Open3D exposes NDT through +``open3d.pipelines.registration.registration_ndt``. The main parameters are +collected in ``NormalDistributionsTransformOption``, including both voxel +Gaussian model parameters and convergence criteria. Optimization stops when +the pose update is small or the relative change in mean Mahalanobis objective +falls below the configured threshold: + +.. literalinclude:: ../../../examples/python/pipelines/ndt_registration.py + :language: python + :start-after: option = + :end-before: print("Apply Normal Distributions Transform registration") + +The full runnable script is available at +``examples/python/pipelines/ndt_registration.py``. diff --git a/docs/tutorial/reference.rst b/docs/tutorial/reference.rst index 2afcb0a9f7a..59f909850e5 100644 --- a/docs/tutorial/reference.rst +++ b/docs/tutorial/reference.rst @@ -6,11 +6,13 @@ Reference .. [ArujoAndOliveira2020] A. Araújo and M. Oliveira, A robust statistics approach for plane detection in unorganized point clouds, Pattern Recognition, 2020 .. [Bernardini1999] F. Bernardini and J. Mittleman and HRushmeier and C. Silva and G. Taubin: The ball-pivoting algorithm for surface reconstruction, IEEE transactions on visualization and computer graphics, 5(4), 349-359, 1999 .. [BeslAndMcKay1992] Paul J. Besl and Neil D. McKay, A Method for Registration of 3D Shapes, PAMI, 1992. +.. [BiberAndStrasser2003] P. Biber and W. Straßer, The Normal Distributions Transform: A New Approach to Laser Scan Matching, Proceedings of the 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003), vol. 3, pp. 2743-2748, IEEE, 2003. .. [ChenAndMedioni1992] Y. Chen and G. G. Medioni, Object modelling by registration of multiple range images, Image and Vision Computing, 10(3), 1992. .. [Choi2015] S. Choi, Q.-Y. Zhou, and V. Koltun, Robust Reconstruction of Indoor Scenes, CVPR, 2015. .. [Curless1996] B. Curless and M. Levoy. A volumetric method for building complex models from range images. In SIGGRAPH, 1996. .. [Edelsbrunner1983] H. Edelsbrunner and D. G. Kirkpatrick and R. Seidel: On the shape of a set of points in the plane, IEEE Transactions on Information Theory, 29 (4): 551–559, 1983 .. [Ester1996] M. Ester and H.-P. Kriegel and J Sander and X. Xu, A density-based algorithm for discovering clusters in large spatial databases with noise, KDD, 1996. +.. [Gao2023] X. Gao, SLAM Technology in Autonomous Driving and Robotics: From Theory to Practice (in Chinese), Publishing House of Electronics Industry, 2023. .. [Katz2007] S. Katz and A. Tal and R. Basri, Direct visibility of point sets, SIGGRAPH, 2007. .. [Kazhdan2006] M. Kazhdan and M. Bolitho and H. Hoppe: Poisson surface reconstruction, Eurographics, 2006. .. [Knapitsch2017] A. Knapitsch and J. Park and Q. Zhou and V. Koltun: Tanks and Temples: Benchmarking Large-Scale Scene Reconstruction, ACM Transactions on Graphics, vol 36 (4), 2017 diff --git a/examples/python/pipelines/ndt_registration.py b/examples/python/pipelines/ndt_registration.py new file mode 100644 index 00000000000..28fbbef1397 --- /dev/null +++ b/examples/python/pipelines/ndt_registration.py @@ -0,0 +1,53 @@ +# ---------------------------------------------------------------------------- +# - Open3D: www.open3d.org - +# ---------------------------------------------------------------------------- +# Copyright (c) 2018-2024 www.open3d.org +# SPDX-License-Identifier: MIT +# ---------------------------------------------------------------------------- +"""Normal Distributions Transform registration example.""" + +import copy +import numpy as np +import open3d as o3d + + +def draw_registration_result(source, target, transformation): + source_temp = copy.deepcopy(source) + target_temp = copy.deepcopy(target) + source_temp.paint_uniform_color([1, 0.706, 0]) + target_temp.paint_uniform_color([0, 0.651, 0.929]) + source_temp.transform(transformation) + o3d.visualization.draw([source_temp, target_temp]) + + +if __name__ == "__main__": + pcd_data = o3d.data.DemoICPPointClouds() + source = o3d.io.read_point_cloud(pcd_data.paths[0]) + target = o3d.io.read_point_cloud(pcd_data.paths[1]) + + trans_init = np.eye(4) + + print("Initial alignment") + print( + o3d.pipelines.registration.evaluate_registration( + source, target, 0.02, trans_init), "\n") + + source_downsampled = source.voxel_down_sample(voxel_size=0.04) + + option = o3d.pipelines.registration.NormalDistributionsTransformOption( + voxel_size=0.5, + min_points_per_voxel=6, + covariance_regularization=1e-6, + transformation_epsilon=1e-6, + relative_objective=1e-6, + max_iteration=200, + outlier_threshold=9.0, + neighbor_search_type=1) + + print("Apply Normal Distributions Transform registration") + reg_ndt = o3d.pipelines.registration.registration_ndt( + source_downsampled, target, option, trans_init) + print(reg_ndt) + print("Transformation is:") + print(reg_ndt.transformation, "\n") + draw_registration_result(source, target, reg_ndt.transformation) diff --git a/python/test/pipelines/test_normal_distributions_transform.py b/python/test/pipelines/test_normal_distributions_transform.py new file mode 100644 index 00000000000..5ec5c82c50a --- /dev/null +++ b/python/test/pipelines/test_normal_distributions_transform.py @@ -0,0 +1,154 @@ +# ---------------------------------------------------------------------------- +# - Open3D: www.open3d.org - +# ---------------------------------------------------------------------------- +# Copyright (c) 2018-2024 www.open3d.org +# SPDX-License-Identifier: MIT +# ---------------------------------------------------------------------------- + +import numpy as np +import open3d as o3d +import pytest + + +def make_structured_point_cloud(): + points = [] + centers = [ + np.array([-1.2, -0.8, -0.4]), + np.array([-0.2, 0.7, 0.3]), + np.array([0.9, -0.1, 0.8]), + np.array([1.5, 1.0, -0.2]), + np.array([-1.5, 1.1, 0.9]), + ] + for cluster, center in enumerate(centers): + angle = 0.35 * cluster + c = np.cos(angle) + s = np.sin(angle) + rotation = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]) + for i in range(160): + a = ((i % 16) - 7.5) / 7.5 + b = (((i // 16) % 10) - 4.5) / 4.5 + h = np.sin(0.37 * i + cluster) + offset = np.array([ + 0.20 * a + 0.04 * b, + 0.13 * b + 0.03 * h, + 0.08 * h + 0.025 * a * b, + ]) + points.append(center + rotation @ offset) + + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(np.asarray(points)) + return pcd + + +def make_transformation(): + transformation = np.eye(4) + angle = 0.08 + c = np.cos(angle) + s = np.sin(angle) + transformation[:3, :3] = np.array([[c, -s, 0.0], [s, c, 0.0], + [0.0, 0.0, 1.0]]) + transformation[:3, 3] = np.array([0.24, -0.17, 0.11]) + return transformation + + +def test_normal_distributions_transform_option(): + option = o3d.pipelines.registration.NormalDistributionsTransformOption( + voxel_size=0.8, + min_points_per_voxel=4, + covariance_regularization=1e-3, + transformation_epsilon=0.01, + relative_objective=1e-7, + max_iteration=40, + outlier_threshold=9.0, + neighbor_search_type=1) + + assert option.voxel_size == 0.8 + assert option.min_points_per_voxel == 4 + assert option.relative_objective == 1e-7 + assert option.max_iteration == 40 + assert "NormalDistributionsTransformOption" in repr(option) + + +def test_transformation_estimation_for_ndt_is_not_exposed(): + assert not hasattr(o3d.pipelines.registration, + "TransformationEstimationForNDT") + + +def test_registration_ndt_recovers_known_transform(): + source = make_structured_point_cloud() + target = o3d.geometry.PointCloud(source) + expected = make_transformation() + target.transform(expected) + + initial = o3d.pipelines.registration.evaluate_registration( + source, target, 0.35) + result = o3d.pipelines.registration.registration_ndt( + source, target, + o3d.pipelines.registration.NormalDistributionsTransformOption( + voxel_size=0.8, + min_points_per_voxel=4, + covariance_regularization=1e-3, + transformation_epsilon=0.01, + relative_objective=1e-7, + max_iteration=40, + outlier_threshold=9.0, + neighbor_search_type=1), np.eye(4)) + refined = o3d.pipelines.registration.evaluate_registration( + source, target, 0.35, result.transformation) + + assert refined.fitness > initial.fitness + assert refined.inlier_rmse < initial.inlier_rmse + assert result.fitness > 0.80 + assert result.inlier_rmse < 0.16 + np.testing.assert_allclose(result.transformation, expected, atol=5e-2) + + source_transformed = o3d.geometry.PointCloud(source) + source_transformed.transform(result.transformation) + correspondences = np.asarray(result.correspondence_set) + source_points = np.asarray(source_transformed.points) + target_points = np.asarray(target.points) + errors = (source_points[correspondences[:, 0]] - + target_points[correspondences[:, 1]]) + expected_rmse = np.sqrt(np.mean(np.sum(errors * errors, axis=1))) + np.testing.assert_allclose(result.inlier_rmse, expected_rmse) + + +def test_registration_ndt_rejects_icp_convergence_criteria(): + source = make_structured_point_cloud() + target = o3d.geometry.PointCloud(source) + + with pytest.raises(TypeError): + o3d.pipelines.registration.registration_ndt( + source, target, + o3d.pipelines.registration.NormalDistributionsTransformOption(), + np.eye(4), o3d.pipelines.registration.ICPConvergenceCriteria()) + + +def test_registration_ndt_rejects_invalid_options(): + with pytest.raises(RuntimeError): + o3d.pipelines.registration.NormalDistributionsTransformOption( + voxel_size=-1.0) + with pytest.raises(RuntimeError): + o3d.pipelines.registration.NormalDistributionsTransformOption( + voxel_size=1.0, outlier_threshold=0.0) + with pytest.raises(RuntimeError): + o3d.pipelines.registration.NormalDistributionsTransformOption( + voxel_size=1.0, neighbor_search_type=2) + with pytest.raises(RuntimeError): + o3d.pipelines.registration.NormalDistributionsTransformOption( + voxel_size=1.0, relative_objective=0.0) + + +def test_registration_ndt_rejects_mutated_invalid_option(): + source = make_structured_point_cloud() + target = o3d.geometry.PointCloud(source) + option = o3d.pipelines.registration.NormalDistributionsTransformOption() + option.voxel_size = 0.0 + + with pytest.raises(RuntimeError): + o3d.pipelines.registration.registration_ndt(source, target, option) + + option = o3d.pipelines.registration.NormalDistributionsTransformOption() + option.relative_objective = np.nan + with pytest.raises(RuntimeError): + o3d.pipelines.registration.registration_ndt(source, target, option)