From 131ebe555d024447af62e12caf3bd53c8bd95ed3 Mon Sep 17 00:00:00 2001 From: "xiang.zeng" Date: Mon, 13 Jul 2026 12:45:50 +0800 Subject: [PATCH 1/5] Add Normal Distributions Transform registration --- CHANGELOG.md | 1 + cpp/open3d/Open3D.h.in | 1 + cpp/open3d/pipelines/CMakeLists.txt | 1 + .../NormalDistributionsTransform.cpp | 453 ++++++++++++++++++ .../NormalDistributionsTransform.h | 97 ++++ .../pipelines/registration/registration.cpp | 98 ++++ cpp/tests/pipelines/CMakeLists.txt | 1 + .../NormalDistributionsTransform.cpp | 331 +++++++++++++ docs/tutorial/pipelines/index.rst | 1 + docs/tutorial/pipelines/ndt_registration.rst | 26 + docs/tutorial/reference.rst | 2 + examples/python/pipelines/ndt_registration.py | 53 ++ .../test_normal_distributions_transform.py | 154 ++++++ 13 files changed, 1219 insertions(+) create mode 100644 cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp create mode 100644 cpp/open3d/pipelines/registration/NormalDistributionsTransform.h create mode 100644 cpp/tests/pipelines/registration/NormalDistributionsTransform.cpp create mode 100644 docs/tutorial/pipelines/ndt_registration.rst create mode 100644 examples/python/pipelines/ndt_registration.py create mode 100644 python/test/pipelines/test_normal_distributions_transform.py diff --git a/CHANGELOG.md b/CHANGELOG.md index 69577c2a050..9b0e6346363 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -44,6 +44,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) From e54705578ef951ab7ff382ce829c30048c281c69 Mon Sep 17 00:00:00 2001 From: "xiang.zeng" Date: Tue, 18 Aug 2026 14:10:07 +0800 Subject: [PATCH 2/5] Address NDT review feedback --- CHANGELOG.md | 2 +- .../NormalDistributionsTransform.cpp | 70 ++++++++++++------- .../NormalDistributionsTransform.h | 4 +- .../pipelines/registration/registration.cpp | 21 ++---- docs/tutorial/pipelines/ndt_registration.rst | 5 +- 5 files changed, 58 insertions(+), 44 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index cbe4c433cf8..54e1aa63906 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -51,7 +51,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. +- Add 3D Normal Distributions Transform registration with C++ and Python APIs (PR #7517). - 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/pipelines/registration/NormalDistributionsTransform.cpp b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp index 8b78d544b3a..9064d34c1cb 100644 --- a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp +++ b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp @@ -62,6 +62,23 @@ struct VoxelGaussian { using VoxelMap = std::unordered_map; +struct VoxelAccumulator { + int count = 0; + Eigen::Vector3d mean = Eigen::Vector3d::Zero(); + Eigen::Matrix3d covariance_accumulator = Eigen::Matrix3d::Zero(); + + void AddPoint(const Eigen::Vector3d &point) { + ++count; + const Eigen::Vector3d delta = point - mean; + mean += delta / static_cast(count); + const Eigen::Vector3d delta_after_update = point - mean; + covariance_accumulator += delta * delta_after_update.transpose(); + } +}; + +using VoxelAccumulatorMap = + std::unordered_map; + struct NDTLinearSystem { Eigen::Matrix6d JTJ = Eigen::Matrix6d::Zero(); Eigen::Vector6d JTr = Eigen::Vector6d::Zero(); @@ -152,39 +169,24 @@ void ValidateNDTOption(const NormalDistributionsTransformOption &option) { 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); + VoxelAccumulatorMap voxel_accumulators; + for (const Eigen::Vector3d &point : target.points_) { + voxel_accumulators[GetVoxelKey(point, inv_voxel_size)].AddPoint(point); } VoxelMap voxel_map; - for (const auto &item : voxel_indices) { - const auto &indices = item.second; - if (static_cast(indices.size()) < option.min_points_per_voxel_) { + for (const auto &item : voxel_accumulators) { + const VoxelAccumulator &accumulator = item.second; + if (accumulator.count < 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); + gaussian.count = accumulator.count; + gaussian.mean = accumulator.mean; + const Eigen::Matrix3d covariance = + accumulator.covariance_accumulator / + static_cast(accumulator.count - 1); Eigen::SelfAdjointEigenSolver solver(covariance); if (solver.info() != Eigen::Success) { @@ -205,6 +207,22 @@ VoxelMap BuildVoxelGaussians(const geometry::PointCloud &target, solver.eigenvectors().transpose(); voxel_map.emplace(item.first, gaussian); } + + for (int i = 0; i < static_cast(target.points_.size()); ++i) { + const Eigen::Vector3d &point = target.points_[i]; + auto voxel_itr = voxel_map.find(GetVoxelKey(point, inv_voxel_size)); + if (voxel_itr == voxel_map.end()) { + continue; + } + VoxelGaussian &gaussian = voxel_itr->second; + const double distance2 = (point - gaussian.mean).squaredNorm(); + if (gaussian.representative_index < 0 || + distance2 < (target.points_[gaussian.representative_index] - + gaussian.mean) + .squaredNorm()) { + gaussian.representative_index = i; + } + } return voxel_map; } diff --git a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h index 71d570f7ea1..7dae0670441 100644 --- a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h +++ b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h @@ -79,7 +79,9 @@ class NormalDistributionsTransformOption { /// /// This implementation builds a voxelized Gaussian model from the target point /// cloud and optimizes the rigid source-to-target transformation with -/// Gauss-Newton iterations. +/// Gauss-Newton iterations. The returned correspondence set uses the target +/// point closest to each accepted voxel mean as its representative, and the +/// inlier RMSE is computed from those representative point correspondences. /// /// \param source The source point cloud. /// \param target The target point cloud. diff --git a/cpp/pybind/pipelines/registration/registration.cpp b/cpp/pybind/pipelines/registration/registration.cpp index a9c6ae31f83..087a6b768d0 100644 --- a/cpp/pybind/pipelines/registration/registration.cpp +++ b/cpp/pybind/pipelines/registration/registration.cpp @@ -418,18 +418,8 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``. 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); - }), + .def(py::init(), "voxel_size"_a = 1.0, "min_points_per_voxel"_a = 6, "covariance_regularization"_a = 1e-3, "transformation_epsilon"_a = 1e-6, @@ -773,8 +763,6 @@ must hold true for all edges.)"); "the " "source point's correspondence is itself."}, {"option", "Registration option"}, - {"ndt_option", - "Normal Distributions Transform registration option."}, {"ransac_n", "Fit ransac with ``ransac_n`` correspondences"}, {"source_feature", "Source point cloud feature."}, @@ -834,8 +822,11 @@ must hold true for all edges.)"); "source"_a, "target"_a, "option"_a = NormalDistributionsTransformOption(), "init"_a = Eigen::Matrix4d::Identity()); + auto ndt_argument_docstrings = map_shared_argument_docstrings; + ndt_argument_docstrings["option"] = + "Normal Distributions Transform registration option."; docstring::FunctionDocInject(m_registration, "registration_ndt", - map_shared_argument_docstrings); + ndt_argument_docstrings); m_registration.def( "registration_ransac_based_on_correspondence", diff --git a/docs/tutorial/pipelines/ndt_registration.rst b/docs/tutorial/pipelines/ndt_registration.rst index 5930691d3f0..60edcfa367a 100644 --- a/docs/tutorial/pipelines/ndt_registration.rst +++ b/docs/tutorial/pipelines/ndt_registration.rst @@ -15,7 +15,10 @@ Open3D exposes NDT through 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: +falls below the configured threshold. In the returned ``RegistrationResult``, +each accepted voxel is represented by the target point closest to its mean, +and ``inlier_rmse`` is the Euclidean RMSE over those representative point +correspondences: .. literalinclude:: ../../../examples/python/pipelines/ndt_registration.py :language: python From 6c2544ef5d457cb080ef96b320a8abbd813cba32 Mon Sep 17 00:00:00 2001 From: Sameer Sheorey Date: Wed, 19 Aug 2026 13:09:27 -0700 Subject: [PATCH 3/5] Improve NDT registration performance and documentation - parallelize NDT reductions with TBB - reuse linear-system traversal for per-iteration fitness and RMSE - reduce voxel neighbor and representative-point allocations - clarify the Gauss-Newton point-to-distribution formulation - remove the redundant RegistrationResult declaration --- .../NormalDistributionsTransform.cpp | 354 ++++++++++++------ .../NormalDistributionsTransform.h | 2 - docs/tutorial/pipelines/ndt_registration.rst | 9 +- 3 files changed, 252 insertions(+), 113 deletions(-) diff --git a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp index 9064d34c1cb..8fdd598f34b 100644 --- a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp +++ b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp @@ -7,14 +7,17 @@ #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. +// This implements a Gauss-Newton point-to-distribution NDT variant based on +// the 3D formulation described in +// https://github.com/gaoxiang12/slam_in_autonomous_driving/blob/master/src/ch7/ndt_3d.cc. +// It models target voxels as regularized Gaussians, rejects outliers by their +// Mahalanobis distance, and applies left-perturbation SE(3) updates. #include +#include +#include #include +#include #include #include #include @@ -26,6 +29,7 @@ #include "open3d/utility/Eigen.h" #include "open3d/utility/Helper.h" #include "open3d/utility/Logging.h" +#include "open3d/utility/Parallel.h" namespace open3d { namespace pipelines { @@ -64,11 +68,13 @@ using VoxelMap = std::unordered_map; struct VoxelAccumulator { int count = 0; + std::vector point_indices; Eigen::Vector3d mean = Eigen::Vector3d::Zero(); Eigen::Matrix3d covariance_accumulator = Eigen::Matrix3d::Zero(); - void AddPoint(const Eigen::Vector3d &point) { + void AddPoint(const Eigen::Vector3d &point, int point_index) { ++count; + point_indices.push_back(point_index); const Eigen::Vector3d delta = point - mean; mean += delta / static_cast(count); const Eigen::Vector3d delta_after_update = point - mean; @@ -84,10 +90,24 @@ struct NDTLinearSystem { Eigen::Vector6d JTr = Eigen::Vector6d::Zero(); double residual2 = 0.0; int residual_count = 0; + double euclidean_error2 = 0.0; + int correspondence_count = 0; double MeanObjective() const { return residual2 / static_cast(residual_count); } + + double Fitness(std::size_t source_size) const { + return static_cast(correspondence_count) / + static_cast(source_size); + } + + double InlierRMSE() const { + return correspondence_count == 0 + ? 0.0 + : std::sqrt(euclidean_error2 / + static_cast(correspondence_count)); + } }; VoxelKey GetVoxelKey(const Eigen::Vector3d &point, double inv_voxel_size) { @@ -119,15 +139,16 @@ VoxelKey GetVoxelKey(const Eigen::Vector3d &point, double inv_voxel_size) { static_cast(rounded.z())}; } -std::vector GetNeighborOffsets(int neighbor_search_type) { - std::vector offsets{{0, 0, 0}}; +using NeighborOffsets = std::array; + +NeighborOffsets GetNeighborOffsets(int neighbor_search_type, + std::size_t &offset_count) { + NeighborOffsets offsets{{{0, 0, 0}, {-1, 0, 0}, {1, 0, 0}, + {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, + {0, 0, 1}}}; + offset_count = 1; 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}); + offset_count = offsets.size(); } return offsets; } @@ -170,8 +191,9 @@ VoxelMap BuildVoxelGaussians(const geometry::PointCloud &target, const NormalDistributionsTransformOption &option) { const double inv_voxel_size = 1.0 / option.voxel_size_; VoxelAccumulatorMap voxel_accumulators; - for (const Eigen::Vector3d &point : target.points_) { - voxel_accumulators[GetVoxelKey(point, inv_voxel_size)].AddPoint(point); + for (int i = 0; i < static_cast(target.points_.size()); ++i) { + voxel_accumulators[GetVoxelKey(target.points_[i], inv_voxel_size)] + .AddPoint(target.points_[i], i); } VoxelMap voxel_map; @@ -205,64 +227,214 @@ VoxelMap BuildVoxelGaussians(const geometry::PointCloud &target, gaussian.information = solver.eigenvectors() * eigenvalues.cwiseInverse().asDiagonal() * solver.eigenvectors().transpose(); - voxel_map.emplace(item.first, gaussian); - } - - for (int i = 0; i < static_cast(target.points_.size()); ++i) { - const Eigen::Vector3d &point = target.points_[i]; - auto voxel_itr = voxel_map.find(GetVoxelKey(point, inv_voxel_size)); - if (voxel_itr == voxel_map.end()) { - continue; - } - VoxelGaussian &gaussian = voxel_itr->second; - const double distance2 = (point - gaussian.mean).squaredNorm(); - if (gaussian.representative_index < 0 || - distance2 < (target.points_[gaussian.representative_index] - + for (const int point_index : accumulator.point_indices) { + const double distance2 = + (target.points_[point_index] - gaussian.mean).squaredNorm(); + if (gaussian.representative_index < 0 || + distance2 < + (target.points_[gaussian.representative_index] - gaussian.mean) .squaredNorm()) { - gaussian.representative_index = i; + gaussian.representative_index = point_index; + } } + voxel_map.emplace(item.first, gaussian); } return voxel_map; } +struct NDTLinearSystemReducer { + const geometry::PointCloud &source_transformed; + const geometry::PointCloud ⌖ + const VoxelMap &voxel_map; + const NeighborOffsets &offsets; + std::size_t offset_count; + double inv_voxel_size; + double outlier_threshold; + NDTLinearSystem system; + + NDTLinearSystemReducer(const geometry::PointCloud &source_transformed_, + const geometry::PointCloud &target_, + const VoxelMap &voxel_map_, + const NeighborOffsets &offsets_, + std::size_t offset_count_, + double inv_voxel_size_, + double outlier_threshold_) + : source_transformed(source_transformed_), + target(target_), + voxel_map(voxel_map_), + offsets(offsets_), + offset_count(offset_count_), + inv_voxel_size(inv_voxel_size_), + outlier_threshold(outlier_threshold_) {} + + NDTLinearSystemReducer(NDTLinearSystemReducer &other, tbb::split) + : source_transformed(other.source_transformed), + target(other.target), + voxel_map(other.voxel_map), + offsets(other.offsets), + offset_count(other.offset_count), + inv_voxel_size(other.inv_voxel_size), + outlier_threshold(other.outlier_threshold) {} + + void operator()(const tbb::blocked_range &range) { + for (std::size_t i = range.begin(); i < range.end(); ++i) { + const Eigen::Vector3d &point = source_transformed.points_[i]; + const VoxelKey key = GetVoxelKey(point, inv_voxel_size); + double best_residual2 = outlier_threshold; + int best_target_index = -1; + for (std::size_t j = 0; j < offset_count; ++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 Eigen::Matrix3d &information = + voxel_itr->second.information; + const double distance = + diff.transpose() * information * diff; + if (!std::isfinite(distance) || distance > outlier_threshold) { + continue; + } + + if (distance <= best_residual2) { + best_residual2 = distance; + best_target_index = voxel_itr->second.representative_index; + } + + Eigen::Matrix jacobian; + jacobian.block<3, 3>(0, 0) = -utility::SkewMatrix(point); + jacobian.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); + system.JTJ.noalias() += + jacobian.transpose() * information * jacobian; + system.JTr.noalias() += + jacobian.transpose() * information * diff; + system.residual2 += distance; + ++system.residual_count; + } + if (best_target_index >= 0) { + system.euclidean_error2 += + (point - target.points_[best_target_index]) + .squaredNorm(); + ++system.correspondence_count; + } + } + } + + void join(NDTLinearSystemReducer &other) { + system.JTJ += other.system.JTJ; + system.JTr += other.system.JTr; + system.residual2 += other.system.residual2; + system.residual_count += other.system.residual_count; + system.euclidean_error2 += other.system.euclidean_error2; + system.correspondence_count += other.system.correspondence_count; + } +}; + NDTLinearSystem ComputeNDTLinearSystem( const geometry::PointCloud &source_transformed, + const geometry::PointCloud &target, 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; - } + std::size_t offset_count; + const auto offsets = + GetNeighborOffsets(option.neighbor_search_type_, offset_count); + NDTLinearSystemReducer reducer(source_transformed, target, voxel_map, + offsets, + offset_count, + inv_voxel_size, option.outlier_threshold_); + tbb::parallel_reduce( + tbb::blocked_range( + 0, source_transformed.points_.size(), + utility::DefaultGrainSizeTBB()), + reducer); + return std::move(reducer.system); +} - 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; - } +struct NDTResultReducer { + const geometry::PointCloud &source_transformed; + const geometry::PointCloud ⌖ + const VoxelMap &voxel_map; + const NeighborOffsets &offsets; + std::size_t offset_count; + double inv_voxel_size; + double outlier_threshold; + CorrespondenceSet correspondences; + double euclidean_error2 = 0.0; - Eigen::Matrix jacobian; - jacobian.block<3, 3>(0, 0) = -utility::SkewMatrix(point); - jacobian.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); + NDTResultReducer(const geometry::PointCloud &source_transformed_, + const geometry::PointCloud &target_, + const VoxelMap &voxel_map_, + const NeighborOffsets &offsets_, + std::size_t offset_count_, + double inv_voxel_size_, + double outlier_threshold_) + : source_transformed(source_transformed_), + target(target_), + voxel_map(voxel_map_), + offsets(offsets_), + offset_count(offset_count_), + inv_voxel_size(inv_voxel_size_), + outlier_threshold(outlier_threshold_) {} + + NDTResultReducer(NDTResultReducer &other, tbb::split) + : source_transformed(other.source_transformed), + target(other.target), + voxel_map(other.voxel_map), + offsets(other.offsets), + offset_count(other.offset_count), + inv_voxel_size(other.inv_voxel_size), + outlier_threshold(other.outlier_threshold) {} + + void operator()(const tbb::blocked_range &range) { + for (std::size_t i = range.begin(); i < range.end(); ++i) { + const Eigen::Vector3d &point = source_transformed.points_[i]; + const VoxelKey key = GetVoxelKey(point, inv_voxel_size); + bool has_inlier = false; + double best_residual2 = outlier_threshold; + double best_euclidean_error2 = 0.0; + int best_target_index = -1; + for (std::size_t j = 0; j < offset_count; ++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(); + } + } - system.JTJ += jacobian.transpose() * information * jacobian; - system.JTr += jacobian.transpose() * information * diff; - system.residual2 += distance; - ++system.residual_count; + if (has_inlier) { + correspondences.emplace_back(static_cast(i), + best_target_index); + euclidean_error2 += best_euclidean_error2; + } } } - return system; -} + + void join(NDTResultReducer &other) { + correspondences.insert(correspondences.end(), + other.correspondences.begin(), + other.correspondences.end()); + euclidean_error2 += other.euclidean_error2; + } +}; RegistrationResult EvaluateNDTResult( const geometry::PointCloud &source_transformed, @@ -271,49 +443,19 @@ RegistrationResult EvaluateNDTResult( 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; - } - } - + std::size_t offset_count; + const auto offsets = + GetNeighborOffsets(option.neighbor_search_type_, offset_count); + NDTResultReducer reducer(source_transformed, target, voxel_map, offsets, + offset_count, + inv_voxel_size, option.outlier_threshold_); + tbb::parallel_reduce( + tbb::blocked_range( + 0, source_transformed.points_.size(), + utility::DefaultGrainSizeTBB()), + reducer); + result.correspondence_set_ = std::move(reducer.correspondences); if (!result.correspondence_set_.empty()) { const double correspondence_count = static_cast(result.correspondence_set_.size()); @@ -321,7 +463,7 @@ RegistrationResult EvaluateNDTResult( correspondence_count / static_cast(source_transformed.points_.size()); result.inlier_rmse_ = - std::sqrt(euclidean_error2 / correspondence_count); + std::sqrt(reducer.euclidean_error2 / correspondence_count); } return result; } @@ -371,13 +513,11 @@ RegistrationResult RegistrationNDT( 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); + ComputeNDTLinearSystem(pcd, target, voxel_map, option); if (system.residual_count < 6) { utility::LogWarning( @@ -390,7 +530,8 @@ RegistrationResult RegistrationNDT( utility::LogDebug( "NDT Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}, " "mean Mahalanobis objective {:.4f}", - i, result.fitness_, result.inlier_rmse_, objective); + i, system.Fitness(pcd.points_.size()), system.InlierRMSE(), + objective); if (i > 0) { const double relative_objective_change = std::abs(previous_objective - objective) / @@ -455,15 +596,12 @@ RegistrationResult RegistrationNDT( transformation = candidate_transformation; pcd.Transform(update); - result = EvaluateNDTResult(pcd, target, transformation, voxel_map, - option); - if (update_vector.norm() < option.transformation_epsilon_) { break; } } - return result; + return EvaluateNDTResult(pcd, target, transformation, voxel_map, option); } } // namespace registration diff --git a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h index 7dae0670441..3c5a265f8ea 100644 --- a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h +++ b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.h @@ -20,8 +20,6 @@ class PointCloud; namespace pipelines { namespace registration { -class RegistrationResult; - /// \class NormalDistributionsTransformOption /// /// \brief Class that defines options for 3D Normal Distributions Transform diff --git a/docs/tutorial/pipelines/ndt_registration.rst b/docs/tutorial/pipelines/ndt_registration.rst index 60edcfa367a..6bce9ea94e3 100644 --- a/docs/tutorial/pipelines/ndt_registration.rst +++ b/docs/tutorial/pipelines/ndt_registration.rst @@ -6,9 +6,12 @@ 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]_. +This implementation is a Gauss-Newton point-to-distribution variant of the +Normal Distributions Transform. It is based on the voxel model and practical +3D formulation described by Gao [Gao2023]_, while the original NDT method was +introduced by Biber and Straßer [BiberAndStrasser2003]_. Unlike the original +Newton optimization of a Gaussian score, this variant minimizes squared +Mahalanobis residuals. Open3D exposes NDT through ``open3d.pipelines.registration.registration_ndt``. The main parameters are From ce1b17d91865e6c009eb4a747893a061c3cf5ec6 Mon Sep 17 00:00:00 2001 From: Sameer Sheorey Date: Wed, 19 Aug 2026 14:43:11 -0700 Subject: [PATCH 4/5] Fix main CI failures: SYCL Python version & Windows RPC port - Dockerfile.ci: force CMake Python3_EXECUTABLE to matrix interpreter to prevent inherited pyenv shim selecting Python 3.12 in all jobs. - Dockerfile.ci: remove inherited /open3d*.whl before exporting newly built matrix wheel so downstream steps never see a stale cp312 wheel. - ubuntu-sycl.yml: filter exported wheels to the matrix ABI and verify the correct wheel exists before downstream steps consume it. - ZMQReceiver: add GetLastEndpoint() using zmq::sockopt::last_endpoint. - RemoteFunctions test: bind tcp://localhost:0, query resolved address via GetLastEndpoint(), avoiding Windows reserved port 51454. --- .github/workflows/ubuntu-sycl.yml | 7 +++++ cpp/open3d/io/rpc/DummyReceiver.h | 1 + cpp/open3d/io/rpc/ZMQReceiver.cpp | 5 ++++ cpp/open3d/io/rpc/ZMQReceiver.h | 4 +++ cpp/tests/io/rpc/RemoteFunctions.cpp | 43 +++++++++++----------------- docker/Dockerfile.ci | 6 +++- 6 files changed, 39 insertions(+), 27 deletions(-) diff --git a/.github/workflows/ubuntu-sycl.yml b/.github/workflows/ubuntu-sycl.yml index 066d523c8b0..1935a66bd4f 100644 --- a/.github/workflows/ubuntu-sycl.yml +++ b/.github/workflows/ubuntu-sycl.yml @@ -171,6 +171,13 @@ jobs: # We execute docker buildscript with python version argument. docker/docker_build.sh sycl-shared py${{ matrix.python_version }} + # Wheel produced by the Docker build matches the matrix Python version + # via -DPython3_EXECUTABLE in Dockerfile.ci. Keep only that wheel. + PYTHON_ABI="cp${PYTHON_VERSION/./}" + find . -maxdepth 1 -name 'open3d*.whl' \ + ! -name "*${PYTHON_ABI}-${PYTHON_ABI}-*.whl" -delete + compgen -G "open3d*${PYTHON_ABI}-${PYTHON_ABI}-*.whl" > /dev/null + - name: Docker test (Python) run: docker/docker_test.sh sycl-shared python diff --git a/cpp/open3d/io/rpc/DummyReceiver.h b/cpp/open3d/io/rpc/DummyReceiver.h index 71eef978b5b..dc9a6f8ea5e 100644 --- a/cpp/open3d/io/rpc/DummyReceiver.h +++ b/cpp/open3d/io/rpc/DummyReceiver.h @@ -19,6 +19,7 @@ namespace rpc { class DummyReceiver : public ZMQReceiver { public: DummyReceiver(const std::string& address, int timeout); + using ZMQReceiver::GetLastEndpoint; }; } // namespace rpc diff --git a/cpp/open3d/io/rpc/ZMQReceiver.cpp b/cpp/open3d/io/rpc/ZMQReceiver.cpp index 4f72310883c..419df3095b1 100644 --- a/cpp/open3d/io/rpc/ZMQReceiver.cpp +++ b/cpp/open3d/io/rpc/ZMQReceiver.cpp @@ -92,6 +92,11 @@ std::runtime_error ZMQReceiver::GetLastError() { return result; } +std::string ZMQReceiver::GetLastEndpoint() const { + if (!socket_) return ""; + return socket_->get(zmq::sockopt::last_endpoint); +} + void ZMQReceiver::Mainloop() { context_ = GetZMQContext(); socket_ = std::unique_ptr( diff --git a/cpp/open3d/io/rpc/ZMQReceiver.h b/cpp/open3d/io/rpc/ZMQReceiver.h index eb3bf21f54e..8cfedecc892 100644 --- a/cpp/open3d/io/rpc/ZMQReceiver.h +++ b/cpp/open3d/io/rpc/ZMQReceiver.h @@ -60,6 +60,10 @@ class ZMQReceiver { /// Returns the last error from the mainloop thread. std::runtime_error GetLastError(); + /// Returns the endpoint the OS actually bound to (e.g. when address_ is + /// tcp://localhost:0). Must be called after Start() succeeds. + std::string GetLastEndpoint() const; + /// Sets the message processor object which will process incoming messages. void SetMessageProcessor(std::shared_ptr processor); diff --git a/cpp/tests/io/rpc/RemoteFunctions.cpp b/cpp/tests/io/rpc/RemoteFunctions.cpp index 989d3e5e510..63cb67bdf6a 100644 --- a/cpp/tests/io/rpc/RemoteFunctions.cpp +++ b/cpp/tests/io/rpc/RemoteFunctions.cpp @@ -23,11 +23,7 @@ using namespace open3d::io::rpc; namespace open3d { namespace tests { -#ifdef _WIN32 -const std::string connection_address = "tcp://127.0.0.1:51454"; -#else -const std::string connection_address = "ipc:///tmp/open3d_ipc"; -#endif +const std::string connection_address = "tcp://localhost:0"; class RemoteFunctions : public testing::Test { public: @@ -36,69 +32,64 @@ class RemoteFunctions : public testing::Test { TEST_F(RemoteFunctions, SendReceiveUnpackMessages) { { - // start receiver DummyReceiver receiver(connection_address, 500); receiver.Start(); + const std::string addr = receiver.GetLastEndpoint(); geometry::PointCloud pcd; pcd.points_.push_back(Eigen::Vector3d(1, 2, 3)); - auto connection = - std::make_shared(connection_address, 500, 500); + auto connection = std::make_shared(addr, 500, 500); ASSERT_TRUE(SetPointCloud(pcd, "", 0, "", connection)); receiver.Stop(); } { - // start receiver DummyReceiver receiver(connection_address, 500); receiver.Start(); + const std::string addr = receiver.GetLastEndpoint(); geometry::TriangleMesh mesh; mesh.vertices_.push_back(Eigen::Vector3d(1, 2, 3)); mesh.vertices_.push_back(Eigen::Vector3d(1, 2, 3)); mesh.vertices_.push_back(Eigen::Vector3d(1, 2, 3)); mesh.triangles_.push_back(Eigen::Vector3i(0, 1, 2)); - auto connection = - std::make_shared(connection_address, 500, 500); + auto connection = std::make_shared(addr, 500, 500); ASSERT_TRUE(SetTriangleMesh(mesh, "", 0, "", connection)); receiver.Stop(); } { - // start receiver DummyReceiver receiver(connection_address, 500); receiver.Start(); + const std::string addr = receiver.GetLastEndpoint(); camera::PinholeCameraParameters cam; - auto connection = - std::make_shared(connection_address, 500, 500); + auto connection = std::make_shared(addr, 500, 500); ASSERT_TRUE(SetLegacyCamera(cam, "", 0, "", connection)); receiver.Stop(); } { - // start receiver DummyReceiver receiver(connection_address, 500); receiver.Start(); + const std::string addr = receiver.GetLastEndpoint(); - auto connection = - std::make_shared(connection_address, 500, 500); + auto connection = std::make_shared(addr, 500, 500); ASSERT_TRUE(SetTime(0, connection)); receiver.Stop(); } { - // start receiver DummyReceiver receiver(connection_address, 500); receiver.Start(); + const std::string addr = receiver.GetLastEndpoint(); - auto connection = - std::make_shared(connection_address, 500, 500); + auto connection = std::make_shared(addr, 500, 500); ASSERT_TRUE(SetActiveCamera("group/mycam", connection)); receiver.Stop(); } // chain multiple messages to test if the receiver can handle this { - // start receiver DummyReceiver receiver(connection_address, 500); receiver.Start(); + const std::string addr = receiver.GetLastEndpoint(); geometry::PointCloud pcd; pcd.points_.push_back(Eigen::Vector3d(1, 2, 3)); @@ -110,8 +101,7 @@ TEST_F(RemoteFunctions, SendReceiveUnpackMessages) { ASSERT_TRUE(SetTime(0, buf_connection)); - auto connection = - std::make_shared(connection_address, 500, 500); + auto connection = std::make_shared(addr, 500, 500); std::string buf = buf_connection->buffer().str(); auto reply = connection->Send(buf.data(), buf.size()); @@ -141,13 +131,14 @@ TEST_F(RemoteFunctions, SendGarbage) { // start receiver DummyReceiver receiver(connection_address, 500); receiver.Start(); + const std::string addr = receiver.GetLastEndpoint(); // send invalid msg id { std::string data = CreateSerializedRequestMessage("bla123"); // send to receiver - Connection connection(connection_address, 500, 500); + Connection connection(addr, 500, 500); auto reply = connection.Send(data.data(), data.size()); const void* reply_data; size_t reply_size; @@ -176,7 +167,7 @@ TEST_F(RemoteFunctions, SendGarbage) { buf_connection.Send(data.data(), data.size()); // send to receiver - Connection connection(connection_address, 500, 500); + Connection connection(addr, 500, 500); std::string buf = buf_connection.buffer().str(); auto reply = connection.Send(buf.data(), buf.size()); const void* reply_data; @@ -204,7 +195,7 @@ TEST_F(RemoteFunctions, SendGarbage) { buf_connection.Send(data.data(), data.size()); // send to receiver - Connection connection(connection_address, 500, 500); + Connection connection(addr, 500, 500); std::string buf = buf_connection.buffer().str(); auto reply = connection.Send(buf.data(), buf.size()); const void* reply_data; diff --git a/docker/Dockerfile.ci b/docker/Dockerfile.ci index ce1a2d0b5b5..8e3b510d750 100755 --- a/docker/Dockerfile.ci +++ b/docker/Dockerfile.ci @@ -191,6 +191,7 @@ RUN \ -DCMAKE_BUILD_TYPE=Release \ -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} \ -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER} \ + -DPython3_EXECUTABLE=${PYENV_ROOT}/versions/${PYTHON_VERSION}/bin/python \ -DBUILD_SYCL_MODULE=${BUILD_SYCL_MODULE} \ -DBUILD_ISPC_MODULE=${BUILD_ISPC_MODULE} \ -DDEVELOPER_BUILD=${DEVELOPER_BUILD} \ @@ -224,7 +225,10 @@ RUN ccache -s \ && tar -caf /${CCACHE_TAR_NAME}.tar.xz ${CCACHE_DIR_NAME} \ && if [[ "${PACKAGE}" = "ON" ]]; then mv /root/Open3D/build/package/open3d-devel*.tar.xz /; fi \ && if [[ "${PACKAGE}" = "VIEWER" ]]; then mv /root/Open3D/build/package-Open3DViewer-deb/open3d-viewer-*-Linux.deb /; fi \ - && if [[ "${BUILD_SHARED_LIBS}" = "ON" ]]; then mv /root/Open3D/build/lib/python_package/pip_package/open3d*.whl /; fi \ + && if [[ "${BUILD_SHARED_LIBS}" = "ON" ]]; then \ + rm -f /open3d*.whl; \ + mv /root/Open3D/build/lib/python_package/pip_package/open3d*.whl /; \ + fi \ && ls -alh / RUN echo "Docker build done." \ No newline at end of file From b77c9b29ffd17ee8cc4719c6eee6574969643067 Mon Sep 17 00:00:00 2001 From: Sameer Sheorey Date: Wed, 19 Aug 2026 14:45:26 -0700 Subject: [PATCH 5/5] style --- .../NormalDistributionsTransform.cpp | 72 +++++++++---------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp index 8fdd598f34b..2ecd74c9d14 100644 --- a/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp +++ b/cpp/open3d/pipelines/registration/NormalDistributionsTransform.cpp @@ -13,9 +13,10 @@ // It models target voxels as regularized Gaussians, rejects outliers by their // Mahalanobis distance, and applies left-perturbation SE(3) updates. -#include #include #include + +#include #include #include #include @@ -143,8 +144,12 @@ using NeighborOffsets = std::array; NeighborOffsets GetNeighborOffsets(int neighbor_search_type, std::size_t &offset_count) { - NeighborOffsets offsets{{{0, 0, 0}, {-1, 0, 0}, {1, 0, 0}, - {0, -1, 0}, {0, 1, 0}, {0, 0, -1}, + NeighborOffsets offsets{{{0, 0, 0}, + {-1, 0, 0}, + {1, 0, 0}, + {0, -1, 0}, + {0, 1, 0}, + {0, 0, -1}, {0, 0, 1}}}; offset_count = 1; if (neighbor_search_type == 1) { @@ -231,10 +236,9 @@ VoxelMap BuildVoxelGaussians(const geometry::PointCloud &target, const double distance2 = (target.points_[point_index] - gaussian.mean).squaredNorm(); if (gaussian.representative_index < 0 || - distance2 < - (target.points_[gaussian.representative_index] - - gaussian.mean) - .squaredNorm()) { + distance2 < (target.points_[gaussian.representative_index] - + gaussian.mean) + .squaredNorm()) { gaussian.representative_index = point_index; } } @@ -261,7 +265,7 @@ struct NDTLinearSystemReducer { double inv_voxel_size_, double outlier_threshold_) : source_transformed(source_transformed_), - target(target_), + target(target_), voxel_map(voxel_map_), offsets(offsets_), offset_count(offset_count_), @@ -270,7 +274,7 @@ struct NDTLinearSystemReducer { NDTLinearSystemReducer(NDTLinearSystemReducer &other, tbb::split) : source_transformed(other.source_transformed), - target(other.target), + target(other.target), voxel_map(other.voxel_map), offsets(other.offsets), offset_count(other.offset_count), @@ -295,8 +299,7 @@ struct NDTLinearSystemReducer { const Eigen::Vector3d diff = point - voxel_itr->second.mean; const Eigen::Matrix3d &information = voxel_itr->second.information; - const double distance = - diff.transpose() * information * diff; + const double distance = diff.transpose() * information * diff; if (!std::isfinite(distance) || distance > outlier_threshold) { continue; } @@ -337,22 +340,20 @@ struct NDTLinearSystemReducer { NDTLinearSystem ComputeNDTLinearSystem( const geometry::PointCloud &source_transformed, - const geometry::PointCloud &target, + const geometry::PointCloud &target, const VoxelMap &voxel_map, const NormalDistributionsTransformOption &option) { const double inv_voxel_size = 1.0 / option.voxel_size_; std::size_t offset_count; const auto offsets = - GetNeighborOffsets(option.neighbor_search_type_, offset_count); + GetNeighborOffsets(option.neighbor_search_type_, offset_count); NDTLinearSystemReducer reducer(source_transformed, target, voxel_map, - offsets, - offset_count, - inv_voxel_size, option.outlier_threshold_); - tbb::parallel_reduce( - tbb::blocked_range( - 0, source_transformed.points_.size(), - utility::DefaultGrainSizeTBB()), - reducer); + offsets, offset_count, inv_voxel_size, + option.outlier_threshold_); + tbb::parallel_reduce(tbb::blocked_range( + 0, source_transformed.points_.size(), + utility::DefaultGrainSizeTBB()), + reducer); return std::move(reducer.system); } @@ -370,15 +371,15 @@ struct NDTResultReducer { NDTResultReducer(const geometry::PointCloud &source_transformed_, const geometry::PointCloud &target_, const VoxelMap &voxel_map_, - const NeighborOffsets &offsets_, - std::size_t offset_count_, + const NeighborOffsets &offsets_, + std::size_t offset_count_, double inv_voxel_size_, double outlier_threshold_) : source_transformed(source_transformed_), target(target_), voxel_map(voxel_map_), offsets(offsets_), - offset_count(offset_count_), + offset_count(offset_count_), inv_voxel_size(inv_voxel_size_), outlier_threshold(outlier_threshold_) {} @@ -387,7 +388,7 @@ struct NDTResultReducer { target(other.target), voxel_map(other.voxel_map), offsets(other.offsets), - offset_count(other.offset_count), + offset_count(other.offset_count), inv_voxel_size(other.inv_voxel_size), outlier_threshold(other.outlier_threshold) {} @@ -399,10 +400,10 @@ struct NDTResultReducer { double best_residual2 = outlier_threshold; double best_euclidean_error2 = 0.0; int best_target_index = -1; - for (std::size_t j = 0; j < offset_count; ++j) { - const VoxelKey neighbor{key.x + offsets[j].x, - key.y + offsets[j].y, - key.z + offsets[j].z}; + for (std::size_t j = 0; j < offset_count; ++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; @@ -448,13 +449,12 @@ RegistrationResult EvaluateNDTResult( const auto offsets = GetNeighborOffsets(option.neighbor_search_type_, offset_count); NDTResultReducer reducer(source_transformed, target, voxel_map, offsets, - offset_count, - inv_voxel_size, option.outlier_threshold_); - tbb::parallel_reduce( - tbb::blocked_range( - 0, source_transformed.points_.size(), - utility::DefaultGrainSizeTBB()), - reducer); + offset_count, inv_voxel_size, + option.outlier_threshold_); + tbb::parallel_reduce(tbb::blocked_range( + 0, source_transformed.points_.size(), + utility::DefaultGrainSizeTBB()), + reducer); result.correspondence_set_ = std::move(reducer.correspondences); if (!result.correspondence_set_.empty()) { const double correspondence_count =