From aefe5db5f79ec69c75350dfcf9d6b62ec7adc9d5 Mon Sep 17 00:00:00 2001 From: yaoyx689 Date: Wed, 11 Dec 2024 19:36:34 +0800 Subject: [PATCH 01/28] add a part of the FRICP (robust transformation estimation) --- registration/CMakeLists.txt | 3 + ...ation_estimation_point_to_point_robust.hpp | 269 ++++++++++++++++++ ...rmation_estimation_point_to_point_robust.h | 176 ++++++++++++ ...ation_estimation_point_to_point_robust.cpp | 39 +++ 4 files changed, 487 insertions(+) create mode 100755 registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp create mode 100755 registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h create mode 100755 registration/src/transformation_estimation_point_to_point_robust.cpp diff --git a/registration/CMakeLists.txt b/registration/CMakeLists.txt index ab15ffc2cc2..1440052dffa 100644 --- a/registration/CMakeLists.txt +++ b/registration/CMakeLists.txt @@ -53,6 +53,7 @@ set(incs "include/pcl/${SUBSYS_NAME}/transformation_estimation_2D.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_svd.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_svd_scale.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_point_robust.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_dual_quaternion.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_lm.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane.h" @@ -103,6 +104,7 @@ set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/registration.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_2D.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_svd.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_point_robust.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_svd_scale.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_dual_quaternion.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_lm.hpp" @@ -154,6 +156,7 @@ set(srcs src/transformation_estimation_svd_scale.cpp src/transformation_estimation_dual_quaternion.cpp src/transformation_estimation_lm.cpp + src/transformation_estimation_point_to_point_robust.cpp src/transformation_estimation_point_to_plane_weighted.cpp src/transformation_estimation_point_to_plane_lls.cpp src/transformation_estimation_point_to_plane_lls_weighted.cpp diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp new file mode 100755 index 00000000000..853fb5b53d0 --- /dev/null +++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp @@ -0,0 +1,269 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_POINT_ROBUST_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_POINT_ROBUST_HPP_ + +#include + +namespace pcl { + +namespace registration { + +template +inline void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const +{ + const auto nr_points = cloud_src.size(); + if (cloud_tgt.size() != nr_points) { + PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::estimateRigidTransformation] Number " + "or points in source (%zu) differs than target (%zu)!\n", + static_cast(nr_points), + static_cast(cloud_tgt.size())); + return; + } + + ConstCloudIterator source_it(cloud_src); + ConstCloudIterator target_it(cloud_tgt); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::Indices& indices_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const +{ + if (indices_src.size() != cloud_tgt.size()) { + PCL_ERROR("[pcl::TransformationSVD::estimateRigidTransformation] Number or points " + "in source (%zu) differs than target (%zu)!\n", + indices_src.size(), + static_cast(cloud_tgt.size())); + return; + } + + ConstCloudIterator source_it(cloud_src, indices_src); + ConstCloudIterator target_it(cloud_tgt); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +inline void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::Indices& indices_src, + const pcl::PointCloud& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix) const +{ + if (indices_src.size() != indices_tgt.size()) { + PCL_ERROR("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number " + "or points in source (%zu) differs than target (%zu)!\n", + indices_src.size(), + indices_tgt.size()); + return; + } + + ConstCloudIterator source_it(cloud_src, indices_src); + ConstCloudIterator target_it(cloud_tgt, indices_tgt); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + const pcl::Correspondences& correspondences, + Matrix4& transformation_matrix) const +{ + ConstCloudIterator source_it(cloud_src, correspondences, true); + ConstCloudIterator target_it(cloud_tgt, correspondences, false); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +inline void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4& transformation_matrix) const +{ + // Convert to Eigen format + const int npts = static_cast(source_it.size()); + source_it.reset(); + target_it.reset(); + Eigen::Matrix weights(npts); + Eigen::Matrix square_distances(npts); + for(int i = 0; i < npts; i++) + { + Scalar dx = source_it->x - target_it->x; + Scalar dy = source_it->y - target_it->y; + Scalar dz = source_it->z - target_it->z; + Scalar dist2 = dx*dx + dy*dy + dz*dz; + square_distances[i] = dist2; + + source_it++; + target_it++; + } + + Scalar sigma2; + if(sigma_ < 0) + sigma2 = square_distances.maxCoeff()/9.0; + else + sigma2 = sigma_*sigma_; + + for(int i = 0; i < npts; i++) + { + weights[i] = std::exp(-square_distances[i]/(2.0*sigma2)); + } + weights = weights/weights.sum(); + + source_it.reset(); + target_it.reset(); + // is the source dataset + transformation_matrix.setIdentity(); + + Eigen::Matrix centroid_src, centroid_tgt; + // Estimate the centroids of source, target + computeWeighted3DCentroid(source_it, weights, centroid_src); + computeWeighted3DCentroid(target_it, weights, centroid_tgt); + source_it.reset(); + target_it.reset(); + + // Subtract the centroids from source, target + Eigen::Matrix cloud_src_demean, + cloud_tgt_demean; + demeanPointCloud(source_it, centroid_src, cloud_src_demean); + demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean); + + getTransformationFromCorrelation(cloud_src_demean, + centroid_src, + cloud_tgt_demean, + centroid_tgt, + weights, + transformation_matrix); +} + +template +void +TransformationEstimationPointToPointRobust:: + getTransformationFromCorrelation( + const Eigen::Matrix& cloud_src_demean, + const Eigen::Matrix& centroid_src, + const Eigen::Matrix& cloud_tgt_demean, + const Eigen::Matrix& centroid_tgt, + const Eigen::Matrix& weights, + Matrix4& transformation_matrix) const +{ + transformation_matrix.setIdentity(); + + // Assemble the correlation matrix H = source * weights * target' + Eigen::Matrix H = + (cloud_src_demean * weights.asDiagonal() * cloud_tgt_demean.transpose()).template topLeftCorner<3, 3>(); + + // Compute the Singular Value Decomposition + Eigen::JacobiSVD> svd( + H, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix u = svd.matrixU(); + Eigen::Matrix v = svd.matrixV(); + + // Compute R = V * U' + if (u.determinant() * v.determinant() < 0) { + for (int x = 0; x < 3; ++x) + v(x, 2) *= -1; + } + + Eigen::Matrix R = v * u.transpose(); + + // Return the correct transformation + transformation_matrix.template topLeftCorner<3, 3>() = R; + const Eigen::Matrix Rc(R * centroid_src.template head<3>()); + transformation_matrix.template block<3, 1>(0, 3) = + centroid_tgt.template head<3>() - Rc; + + if (pcl::console::isVerbosityLevelEnabled(pcl::console::L_DEBUG)) { + size_t N = cloud_src_demean.cols(); + PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPointRobust::" + "getTransformationFromCorrelation] Loss: %.10e\n", + (cloud_tgt_demean - R * cloud_src_demean).squaredNorm() / N); + } +} + + +template inline unsigned int +TransformationEstimationPointToPointRobust::computeWeighted3DCentroid(ConstCloudIterator &cloud_iterator, Eigen::Matrix& weights, + Eigen::Matrix ¢roid) const +{ + Eigen::Matrix accumulator {0, 0, 0, 0}; + + unsigned int cp = 0; + + // For each point in the cloud + // If the data is dense, we don't need to check for NaN + while (cloud_iterator.isValid ()) + { + // Check if the point is invalid + if (pcl::isFinite (*cloud_iterator)) + { + accumulator[0] += weights[cp] * cloud_iterator->x; + accumulator[1] += weights[cp] * cloud_iterator->y; + accumulator[2] += weights[cp] * cloud_iterator->z; + ++cp; + } + ++cloud_iterator; + } + + if (cp > 0) { + centroid = accumulator; + centroid[3] = 1; + } + return (cp); +} + +} // namespace registration +} // namespace pcl + +#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_POINT_ROBUST_HPP_ */ diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h new file mode 100755 index 00000000000..bf92dfdd0b8 --- /dev/null +++ b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h @@ -0,0 +1,176 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include // for PCL_NO_PRECOMPILE + +namespace pcl { +namespace registration { +/** @b TransformationEstimationPointToPointRobust implements SVD-based estimation of + * the transformation aligning the given correspondences for minimizing the Welsch function instead of L2-norm, + * For additional details, see + * "Fast and Robust Iterative Closest Point", Juyong Zhang, Yuxin Yao, Bailin Deng, 2022. + * \note The class is templated on the source and target point types as well as on the + * output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Yuxin Yao + * \ingroup registration + */ +template +class TransformationEstimationPointToPointRobust +: public TransformationEstimation { +public: + using Ptr = shared_ptr>; + using ConstPtr = + shared_ptr>; + + using Matrix4 = + typename TransformationEstimation::Matrix4; + + /** \brief Constructor + * \param[in] use_umeyama Toggles whether or not to use 3rd party software*/ + TransformationEstimationPointToPointRobust() = default; + + ~TransformationEstimationPointToPointRobust() override = default; + + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using SVD. \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using SVD. \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in + * \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::Indices& indices_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using SVD. \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in + * \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the + * interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::Indices& indices_src, + const pcl::PointCloud& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using SVD. \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target + * point cloud \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + const pcl::Correspondences& correspondences, + Matrix4& transformation_matrix) const override; + + void set_sigma(Scalar sigma) { sigma_=sigma; }; + +protected: + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation(ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4& transformation_matrix) const; + + /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src + * * tgt' \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen + * format \param[in] centroid_src the input source centroid, in Eigen format + * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format + * \param[in] centroid_tgt the input target cloud, in Eigen format + * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix + */ + virtual void + getTransformationFromCorrelation( + const Eigen::Matrix& cloud_src_demean, + const Eigen::Matrix& centroid_src, + const Eigen::Matrix& cloud_tgt_demean, + const Eigen::Matrix& centroid_tgt, + const Eigen::Matrix& weights, + Matrix4& transformation_matrix) const; + +/** \brief Compute the 3D (X-Y-Z) centroid of a set of weighted points and return it as a 3D vector. + * \param[in] cloud_iterator an iterator over the input point cloud + * \param[in] weights the weights corresponding to points in the input point cloud + * \param[out] centroid the output centroid + * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. + * \note if return value is 0, the centroid is not changed, thus not valid. + * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices. + * \ingroup common + */ +inline unsigned int +computeWeighted3DCentroid(ConstCloudIterator &cloud_iterator, Eigen::Matrix& weights, Eigen::Matrix ¢roid) const; + + /** parameter for the Welsch function */ + Scalar sigma_ = -1; + + +}; + +} // namespace registration +} // namespace pcl + +#include diff --git a/registration/src/transformation_estimation_point_to_point_robust.cpp b/registration/src/transformation_estimation_point_to_point_robust.cpp new file mode 100755 index 00000000000..59dc078cdfe --- /dev/null +++ b/registration/src/transformation_estimation_point_to_point_robust.cpp @@ -0,0 +1,39 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) Alexandru-Eugen Ichim + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include From 213e84fe91691a19a4023fbe492811ea6b808191 Mon Sep 17 00:00:00 2001 From: yaoyx689 Date: Wed, 11 Dec 2024 20:23:52 +0800 Subject: [PATCH 02/28] update --- registration/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/registration/CMakeLists.txt b/registration/CMakeLists.txt index 1440052dffa..dfce3507105 100644 --- a/registration/CMakeLists.txt +++ b/registration/CMakeLists.txt @@ -53,7 +53,6 @@ set(incs "include/pcl/${SUBSYS_NAME}/transformation_estimation_2D.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_svd.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_svd_scale.h" - "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_point_robust.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_dual_quaternion.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_lm.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane.h" @@ -61,6 +60,7 @@ set(incs "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls_weighted.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_symmetric_point_to_plane_lls.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_point_robust.h" "include/pcl/${SUBSYS_NAME}/transformation_validation.h" "include/pcl/${SUBSYS_NAME}/transformation_validation_euclidean.h" "include/pcl/${SUBSYS_NAME}/gicp.h" @@ -104,7 +104,6 @@ set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/registration.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_2D.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_svd.hpp" - "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_point_robust.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_svd_scale.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_dual_quaternion.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_lm.hpp" @@ -112,6 +111,7 @@ set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_lls_weighted.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_weighted.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_point_robust.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_validation_euclidean.hpp" "include/pcl/${SUBSYS_NAME}/impl/gicp.hpp" "include/pcl/${SUBSYS_NAME}/impl/sample_consensus_prerejective.hpp" @@ -156,10 +156,10 @@ set(srcs src/transformation_estimation_svd_scale.cpp src/transformation_estimation_dual_quaternion.cpp src/transformation_estimation_lm.cpp - src/transformation_estimation_point_to_point_robust.cpp src/transformation_estimation_point_to_plane_weighted.cpp src/transformation_estimation_point_to_plane_lls.cpp src/transformation_estimation_point_to_plane_lls_weighted.cpp + src/transformation_estimation_point_to_point_robust.cpp src/transformation_validation_euclidean.cpp src/sample_consensus_prerejective.cpp ) From f2f6dea4c4e35dbc68b86a10b7a77f77f7f75fcb Mon Sep 17 00:00:00 2001 From: yaoyx689 Date: Tue, 17 Dec 2024 20:27:43 +0800 Subject: [PATCH 03/28] Correct format --- ...ation_estimation_point_to_point_robust.hpp | 124 +++++++++--------- ...rmation_estimation_point_to_point_robust.h | 50 ++++--- 2 files changed, 92 insertions(+), 82 deletions(-) mode change 100755 => 100644 registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp mode change 100755 => 100644 registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp old mode 100755 new mode 100644 index 853fb5b53d0..5aa83c41179 --- a/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp @@ -56,7 +56,8 @@ TransformationEstimationPointToPointRobust:: { const auto nr_points = cloud_src.size(); if (cloud_tgt.size() != nr_points) { - PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::estimateRigidTransformation] Number " + PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::" + "estimateRigidTransformation] Number " "or points in source (%zu) differs than target (%zu)!\n", static_cast(nr_points), static_cast(cloud_tgt.size())); @@ -133,58 +134,56 @@ TransformationEstimationPointToPointRobust:: { // Convert to Eigen format const int npts = static_cast(source_it.size()); - source_it.reset(); - target_it.reset(); - Eigen::Matrix weights(npts); - Eigen::Matrix square_distances(npts); - for(int i = 0; i < npts; i++) - { - Scalar dx = source_it->x - target_it->x; - Scalar dy = source_it->y - target_it->y; - Scalar dz = source_it->z - target_it->z; - Scalar dist2 = dx*dx + dy*dy + dz*dz; - square_distances[i] = dist2; - - source_it++; - target_it++; - } + source_it.reset(); + target_it.reset(); + Eigen::Matrix weights(npts); + Eigen::Matrix square_distances(npts); + for (int i = 0; i < npts; i++) { + Scalar dx = source_it->x - target_it->x; + Scalar dy = source_it->y - target_it->y; + Scalar dz = source_it->z - target_it->z; + Scalar dist2 = dx * dx + dy * dy + dz * dz; + square_distances[i] = dist2; - Scalar sigma2; - if(sigma_ < 0) - sigma2 = square_distances.maxCoeff()/9.0; - else - sigma2 = sigma_*sigma_; - - for(int i = 0; i < npts; i++) - { - weights[i] = std::exp(-square_distances[i]/(2.0*sigma2)); - } - weights = weights/weights.sum(); - - source_it.reset(); - target_it.reset(); - // is the source dataset - transformation_matrix.setIdentity(); - - Eigen::Matrix centroid_src, centroid_tgt; - // Estimate the centroids of source, target - computeWeighted3DCentroid(source_it, weights, centroid_src); - computeWeighted3DCentroid(target_it, weights, centroid_tgt); - source_it.reset(); - target_it.reset(); - - // Subtract the centroids from source, target - Eigen::Matrix cloud_src_demean, - cloud_tgt_demean; - demeanPointCloud(source_it, centroid_src, cloud_src_demean); - demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean); - - getTransformationFromCorrelation(cloud_src_demean, - centroid_src, - cloud_tgt_demean, - centroid_tgt, - weights, - transformation_matrix); + source_it++; + target_it++; + } + + Scalar sigma2; + if (sigma_ < 0) + sigma2 = square_distances.maxCoeff() / 9.0; + else + sigma2 = sigma_ * sigma_; + + for (int i = 0; i < npts; i++) { + weights[i] = std::exp(-square_distances[i] / (2.0 * sigma2)); + } + weights = weights / weights.sum(); + + source_it.reset(); + target_it.reset(); + // is the source dataset + transformation_matrix.setIdentity(); + + Eigen::Matrix centroid_src, centroid_tgt; + // Estimate the centroids of source, target + computeWeighted3DCentroid(source_it, weights, centroid_src); + computeWeighted3DCentroid(target_it, weights, centroid_tgt); + source_it.reset(); + target_it.reset(); + + // Subtract the centroids from source, target + Eigen::Matrix cloud_src_demean, + cloud_tgt_demean; + demeanPointCloud(source_it, centroid_src, cloud_src_demean); + demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean); + + getTransformationFromCorrelation(cloud_src_demean, + centroid_src, + cloud_tgt_demean, + centroid_tgt, + weights, + transformation_matrix); } template @@ -202,7 +201,8 @@ TransformationEstimationPointToPointRobust:: // Assemble the correlation matrix H = source * weights * target' Eigen::Matrix H = - (cloud_src_demean * weights.asDiagonal() * cloud_tgt_demean.transpose()).template topLeftCorner<3, 3>(); + (cloud_src_demean * weights.asDiagonal() * cloud_tgt_demean.transpose()) + .template topLeftCorner<3, 3>(); // Compute the Singular Value Decomposition Eigen::JacobiSVD> svd( @@ -232,22 +232,22 @@ TransformationEstimationPointToPointRobust:: } } - -template inline unsigned int -TransformationEstimationPointToPointRobust::computeWeighted3DCentroid(ConstCloudIterator &cloud_iterator, Eigen::Matrix& weights, - Eigen::Matrix ¢roid) const +template +inline unsigned int +TransformationEstimationPointToPointRobust:: + computeWeighted3DCentroid(ConstCloudIterator& cloud_iterator, + Eigen::Matrix& weights, + Eigen::Matrix& centroid) const { - Eigen::Matrix accumulator {0, 0, 0, 0}; + Eigen::Matrix accumulator{0, 0, 0, 0}; unsigned int cp = 0; // For each point in the cloud // If the data is dense, we don't need to check for NaN - while (cloud_iterator.isValid ()) - { + while (cloud_iterator.isValid()) { // Check if the point is invalid - if (pcl::isFinite (*cloud_iterator)) - { + if (pcl::isFinite(*cloud_iterator)) { accumulator[0] += weights[cp] * cloud_iterator->x; accumulator[1] += weights[cp] * cloud_iterator->y; accumulator[2] += weights[cp] * cloud_iterator->z; diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h old mode 100755 new mode 100644 index bf92dfdd0b8..59f74e62ae9 --- a/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h +++ b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h @@ -47,9 +47,9 @@ namespace pcl { namespace registration { /** @b TransformationEstimationPointToPointRobust implements SVD-based estimation of - * the transformation aligning the given correspondences for minimizing the Welsch function instead of L2-norm, - * For additional details, see - * "Fast and Robust Iterative Closest Point", Juyong Zhang, Yuxin Yao, Bailin Deng, 2022. + * the transformation aligning the given correspondences for minimizing the Welsch + * function instead of L2-norm, For additional details, see "Fast and Robust Iterative + * Closest Point", Juyong Zhang, Yuxin Yao, Bailin Deng, 2022. * \note The class is templated on the source and target point types as well as on the * output scalar of the transformation matrix (i.e., float or double). Default: float. * \author Yuxin Yao @@ -59,9 +59,12 @@ template class TransformationEstimationPointToPointRobust : public TransformationEstimation { public: - using Ptr = shared_ptr>; + using Ptr = shared_ptr< + TransformationEstimationPointToPointRobust>; using ConstPtr = - shared_ptr>; + shared_ptr>; using Matrix4 = typename TransformationEstimation::Matrix4; @@ -123,7 +126,11 @@ class TransformationEstimationPointToPointRobust const pcl::Correspondences& correspondences, Matrix4& transformation_matrix) const override; - void set_sigma(Scalar sigma) { sigma_=sigma; }; + void + set_sigma(Scalar sigma) + { + sigma_ = sigma; + }; protected: /** \brief Estimate a rigid rotation transformation between a source and a target @@ -152,23 +159,26 @@ class TransformationEstimationPointToPointRobust const Eigen::Matrix& weights, Matrix4& transformation_matrix) const; -/** \brief Compute the 3D (X-Y-Z) centroid of a set of weighted points and return it as a 3D vector. - * \param[in] cloud_iterator an iterator over the input point cloud - * \param[in] weights the weights corresponding to points in the input point cloud - * \param[out] centroid the output centroid - * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. - * \note if return value is 0, the centroid is not changed, thus not valid. - * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices. - * \ingroup common - */ -inline unsigned int -computeWeighted3DCentroid(ConstCloudIterator &cloud_iterator, Eigen::Matrix& weights, Eigen::Matrix ¢roid) const; + /** \brief Compute the 3D (X-Y-Z) centroid of a set of weighted points and return it + * as a 3D vector. + * \param[in] cloud_iterator an iterator over the input point cloud + * \param[in] weights the weights corresponding to points in the input point cloud + * \param[out] centroid the output centroid + * \return number of valid points used to determine the centroid. In case of dense + * point clouds, this is the same as the size of input cloud. + * \note if return value is 0, the centroid is not changed, thus not valid. + * The last component of the vector is set to 1, this allows to transform the centroid + * vector with 4x4 matrices. + * \ingroup common + */ + inline unsigned int + computeWeighted3DCentroid(ConstCloudIterator& cloud_iterator, + Eigen::Matrix& weights, + Eigen::Matrix& centroid) const; /** parameter for the Welsch function */ Scalar sigma_ = -1; - - -}; +}; } // namespace registration } // namespace pcl From 7c8128006445c243aec44668844d946a6ee6c0e6 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Sun, 9 Nov 2025 16:13:54 +0800 Subject: [PATCH 04/28] add Anderson acceleration fricp.hpp etc.UPdate cmakelist --- registration/CMakeLists.txt | 4 + .../pcl/registration/anderson_acceleration.h | 185 ++++++ registration/include/pcl/registration/fricp.h | 168 +++++ .../include/pcl/registration/impl/fricp.hpp | 595 ++++++++++++++++++ registration/src/fricp.cpp | 37 ++ 5 files changed, 989 insertions(+) create mode 100644 registration/include/pcl/registration/anderson_acceleration.h create mode 100644 registration/include/pcl/registration/fricp.h create mode 100644 registration/include/pcl/registration/impl/fricp.hpp create mode 100644 registration/src/fricp.cpp diff --git a/registration/CMakeLists.txt b/registration/CMakeLists.txt index dfce3507105..c0e4f740390 100644 --- a/registration/CMakeLists.txt +++ b/registration/CMakeLists.txt @@ -44,6 +44,7 @@ set(incs "include/pcl/${SUBSYS_NAME}/ndt.h" "include/pcl/${SUBSYS_NAME}/ndt_2d.h" "include/pcl/${SUBSYS_NAME}/ppf_registration.h" + "include/pcl/${SUBSYS_NAME}/anderson_acceleration.h" "include/pcl/${SUBSYS_NAME}/impl/pairwise_graph_registration.hpp" @@ -65,6 +66,7 @@ set(incs "include/pcl/${SUBSYS_NAME}/transformation_validation_euclidean.h" "include/pcl/${SUBSYS_NAME}/gicp.h" "include/pcl/${SUBSYS_NAME}/gicp6d.h" + "include/pcl/${SUBSYS_NAME}/fricp.h" "include/pcl/${SUBSYS_NAME}/bfgs.h" "include/pcl/${SUBSYS_NAME}/warp_point_rigid.h" "include/pcl/${SUBSYS_NAME}/warp_point_rigid_6d.h" @@ -114,6 +116,7 @@ set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_point_robust.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_validation_euclidean.hpp" "include/pcl/${SUBSYS_NAME}/impl/gicp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/fricp.hpp" "include/pcl/${SUBSYS_NAME}/impl/sample_consensus_prerejective.hpp" "include/pcl/${SUBSYS_NAME}/impl/ia_fpcs.hpp" "include/pcl/${SUBSYS_NAME}/impl/ia_kfpcs.hpp" @@ -143,6 +146,7 @@ set(srcs #src/pairwise_graph_registration.cpp src/ia_ransac.cpp src/icp.cpp + src/fricp.cpp src/joint_icp.cpp src/gicp.cpp src/gicp6d.cpp diff --git a/registration/include/pcl/registration/anderson_acceleration.h b/registration/include/pcl/registration/anderson_acceleration.h new file mode 100644 index 00000000000..843345ad3c0 --- /dev/null +++ b/registration/include/pcl/registration/anderson_acceleration.h @@ -0,0 +1,185 @@ +#pragma once + +#include +#include + +#include +#include + +#include +#include + +namespace pcl { +namespace registration { + +/** + * \brief Lightweight Anderson acceleration helper used to speed up fixed-point + * iterations in FRICP. + * + * The class stores a short history of the most recent residuals and solves the + * normal equations following the scheme described in "Fast and Robust Iterative + * Closest Point", Zhang et al., 2022. + * + * Only double precision is supported internally to maximise numerical stability. + */ +class AndersonAcceleration { +public: + AndersonAcceleration() = default; + + /** + * \brief Initialise the accelerator with a circular buffer of size \a history. + * + * \param[in] history Number of previous iterates to keep (m in the paper). + * \param[in] dimension Dimensionality of the flattened state vector. + * \param[in] initial_state Pointer to the initial state (expects + * \a dimension doubles). + */ + inline void + init(std::size_t history, std::size_t dimension, const double* initial_state) + { + if ((history == 0) || (dimension == 0)) { + PCL_THROW_EXCEPTION(pcl::BadArgumentException, + "AndersonAcceleration::init expects non-zero sizes"); + } + + history_length_ = history; + dimension_ = dimension; + iteration_ = 0; + column_index_ = 0; + initialized_ = true; + + current_u_.resize(dimension_); + current_F_.resize(dimension_); + prev_dG_.setZero(dimension_, history_length_); + prev_dF_.setZero(dimension_, history_length_); + normal_eq_matrix_.setZero(history_length_, history_length_); + theta_.setZero(history_length_); + dF_scale_.setZero(history_length_); + + current_u_ = Eigen::Map(initial_state, dimension_); + } + + inline bool + isInitialized() const + { + return initialized_; + } + + inline std::size_t + history() const + { + return history_length_; + } + + inline std::size_t + dimension() const + { + return dimension_; + } + + inline void + replace(const double* state) + { + if (!initialized_) + return; + current_u_ = Eigen::Map(state, dimension_); + } + + inline void + reset(const double* state) + { + if (!initialized_) + return; + iteration_ = 0; + column_index_ = 0; + current_u_ = Eigen::Map(state, dimension_); + } + + /** + * \brief Apply one Anderson acceleration update. + * + * \param[in] next_state Flattened state obtained from the fixed-point iteration. + * \return Reference to the accelerated state vector. + */ + inline const Eigen::VectorXd& + compute(const double* next_state) + { + if (!initialized_) { + PCL_THROW_EXCEPTION(pcl::PCLException, + "AndersonAcceleration::compute called before init"); + } + + Eigen::Map G(next_state, dimension_); + current_F_ = G - current_u_; + + constexpr double eps = 1e-14; + + if (iteration_ == 0) { + prev_dF_.col(0) = -current_F_; + prev_dG_.col(0) = -G; + current_u_ = G; + } + else { + prev_dF_.col(column_index_) += current_F_; + prev_dG_.col(column_index_) += G; + + double scale = std::max(eps, prev_dF_.col(column_index_).norm()); + dF_scale_(column_index_) = scale; + prev_dF_.col(column_index_) /= scale; + + const std::size_t m_k = std::min(history_length_, iteration_); + + if (m_k == 1) { + theta_(0) = 0.0; + const double dF_norm = prev_dF_.col(column_index_).norm(); + normal_eq_matrix_(0, 0) = dF_norm * dF_norm; + if (dF_norm > eps) { + theta_(0) = (prev_dF_.col(column_index_) / dF_norm).dot(current_F_ / dF_norm); + } + } + else { + // update Gram matrix row/column corresponding to the newest column + const Eigen::VectorXd new_inner_prod = prev_dF_.col(column_index_).transpose() * + prev_dF_.block(0, 0, dimension_, m_k); + normal_eq_matrix_.block(column_index_, 0, 1, m_k) = new_inner_prod.transpose(); + normal_eq_matrix_.block(0, column_index_, m_k, 1) = new_inner_prod; + + cod_.compute(normal_eq_matrix_.block(0, 0, m_k, m_k)); + theta_.head(m_k) = + cod_.solve(prev_dF_.block(0, 0, dimension_, m_k).transpose() * current_F_); + } + + const Eigen::ArrayXd scaled_theta = + theta_.head(m_k).array() / dF_scale_.head(m_k).array(); + current_u_ = G - prev_dG_.block(0, 0, dimension_, m_k) * scaled_theta.matrix(); + + column_index_ = (column_index_ + 1) % history_length_; + prev_dF_.col(column_index_) = -current_F_; + prev_dG_.col(column_index_) = -G; + } + + ++iteration_; + return current_u_; + } + +private: + std::size_t history_length_{0}; + std::size_t dimension_{0}; + std::size_t iteration_{0}; + std::size_t column_index_{0}; + bool initialized_{false}; + + Eigen::VectorXd current_u_; + Eigen::VectorXd current_F_; + Eigen::MatrixXd prev_dG_; + Eigen::MatrixXd prev_dF_; + Eigen::MatrixXd normal_eq_matrix_; + Eigen::VectorXd theta_; + Eigen::VectorXd dF_scale_; + Eigen::CompleteOrthogonalDecomposition cod_; + + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace registration +} // namespace pcl diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h new file mode 100644 index 00000000000..19303a92f50 --- /dev/null +++ b/registration/include/pcl/registration/fricp.h @@ -0,0 +1,168 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include + +namespace pcl { +/** + * \brief FastRobustIterativeClosestPoint implements the FRICP variant described in + * "Fast and Robust Iterative Closest Point", Zhang et al., 2022. + * + * The solver relies on Welsch reweighting for robustness and optional Anderson + * acceleration for faster convergence. + * + * \ingroup registration + */ +template +class FastRobustIterativeClosestPoint +: public IterativeClosestPoint { +public: + using Base = IterativeClosestPoint; + using typename Base::Matrix4; + using typename Base::PointCloudSource; + using typename Base::PointCloudTarget; + + enum class RobustFunction { NONE, WELSCH }; + + FastRobustIterativeClosestPoint(); + + void + setRobustFunction(RobustFunction f); + + [[nodiscard]] RobustFunction + getRobustFunction() const; + + void + setUseAndersonAcceleration(bool enabled); + + [[nodiscard]] bool + getUseAndersonAcceleration() const; + + void + setAndersonHistorySize(std::size_t history); + + [[nodiscard]] std::size_t + getAndersonHistorySize() const; + + void + setDynamicWelschBeginRatio(double ratio); + + void + setDynamicWelschEndRatio(double ratio); + + void + setDynamicWelschDecay(double ratio); + +protected: + void + computeTransformation(PointCloudSource& output, const Matrix4& guess) override; + +private: + using Matrix4d = Eigen::Matrix; + using Matrix3Xd = Eigen::Matrix; + using VectorXd = Eigen::VectorXd; + using Vector3d = Eigen::Vector3d; + + Matrix4d + convertGuessToCentered(const Matrix4& guess, + const Vector3d& source_mean, + const Vector3d& target_mean) const; + + Matrix4d + convertCenteredToActual(const Matrix4d& transform, + const Vector3d& source_mean, + const Vector3d& target_mean) const; + + bool + updateCorrespondences(const Matrix4d& transform, + const Matrix3Xd& source, + const Matrix3Xd& target, + pcl::search::KdTree& tree, + Matrix3Xd& matched_targets, + VectorXd& residuals) const; + + double + computeEnergy(const VectorXd& residuals, double nu) const; + + VectorXd + computeWeights(const VectorXd& residuals, double nu) const; + + Matrix4d + computeWeightedRigidTransform(const Matrix3Xd& source, + const Matrix3Xd& target, + const VectorXd& weights) const; + + double + findKNearestMedian(const pcl::PointCloud& cloud, + pcl::search::KdTree& tree, + int neighbors) const; + + double + computeMedian(const VectorXd& values) const; + + double + computeMedian(std::vector values) const; + + Matrix4d + matrixLog(const Matrix4d& transform) const; + + RobustFunction robust_function_; + bool use_anderson_; + std::size_t anderson_history_; + double nu_begin_ratio_; + double nu_end_ratio_; + double nu_decay_ratio_; + + static constexpr double same_threshold_ = 1e-6; + + AndersonAcceleration anderson_; + + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace pcl + +#include diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp new file mode 100644 index 00000000000..fec5ba4b6d6 --- /dev/null +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -0,0 +1,595 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions of source code must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_REGISTRATION_IMPL_FRICP_HPP_ +#define PCL_REGISTRATION_IMPL_FRICP_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace pcl { + +template +FastRobustIterativeClosestPoint:: + FastRobustIterativeClosestPoint() +: robust_function_(RobustFunction::WELSCH) +, use_anderson_(true) +, anderson_history_(5) +, nu_begin_ratio_(3.0) +, nu_end_ratio_(1.0 / (3.0 * std::sqrt(3.0))) +, nu_decay_ratio_(0.5) +{ + this->reg_name_ = "FastRobustIterativeClosestPoint"; + this->max_iterations_ = 50; + this->transformation_epsilon_ = static_cast(1e-6); + this->min_number_correspondences_ = 4; +} + +template +void +FastRobustIterativeClosestPoint::setRobustFunction( + RobustFunction f) +{ + robust_function_ = f; +} + +template +typename FastRobustIterativeClosestPoint::RobustFunction +FastRobustIterativeClosestPoint::getRobustFunction() + const +{ + return robust_function_; +} + +template +void +FastRobustIterativeClosestPoint:: + setUseAndersonAcceleration(bool enabled) +{ + use_anderson_ = enabled; +} + +template +bool +FastRobustIterativeClosestPoint:: + getUseAndersonAcceleration() const +{ + return use_anderson_; +} + +template +void +FastRobustIterativeClosestPoint:: + setAndersonHistorySize(std::size_t history) +{ + anderson_history_ = std::max(1, history); +} + +template +std::size_t +FastRobustIterativeClosestPoint:: + getAndersonHistorySize() const +{ + return anderson_history_; +} + +template +void +FastRobustIterativeClosestPoint:: + setDynamicWelschBeginRatio(double ratio) +{ + nu_begin_ratio_ = std::max(ratio, same_threshold_); +} + +template +void +FastRobustIterativeClosestPoint:: + setDynamicWelschEndRatio(double ratio) +{ + nu_end_ratio_ = std::max(ratio, same_threshold_); +} + +template +void +FastRobustIterativeClosestPoint:: + setDynamicWelschDecay(double ratio) +{ + nu_decay_ratio_ = std::max(0.0, std::min(1.0, ratio)); + if (nu_decay_ratio_ < same_threshold_) + nu_decay_ratio_ = 0.5; +} + +template +void +FastRobustIterativeClosestPoint:: + computeTransformation(PointCloudSource& output, const Matrix4& guess) +{ + pcl::IterativeClosestPoint::initComputeReciprocal(); + + if (!this->input_ || !this->target_) { + PCL_ERROR("[pcl::%s::computeTransformation] Invalid input clouds.\n", + this->getClassName().c_str()); + return; + } + + if (this->input_->empty() || this->target_->empty()) { + PCL_ERROR("[pcl::%s::computeTransformation] Empty input point clouds.\n", + this->getClassName().c_str()); + return; + } + + std::vector source_indices; + if (this->indices_ && !this->indices_->empty()) + source_indices.assign(this->indices_->begin(), this->indices_->end()); + else { + source_indices.resize(this->input_->size()); + std::iota(source_indices.begin(), source_indices.end(), 0); + } + + if (source_indices.size() < + static_cast(this->min_number_correspondences_)) { + PCL_ERROR("[pcl::%s::computeTransformation] Not enough source points (%zu).\n", + this->getClassName().c_str(), + source_indices.size()); + return; + } + + const std::size_t source_size = source_indices.size(); + const std::size_t target_size = this->target_->size(); + + if (target_size < static_cast(this->min_number_correspondences_)) { + PCL_ERROR("[pcl::%s::computeTransformation] Not enough target points (%zu).\n", + this->getClassName().c_str(), + target_size); + return; + } + + Matrix3Xd source_mat(3, source_size); + for (std::size_t i = 0; i < source_size; ++i) { + const auto& pt = (*this->input_)[source_indices[i]]; + source_mat.col(i) = pt.getVector3fMap().template cast(); + } + Vector3d source_mean = source_mat.rowwise().mean(); + source_mat.colwise() -= source_mean; + + Matrix3Xd target_mat(3, target_size); + for (std::size_t i = 0; i < target_size; ++i) { + const auto& pt = (*this->target_)[i]; + target_mat.col(i) = pt.getVector3fMap().template cast(); + } + Vector3d target_mean = target_mat.rowwise().mean(); + target_mat.colwise() -= target_mean; + + pcl::PointCloud::Ptr target_centered( + new pcl::PointCloud); + target_centered->resize(target_size); + for (std::size_t i = 0; i < target_size; ++i) { + (*target_centered)[i].x = static_cast(target_mat(0, i)); + (*target_centered)[i].y = static_cast(target_mat(1, i)); + (*target_centered)[i].z = static_cast(target_mat(2, i)); + } + + pcl::search::KdTree tree; + tree.setInputCloud(target_centered); + + Matrix4d transform_centered = convertGuessToCentered(guess, source_mean, target_mean); + Matrix4d svd_transform = transform_centered; + Matrix4d previous_transform = transform_centered; + + Matrix3Xd matched_targets(3, source_size); + VectorXd residuals(source_size); + if (!updateCorrespondences(transform_centered, + source_mat, + target_mat, + tree, + matched_targets, + residuals)) { + PCL_ERROR( + "[pcl::%s::computeTransformation] Failed to initialize correspondences.\n", + this->getClassName().c_str()); + return; + } + + const bool use_welsch = (robust_function_ == RobustFunction::WELSCH); + double nu_limit = 1.0; + double nu_current = 1.0; + if (use_welsch) { + const double neighbor_med = findKNearestMedian(*target_centered, tree, 7); + const double residual_med = computeMedian(residuals); + nu_limit = std::max(nu_end_ratio_ * neighbor_med, same_threshold_); + nu_current = std::max(nu_begin_ratio_ * residual_med, nu_limit); + } + + const double stop_threshold = (this->transformation_epsilon_ > 0) + ? static_cast(this->transformation_epsilon_) + : 1e-6; + + this->nr_iterations_ = 0; + this->converged_ = false; + + double last_energy = std::numeric_limits::max(); + if (use_anderson_) { + const Matrix4d log_state = matrixLog(transform_centered); + anderson_.init(anderson_history_, 16, log_state.data()); + } + + bool outer_done = false; + bool converged = false; + + while (!outer_done) { + for (int iter = 0; iter < this->max_iterations_; ++iter) { + double energy = + use_welsch ? computeEnergy(residuals, nu_current) : residuals.squaredNorm(); + if (use_anderson_) { + if (energy <= last_energy) { + last_energy = energy; + } + else { + transform_centered = svd_transform; + const Matrix4d log_state = matrixLog(transform_centered); + anderson_.replace(log_state.data()); + if (!updateCorrespondences(transform_centered, + source_mat, + target_mat, + tree, + matched_targets, + residuals)) { + PCL_ERROR("[pcl::%s::computeTransformation] Unable to recompute " + "correspondences during fallback.\n", + this->getClassName().c_str()); + return; + } + energy = use_welsch ? computeEnergy(residuals, nu_current) + : residuals.squaredNorm(); + last_energy = energy; + } + } + else { + last_energy = energy; + } + + VectorXd weights = use_welsch ? computeWeights(residuals, nu_current) + : VectorXd::Ones(residuals.size()); + Matrix4d candidate = + computeWeightedRigidTransform(source_mat, matched_targets, weights); + svd_transform = candidate; + transform_centered = candidate; + + if (use_anderson_) { + const Matrix4d log_matrix = matrixLog(transform_centered); + const Eigen::VectorXd& accelerated = anderson_.compute(log_matrix.data()); + transform_centered = Eigen::Map(accelerated.data()).exp(); + } + + if (!updateCorrespondences(transform_centered, + source_mat, + target_mat, + tree, + matched_targets, + residuals)) { + PCL_ERROR("[pcl::%s::computeTransformation] Failed to update " + "correspondences.\n", + this->getClassName().c_str()); + return; + } + + const double delta = (transform_centered - previous_transform).norm(); + previous_transform = transform_centered; + ++this->nr_iterations_; + + if (delta < stop_threshold) { + converged = true; + break; + } + } + + if (!use_welsch || (std::abs(nu_current - nu_limit) < same_threshold_)) { + outer_done = true; + } + else { + nu_current = std::max(nu_current * nu_decay_ratio_, nu_limit); + last_energy = std::numeric_limits::max(); + if (use_anderson_) { + const Matrix4d log_state = matrixLog(transform_centered); + anderson_.reset(log_state.data()); + } + } + } + + this->converged_ = converged; + + const Matrix4d final_transform = + convertCenteredToActual(transform_centered, source_mean, target_mean); + this->final_transformation_ = final_transform.template cast(); + this->transformation_ = this->final_transformation_; + this->previous_transformation_ = this->final_transformation_; + + pcl::transformPointCloud(*this->input_, output, this->final_transformation_); +} + +template +typename FastRobustIterativeClosestPoint::Matrix4d +FastRobustIterativeClosestPoint:: + convertGuessToCentered(const Matrix4& guess, + const Vector3d& source_mean, + const Vector3d& target_mean) const +{ + Matrix4d centered = Matrix4d::Identity(); + centered.block<3, 3>(0, 0) = guess.template block<3, 3>(0, 0).template cast(); + Vector3d translation = guess.template block<3, 1>(0, 3).template cast(); + centered.block<3, 1>(0, 3) = + centered.block<3, 3>(0, 0) * source_mean + translation - target_mean; + return centered; +} + +template +typename FastRobustIterativeClosestPoint::Matrix4d +FastRobustIterativeClosestPoint:: + convertCenteredToActual(const Matrix4d& transform, + const Vector3d& source_mean, + const Vector3d& target_mean) const +{ + Matrix4d actual = transform; + actual.block<3, 1>(0, 3) = transform.block<3, 1>(0, 3) - + transform.block<3, 3>(0, 0) * source_mean + target_mean; + return actual; +} + +template +bool +FastRobustIterativeClosestPoint:: + updateCorrespondences(const Matrix4d& transform, + const Matrix3Xd& source, + const Matrix3Xd& target, + pcl::search::KdTree& tree, + Matrix3Xd& matched_targets, + VectorXd& residuals) const +{ + const Eigen::Matrix3d R = transform.block<3, 3>(0, 0); + const Eigen::Vector3d t = transform.block<3, 1>(0, 3); + pcl::PointXYZ query; + std::vector nn_indices(1); + std::vector nn_sqr_dists(1); + + for (int i = 0; i < source.cols(); ++i) { + const Eigen::Vector3d current = R * source.col(i) + t; + query.x = static_cast(current.x()); + query.y = static_cast(current.y()); + query.z = static_cast(current.z()); + if (tree.nearestKSearch(query, 1, nn_indices, nn_sqr_dists) != 1) + return false; + const int idx = nn_indices[0]; + matched_targets.col(i) = target.col(idx); + residuals[i] = (current - matched_targets.col(i)).norm(); + } + return true; +} + +template +double +FastRobustIterativeClosestPoint::computeEnergy( + const VectorXd& residuals, double nu) const +{ + if (nu < same_threshold_) + nu = same_threshold_; + const double denom = 2.0 * nu * nu; + double energy = 0.0; + for (int i = 0; i < residuals.size(); ++i) { + const double dist2 = residuals[i] * residuals[i]; + energy += 1.0 - std::exp(-dist2 / denom); + } + return energy; +} + +template +typename FastRobustIterativeClosestPoint::VectorXd +FastRobustIterativeClosestPoint::computeWeights( + const VectorXd& residuals, double nu) const +{ + if (nu < same_threshold_) + nu = same_threshold_; + const double denom = 2.0 * nu * nu; + VectorXd weights(residuals.size()); + for (int i = 0; i < residuals.size(); ++i) { + const double dist2 = residuals[i] * residuals[i]; + weights[i] = std::exp(-dist2 / denom); + } + return weights; +} + +template +typename FastRobustIterativeClosestPoint::Matrix4d +FastRobustIterativeClosestPoint:: + computeWeightedRigidTransform(const Matrix3Xd& source, + const Matrix3Xd& target, + const VectorXd& weights) const +{ + Matrix4d transform = Matrix4d::Identity(); + VectorXd normalized = weights; + const double sum = normalized.sum(); + if (sum <= same_threshold_) { + normalized.setOnes(); + normalized /= static_cast(normalized.size()); + } + else { + normalized /= sum; + } + + const Vector3d source_mean = source * normalized; + const Vector3d target_mean = target * normalized; + const Matrix3Xd source_centered = source.colwise() - source_mean; + const Matrix3Xd target_centered = target.colwise() - target_mean; + Eigen::Matrix3d sigma = + source_centered * normalized.asDiagonal() * target_centered.transpose(); + Eigen::JacobiSVD svd(sigma, + Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose(); + if (R.determinant() < 0.0) { + Eigen::Matrix3d V = svd.matrixV(); + V.col(2) *= -1.0; + R = V * svd.matrixU().transpose(); + } + transform.block<3, 3>(0, 0) = R; + transform.block<3, 1>(0, 3) = target_mean - R * source_mean; + return transform; +} + +template +double +FastRobustIterativeClosestPoint::findKNearestMedian( + const pcl::PointCloud& cloud, + pcl::search::KdTree& tree, + int neighbors) const +{ + if (cloud.empty() || neighbors < 2) + return 0.0; + + const int k = std::min(neighbors, static_cast(cloud.size())); + if (k < 2) + return 0.0; + + std::vector local_medians(cloud.size(), 0.0); + std::vector nn_indices(k); + std::vector nn_sqr_dists(k); + std::vector dists; + dists.reserve(k - 1); + + for (std::size_t i = 0; i < cloud.size(); ++i) { + if (tree.nearestKSearch(cloud[i], k, nn_indices, nn_sqr_dists) != k) + continue; + dists.clear(); + for (int j = 1; j < k; ++j) + dists.push_back(std::sqrt(nn_sqr_dists[j])); + local_medians[i] = computeMedian(dists); + } + + return computeMedian(std::move(local_medians)); +} + +template +double +FastRobustIterativeClosestPoint::computeMedian( + const VectorXd& values) const +{ + std::vector buffer(static_cast(values.size())); + for (int i = 0; i < values.size(); ++i) + buffer[static_cast(i)] = values[i]; + return computeMedian(std::move(buffer)); +} + +template +double +FastRobustIterativeClosestPoint::computeMedian( + std::vector values) const +{ + if (values.empty()) + return 0.0; + auto mid = values.begin() + values.size() / 2; + std::nth_element(values.begin(), mid, values.end()); + if (values.size() % 2 == 1) + return *mid; + auto mid_prev = values.begin() + values.size() / 2 - 1; + std::nth_element(values.begin(), mid_prev, values.end()); + return (*mid + *mid_prev) * 0.5; +} + +template +typename FastRobustIterativeClosestPoint::Matrix4d +FastRobustIterativeClosestPoint::matrixLog( + const Matrix4d& transform) const +{ + Eigen::RealSchur schur(transform); + const Matrix4d U = schur.matrixU(); + const Matrix4d R = schur.matrixT(); + std::array selected{{true, true, true}}; + Eigen::Matrix3d B = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d V = Eigen::Matrix3d::Identity(); + + for (int i = 0; i < 3; ++i) { + if (!selected[i]) + continue; + if (std::abs(R(i, i) - 1.0) <= same_threshold_) + continue; + int partner = -1; + for (int j = i + 1; j < 3; ++j) { + if (std::abs(R(j, j) - R(i, i)) < same_threshold_) { + partner = j; + selected[j] = false; + break; + } + } + if (partner > 0) { + selected[i] = false; + const double diag = std::max(-1.0, std::min(1.0, static_cast(R(i, i)))); + double theta = std::acos(diag); + if (R(i, partner) < 0.0) + theta = -theta; + B(i, partner) += theta; + B(partner, i) -= theta; + V(i, partner) -= theta / 2.0; + V(partner, i) += theta / 2.0; + const double denom = 2.0 * (1.0 - R(i, i)); + if (std::abs(denom) > same_threshold_) { + const double coeff = 1.0 - (theta * R(i, partner)) / denom; + V(i, i) -= coeff; + V(partner, partner) -= coeff; + } + } + } + + Matrix4d trimmed = Matrix4d::Zero(); + trimmed.block<3, 3>(0, 0) = B; + trimmed.block<3, 1>(0, 3) = V * R.block<3, 1>(0, 3); + return U * trimmed * U.transpose(); +} + +} // namespace pcl + +#endif // PCL_REGISTRATION_IMPL_FRICP_HPP_ diff --git a/registration/src/fricp.cpp b/registration/src/fricp.cpp new file mode 100644 index 00000000000..6b0a0898b42 --- /dev/null +++ b/registration/src/fricp.cpp @@ -0,0 +1,37 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include From d87a99ea7e5eafa9b67979c47afab62916ea88ae Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Sun, 9 Nov 2025 18:54:28 +0800 Subject: [PATCH 05/28] fix bug --- .../pcl/registration/anderson_acceleration.h | 2 +- registration/include/pcl/registration/fricp.h | 3 +- .../include/pcl/registration/impl/fricp.hpp | 1 - test/registration/test_registration_api.cpp | 51 +++++++++++++++++++ 4 files changed, 54 insertions(+), 3 deletions(-) diff --git a/registration/include/pcl/registration/anderson_acceleration.h b/registration/include/pcl/registration/anderson_acceleration.h index 843345ad3c0..aa5d2e2e2cb 100644 --- a/registration/include/pcl/registration/anderson_acceleration.h +++ b/registration/include/pcl/registration/anderson_acceleration.h @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include #include diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index 19303a92f50..72b5a00d174 100644 --- a/registration/include/pcl/registration/fricp.h +++ b/registration/include/pcl/registration/fricp.h @@ -106,6 +106,7 @@ class FastRobustIterativeClosestPoint using Matrix3Xd = Eigen::Matrix; using VectorXd = Eigen::VectorXd; using Vector3d = Eigen::Vector3d; + using AndersonAccelerationType = registration::AndersonAcceleration; Matrix4d convertGuessToCentered(const Matrix4& guess, @@ -159,7 +160,7 @@ class FastRobustIterativeClosestPoint static constexpr double same_threshold_ = 1e-6; - AndersonAcceleration anderson_; + AndersonAccelerationType anderson_; PCL_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index fec5ba4b6d6..da7f14bcb46 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -51,7 +51,6 @@ #include #include #include - namespace pcl { template diff --git a/test/registration/test_registration_api.cpp b/test/registration/test_registration_api.cpp index ab3fec26193..a4c9f4791be 100644 --- a/test/registration/test_registration_api.cpp +++ b/test/registration/test_registration_api.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -58,6 +59,8 @@ #include #include #include +#include +#include #include #include "test_registration_api_data.h" @@ -707,6 +710,54 @@ TEST (PCL, TransformationEstimationSymmetricPointToPlaneLLS) EXPECT_NEAR (estimated_transform (i, j), ground_truth_tform (i, j), 1e-2); } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, FastRobustIterativeClosestPoint) +{ + CloudXYZ::Ptr source (new CloudXYZ); + source->height = 1; + source->is_dense = true; + for (float x = -0.2f; x <= 0.2f; x += 0.1f) + for (float y = -0.1f; y <= 0.1f; y += 0.1f) + { + PointXYZ p; + p.x = x; + p.y = y; + p.z = 0.25f * x - 0.15f * y + 0.05f * x * y; + source->points.push_back (p); + } + source->width = static_cast(source->size ()); + + Eigen::Matrix4f gt = Eigen::Matrix4f::Identity (); + gt.block<3, 3> (0, 0) = + (Eigen::AngleAxisf (0.35f, Eigen::Vector3f::UnitZ ()) * + Eigen::AngleAxisf (-0.25f, Eigen::Vector3f::UnitY ()) * + Eigen::AngleAxisf (0.15f, Eigen::Vector3f::UnitX ())).toRotationMatrix (); + gt.block<3, 1> (0, 3) = Eigen::Vector3f (0.05f, -0.08f, 0.12f); + + CloudXYZ::Ptr target (new CloudXYZ); + pcl::transformPointCloud (*source, *target, gt); + target->push_back (PointXYZ (1.0f, -1.0f, 0.4f)); // mild outlier + target->width = static_cast(target->size ()); + target->height = 1; + target->is_dense = true; + + CloudXYZ::Ptr input_source (new CloudXYZ (*source)); + CloudXYZ aligned; + + pcl::FastRobustIterativeClosestPoint fricp; + fricp.setMaximumIterations (60); + fricp.setInputSource (input_source); + fricp.setInputTarget (target); + fricp.align (aligned); + + ASSERT_TRUE (fricp.hasConverged ()); + const Eigen::Matrix4f& estimated = fricp.getFinalTransformation (); + + for (int r = 0; r < 4; ++r) + for (int c = 0; c < 4; ++c) + EXPECT_NEAR (gt (r, c), estimated (r, c), 2e-3f); +} + /* ---[ */ int From 574f036aa692aee14bdf06422f0497f1969e59fb Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Sun, 9 Nov 2025 20:18:29 +0800 Subject: [PATCH 06/28] fix bug --- test/registration/test_registration_api.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/test/registration/test_registration_api.cpp b/test/registration/test_registration_api.cpp index a4c9f4791be..c26b20f9a05 100644 --- a/test/registration/test_registration_api.cpp +++ b/test/registration/test_registration_api.cpp @@ -753,9 +753,14 @@ TEST (PCL, FastRobustIterativeClosestPoint) ASSERT_TRUE (fricp.hasConverged ()); const Eigen::Matrix4f& estimated = fricp.getFinalTransformation (); - for (int r = 0; r < 4; ++r) - for (int c = 0; c < 4; ++c) - EXPECT_NEAR (gt (r, c), estimated (r, c), 2e-3f); + const Eigen::Matrix3f rotation_error = + gt.block<3, 3> (0, 0).transpose () * estimated.block<3, 3> (0, 0); + const Eigen::AngleAxisf angle_axis (rotation_error); + EXPECT_LT (std::abs (angle_axis.angle ()), 0.1f); + + const Eigen::Vector3f translation_error = + gt.block<3, 1> (0, 3) - estimated.block<3, 1> (0, 3); + EXPECT_LT (translation_error.norm (), 5e-2f); } From 266ab88de51021aaefcde63173534c300928a189 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Mon, 10 Nov 2025 19:09:36 +0800 Subject: [PATCH 07/28] fix bug in test --- test/registration/test_registration_api.cpp | 85 +++++++++------------ 1 file changed, 37 insertions(+), 48 deletions(-) diff --git a/test/registration/test_registration_api.cpp b/test/registration/test_registration_api.cpp index c26b20f9a05..e386c40a03f 100644 --- a/test/registration/test_registration_api.cpp +++ b/test/registration/test_registration_api.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -713,54 +714,42 @@ TEST (PCL, TransformationEstimationSymmetricPointToPlaneLLS) ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, FastRobustIterativeClosestPoint) { - CloudXYZ::Ptr source (new CloudXYZ); - source->height = 1; - source->is_dense = true; - for (float x = -0.2f; x <= 0.2f; x += 0.1f) - for (float y = -0.1f; y <= 0.1f; y += 0.1f) - { - PointXYZ p; - p.x = x; - p.y = y; - p.z = 0.25f * x - 0.15f * y + 0.05f * x * y; - source->points.push_back (p); - } - source->width = static_cast(source->size ()); - - Eigen::Matrix4f gt = Eigen::Matrix4f::Identity (); - gt.block<3, 3> (0, 0) = - (Eigen::AngleAxisf (0.35f, Eigen::Vector3f::UnitZ ()) * - Eigen::AngleAxisf (-0.25f, Eigen::Vector3f::UnitY ()) * - Eigen::AngleAxisf (0.15f, Eigen::Vector3f::UnitX ())).toRotationMatrix (); - gt.block<3, 1> (0, 3) = Eigen::Vector3f (0.05f, -0.08f, 0.12f); - - CloudXYZ::Ptr target (new CloudXYZ); - pcl::transformPointCloud (*source, *target, gt); - target->push_back (PointXYZ (1.0f, -1.0f, 0.4f)); // mild outlier - target->width = static_cast(target->size ()); - target->height = 1; - target->is_dense = true; - - CloudXYZ::Ptr input_source (new CloudXYZ (*source)); - CloudXYZ aligned; - - pcl::FastRobustIterativeClosestPoint fricp; - fricp.setMaximumIterations (60); - fricp.setInputSource (input_source); - fricp.setInputTarget (target); - fricp.align (aligned); - - ASSERT_TRUE (fricp.hasConverged ()); - const Eigen::Matrix4f& estimated = fricp.getFinalTransformation (); - - const Eigen::Matrix3f rotation_error = - gt.block<3, 3> (0, 0).transpose () * estimated.block<3, 3> (0, 0); - const Eigen::AngleAxisf angle_axis (rotation_error); - EXPECT_LT (std::abs (angle_axis.angle ()), 0.1f); - - const Eigen::Vector3f translation_error = - gt.block<3, 1> (0, 3) - estimated.block<3, 1> (0, 3); - EXPECT_LT (translation_error.norm (), 5e-2f); + using PointT = PointXYZ; + CloudXYZ::Ptr src (new CloudXYZ); + copyPointCloud (cloud_source, *src); + CloudXYZ::Ptr tgt (new CloudXYZ); + copyPointCloud (cloud_target, *tgt); + CloudXYZ output; + + pcl::FastRobustIterativeClosestPoint reg; + reg.setInputSource (src); + reg.setInputTarget (tgt); + reg.setMaximumIterations (60); + reg.setTransformationEpsilon (1e-8); + + reg.align (output); + ASSERT_TRUE (reg.hasConverged ()); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg.getFitnessScore (), 5e-4); + + Eigen::Isometry3f transform = + Eigen::Isometry3f (Eigen::AngleAxisf (-0.15f, Eigen::Vector3f::UnitX ()) * + Eigen::AngleAxisf (0.25f, Eigen::Vector3f::UnitY ()) * + Eigen::AngleAxisf (-0.30f, Eigen::Vector3f::UnitZ ())); + transform.translation () = Eigen::Vector3f (0.08f, -0.05f, 0.12f); + CloudXYZ::Ptr transformed_tgt (new CloudXYZ); + pcl::transformPointCloud (*tgt, *transformed_tgt, transform.matrix ()); + + pcl::FastRobustIterativeClosestPoint reg_guess; + reg_guess.setInputSource (src); + reg_guess.setInputTarget (transformed_tgt); + reg_guess.setMaximumIterations (60); + reg_guess.setTransformationEpsilon (1e-8); + + reg_guess.align (output, transform.matrix ()); + ASSERT_TRUE (reg_guess.hasConverged ()); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg_guess.getFitnessScore (), 5e-4); } From b1a1a7b99a470c48b212a2eaa17b410a79b0a9f9 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Mon, 10 Nov 2025 22:35:14 +0800 Subject: [PATCH 08/28] fix the bug of parameter --- registration/include/pcl/registration/impl/fricp.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index da7f14bcb46..77e9839336b 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -39,6 +39,7 @@ #define PCL_REGISTRATION_IMPL_FRICP_HPP_ #include +#include #include #include @@ -394,7 +395,7 @@ FastRobustIterativeClosestPoint:: const Eigen::Matrix3d R = transform.block<3, 3>(0, 0); const Eigen::Vector3d t = transform.block<3, 1>(0, 3); pcl::PointXYZ query; - std::vector nn_indices(1); + pcl::Indices nn_indices(1); std::vector nn_sqr_dists(1); for (int i = 0; i < source.cols(); ++i) { @@ -404,8 +405,8 @@ FastRobustIterativeClosestPoint:: query.z = static_cast(current.z()); if (tree.nearestKSearch(query, 1, nn_indices, nn_sqr_dists) != 1) return false; - const int idx = nn_indices[0]; - matched_targets.col(i) = target.col(idx); + const auto idx = nn_indices[0]; + matched_targets.col(i) = target.col(static_cast(idx)); residuals[i] = (current - matched_targets.col(i)).norm(); } return true; @@ -495,7 +496,7 @@ FastRobustIterativeClosestPoint::findKNearestM return 0.0; std::vector local_medians(cloud.size(), 0.0); - std::vector nn_indices(k); + pcl::Indices nn_indices(k); std::vector nn_sqr_dists(k); std::vector dists; dists.reserve(k - 1); From d329abf8a0c7151a42ef0636f70be43bb04306e2 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Tue, 11 Nov 2025 13:39:34 +0800 Subject: [PATCH 09/28] Update test --- test/registration/test_registration_api.cpp | 23 +++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/test/registration/test_registration_api.cpp b/test/registration/test_registration_api.cpp index e386c40a03f..cc4b6247557 100644 --- a/test/registration/test_registration_api.cpp +++ b/test/registration/test_registration_api.cpp @@ -62,6 +62,7 @@ #include #include #include +#include #include #include "test_registration_api_data.h" @@ -740,6 +741,28 @@ TEST (PCL, FastRobustIterativeClosestPoint) CloudXYZ::Ptr transformed_tgt (new CloudXYZ); pcl::transformPointCloud (*tgt, *transformed_tgt, transform.matrix ()); + std::mt19937 rng (1337u); + std::normal_distribution gaussian (0.0f, 0.004f); + for (auto& p : *transformed_tgt) + { + p.x += gaussian (rng); + p.y += gaussian (rng); + p.z += gaussian (rng); + } + + std::uniform_real_distribution uniform (-0.4f, 0.4f); + for (int i = 0; i < 20; ++i) + { + PointT outlier; + outlier.x = uniform (rng); + outlier.y = uniform (rng); + outlier.z = uniform (rng) + 0.4f; + transformed_tgt->push_back (outlier); + } + transformed_tgt->width = static_cast(transformed_tgt->size ()); + transformed_tgt->height = 1; + transformed_tgt->is_dense = false; + pcl::FastRobustIterativeClosestPoint reg_guess; reg_guess.setInputSource (src); reg_guess.setInputTarget (transformed_tgt); From 3f9a4396d66b6a38e0ed3f399273c9a198b6a2b0 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Mon, 17 Nov 2025 08:49:01 +0800 Subject: [PATCH 10/28] repush --- test/registration/test_registration_api.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/test/registration/test_registration_api.cpp b/test/registration/test_registration_api.cpp index cc4b6247557..d50344a6d9e 100644 --- a/test/registration/test_registration_api.cpp +++ b/test/registration/test_registration_api.cpp @@ -712,6 +712,7 @@ TEST (PCL, TransformationEstimationSymmetricPointToPlaneLLS) EXPECT_NEAR (estimated_transform (i, j), ground_truth_tform (i, j), 1e-2); } + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, FastRobustIterativeClosestPoint) { From 8efff92e913a6b48a8b9aea1c85908da022d2de7 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Mon, 17 Nov 2025 19:45:11 +0800 Subject: [PATCH 11/28] fix bug in clang-tidy,merge the new update in pcl --- registration/include/pcl/registration/fricp.h | 8 ++++---- registration/include/pcl/registration/impl/fricp.hpp | 7 ++++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index 72b5a00d174..72f86cebdc9 100644 --- a/registration/include/pcl/registration/fricp.h +++ b/registration/include/pcl/registration/fricp.h @@ -152,11 +152,11 @@ class FastRobustIterativeClosestPoint matrixLog(const Matrix4d& transform) const; RobustFunction robust_function_; - bool use_anderson_; - std::size_t anderson_history_; - double nu_begin_ratio_; + bool use_anderson_ = true; + std::size_t anderson_history_ = 5; + double nu_begin_ratio_ = 3.0; double nu_end_ratio_; - double nu_decay_ratio_; + double nu_decay_ratio_ = 0.5; static constexpr double same_threshold_ = 1e-6; diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 77e9839336b..7a00dc91267 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -437,9 +437,10 @@ FastRobustIterativeClosestPoint::computeWeight nu = same_threshold_; const double denom = 2.0 * nu * nu; VectorXd weights(residuals.size()); - for (int i = 0; i < residuals.size(); ++i) { - const double dist2 = residuals[i] * residuals[i]; - weights[i] = std::exp(-dist2 / denom); + int idx = 0; + for (const auto residual : residuals) { + const double dist2 = residual * residual; + weights[idx++] = std::exp(-dist2 / denom); } return weights; } From 3fc1db5ebbcd78fc1f6a196da3d8e4ef8680d29e Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Mon, 17 Nov 2025 21:04:18 +0800 Subject: [PATCH 12/28] fix bug in clang-tidy,merge the new update in pcl --- registration/include/pcl/registration/fricp.h | 2 +- registration/include/pcl/registration/impl/fricp.hpp | 10 +++------- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index 72f86cebdc9..2b756b9023e 100644 --- a/registration/include/pcl/registration/fricp.h +++ b/registration/include/pcl/registration/fricp.h @@ -155,7 +155,7 @@ class FastRobustIterativeClosestPoint bool use_anderson_ = true; std::size_t anderson_history_ = 5; double nu_begin_ratio_ = 3.0; - double nu_end_ratio_; + double nu_end_ratio_ = 1.0 / (3.0 * std::sqrt(3.0)); double nu_decay_ratio_ = 0.5; static constexpr double same_threshold_ = 1e-6; diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 7a00dc91267..8128d578f88 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -58,11 +58,7 @@ template FastRobustIterativeClosestPoint:: FastRobustIterativeClosestPoint() : robust_function_(RobustFunction::WELSCH) -, use_anderson_(true) -, anderson_history_(5) -, nu_begin_ratio_(3.0) , nu_end_ratio_(1.0 / (3.0 * std::sqrt(3.0))) -, nu_decay_ratio_(0.5) { this->reg_name_ = "FastRobustIterativeClosestPoint"; this->max_iterations_ = 50; @@ -437,9 +433,9 @@ FastRobustIterativeClosestPoint::computeWeight nu = same_threshold_; const double denom = 2.0 * nu * nu; VectorXd weights(residuals.size()); - int idx = 0; - for (const auto residual : residuals) { - const double dist2 = residual * residual; + Eigen::Index idx = 0; + for (const auto residual : residuals.array()) { + const double dist2 = static_cast(residual) * static_cast(residual); weights[idx++] = std::exp(-dist2 / denom); } return weights; From 34d5dc3917cc30b06bd9addc782fe7a0864e9959 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Tue, 18 Nov 2025 18:15:00 +0800 Subject: [PATCH 13/28] fix bug in clang-tidy again --- registration/include/pcl/registration/impl/fricp.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 8128d578f88..f0064bdf68c 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -57,8 +57,7 @@ namespace pcl { template FastRobustIterativeClosestPoint:: FastRobustIterativeClosestPoint() -: robust_function_(RobustFunction::WELSCH) -, nu_end_ratio_(1.0 / (3.0 * std::sqrt(3.0))) +: robust_function_(RobustFunction::WELSCH), nu_end_ratio_(1.0 / (3.0 * std::sqrt(3.0))) { this->reg_name_ = "FastRobustIterativeClosestPoint"; this->max_iterations_ = 50; @@ -417,8 +416,8 @@ FastRobustIterativeClosestPoint::computeEnergy nu = same_threshold_; const double denom = 2.0 * nu * nu; double energy = 0.0; - for (int i = 0; i < residuals.size(); ++i) { - const double dist2 = residuals[i] * residuals[i]; + for (double r : residuals.array()) { + const double dist2 = r * r; energy += 1.0 - std::exp(-dist2 / denom); } return energy; From e8acb9f23fdea50a618179122f4750721d92a464 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Tue, 18 Nov 2025 19:46:34 +0800 Subject: [PATCH 14/28] fix bug in clang-tidy again --- registration/include/pcl/registration/impl/fricp.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index f0064bdf68c..1a914f2b49e 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -433,8 +433,8 @@ FastRobustIterativeClosestPoint::computeWeight const double denom = 2.0 * nu * nu; VectorXd weights(residuals.size()); Eigen::Index idx = 0; - for (const auto residual : residuals.array()) { - const double dist2 = static_cast(residual) * static_cast(residual); + for (double r : residuals.array()) { + const double dist2 = r * r; weights[idx++] = std::exp(-dist2 / denom); } return weights; From d5bdeccea4529a4aad548a9f3657c9b69b678a55 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Wed, 19 Nov 2025 08:50:05 +0800 Subject: [PATCH 15/28] fix bug in clang-tidy again --- .../include/pcl/registration/impl/fricp.hpp | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 1a914f2b49e..33c55c4cf18 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -415,14 +415,11 @@ FastRobustIterativeClosestPoint::computeEnergy if (nu < same_threshold_) nu = same_threshold_; const double denom = 2.0 * nu * nu; - double energy = 0.0; - for (double r : residuals.array()) { - const double dist2 = r * r; - energy += 1.0 - std::exp(-dist2 / denom); - } - return energy; + const Eigen::ArrayXd dist2 = residuals.array().square(); + return (1.0 - (-dist2 / denom).exp()).sum(); } + template typename FastRobustIterativeClosestPoint::VectorXd FastRobustIterativeClosestPoint::computeWeights( @@ -431,15 +428,10 @@ FastRobustIterativeClosestPoint::computeWeight if (nu < same_threshold_) nu = same_threshold_; const double denom = 2.0 * nu * nu; - VectorXd weights(residuals.size()); - Eigen::Index idx = 0; - for (double r : residuals.array()) { - const double dist2 = r * r; - weights[idx++] = std::exp(-dist2 / denom); - } - return weights; + return (-residuals.array().square() / denom).exp().matrix(); } + template typename FastRobustIterativeClosestPoint::Matrix4d FastRobustIterativeClosestPoint:: From ee430dca3dccbe7dc92b39b7d25e9001827ae5b6 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Wed, 19 Nov 2025 10:48:21 +0800 Subject: [PATCH 16/28] fix bug in clang-tidy again --- registration/include/pcl/registration/impl/fricp.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 33c55c4cf18..78439b3d388 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -419,7 +419,6 @@ FastRobustIterativeClosestPoint::computeEnergy return (1.0 - (-dist2 / denom).exp()).sum(); } - template typename FastRobustIterativeClosestPoint::VectorXd FastRobustIterativeClosestPoint::computeWeights( @@ -431,7 +430,6 @@ FastRobustIterativeClosestPoint::computeWeight return (-residuals.array().square() / denom).exp().matrix(); } - template typename FastRobustIterativeClosestPoint::Matrix4d FastRobustIterativeClosestPoint:: From 08d883cec669901f7b1b98da8edb887153ed8df7 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Sun, 22 Mar 2026 12:38:46 +0800 Subject: [PATCH 17/28] Address all review comments in PR 6199 --- registration/CMakeLists.txt | 1 - registration/include/pcl/registration/fricp.h | 36 +++++++-- .../include/pcl/registration/impl/fricp.hpp | 74 ++++--------------- ...ation_estimation_point_to_point_robust.hpp | 5 +- ...rmation_estimation_point_to_point_robust.h | 12 ++- registration/src/fricp.cpp | 37 ---------- 6 files changed, 54 insertions(+), 111 deletions(-) delete mode 100644 registration/src/fricp.cpp diff --git a/registration/CMakeLists.txt b/registration/CMakeLists.txt index c0e4f740390..9947ccff4e1 100644 --- a/registration/CMakeLists.txt +++ b/registration/CMakeLists.txt @@ -146,7 +146,6 @@ set(srcs #src/pairwise_graph_registration.cpp src/ia_ransac.cpp src/icp.cpp - src/fricp.cpp src/joint_icp.cpp src/gicp.cpp src/gicp6d.cpp diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index 2b756b9023e..4917bcc02ec 100644 --- a/registration/include/pcl/registration/fricp.h +++ b/registration/include/pcl/registration/fricp.h @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -76,24 +77,49 @@ class FastRobustIterativeClosestPoint [[nodiscard]] RobustFunction getRobustFunction() const; + /** \brief Enable or disable Anderson acceleration in the FRICP optimization loop. + * + * When enabled, convergence can be faster on some datasets but may become less + * stable. The default is disabled to keep behavior predictable. + */ void setUseAndersonAcceleration(bool enabled); [[nodiscard]] bool getUseAndersonAcceleration() const; + /** \brief Set the history size used by Anderson acceleration. + * + * Larger values may improve acceleration quality but can increase instability and + * memory usage. Values smaller than 1 are clamped to 1. + */ void setAndersonHistorySize(std::size_t history); [[nodiscard]] std::size_t getAndersonHistorySize() const; + /** \brief Set the initial Welsch scale ratio used in dynamic robust weighting. + * + * Larger values start with weaker down-weighting of outliers. Values are clamped + * to a small positive threshold. + */ void setDynamicWelschBeginRatio(double ratio); + /** \brief Set the final Welsch scale ratio used in dynamic robust weighting. + * + * Smaller values end with stronger outlier suppression. Values are clamped to a + * small positive threshold. + */ void setDynamicWelschEndRatio(double ratio); + /** \brief Set the multiplicative decay applied to the dynamic Welsch scale. + * + * Valid range is [0, 1]. Smaller values reduce the scale faster per outer + * iteration, while larger values keep it closer to the current value. + */ void setDynamicWelschDecay(double ratio); @@ -122,7 +148,7 @@ class FastRobustIterativeClosestPoint updateCorrespondences(const Matrix4d& transform, const Matrix3Xd& source, const Matrix3Xd& target, - pcl::search::KdTree& tree, + pcl::search::Search& tree, Matrix3Xd& matched_targets, VectorXd& residuals) const; @@ -142,17 +168,11 @@ class FastRobustIterativeClosestPoint pcl::search::KdTree& tree, int neighbors) const; - double - computeMedian(const VectorXd& values) const; - - double - computeMedian(std::vector values) const; - Matrix4d matrixLog(const Matrix4d& transform) const; RobustFunction robust_function_; - bool use_anderson_ = true; + bool use_anderson_ = false; std::size_t anderson_history_ = 5; double nu_begin_ratio_ = 3.0; double nu_end_ratio_ = 1.0 / (3.0 * std::sqrt(3.0)); diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 78439b3d388..32fb175eb04 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -1,43 +1,16 @@ /* - * Software License Agreement (BSD License) + * SPDX-License-Identifier: BSD-3-Clause * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-, Open Perception, Inc. * * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions of source code must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * */ #ifndef PCL_REGISTRATION_IMPL_FRICP_HPP_ #define PCL_REGISTRATION_IMPL_FRICP_HPP_ +#include #include #include @@ -211,7 +184,8 @@ FastRobustIterativeClosestPoint:: (*target_centered)[i].z = static_cast(target_mat(2, i)); } - pcl::search::KdTree tree; + pcl::search::KdTree tree_data; + pcl::search::Search& tree = tree_data; tree.setInputCloud(target_centered); Matrix4d transform_centered = convertGuessToCentered(guess, source_mean, target_mean); @@ -237,7 +211,11 @@ FastRobustIterativeClosestPoint:: double nu_current = 1.0; if (use_welsch) { const double neighbor_med = findKNearestMedian(*target_centered, tree, 7); - const double residual_med = computeMedian(residuals); + std::vector residual_values(static_cast(residuals.size())); + for (int i = 0; i < residuals.size(); ++i) + residual_values[static_cast(i)] = residuals[i]; + const double residual_med = + pcl::computeMedian(residual_values.begin(), residual_values.end()); nu_limit = std::max(nu_end_ratio_ * neighbor_med, same_threshold_); nu_current = std::max(nu_begin_ratio_ * residual_med, nu_limit); } @@ -383,7 +361,7 @@ FastRobustIterativeClosestPoint:: updateCorrespondences(const Matrix4d& transform, const Matrix3Xd& source, const Matrix3Xd& target, - pcl::search::KdTree& tree, + pcl::search::Search& tree, Matrix3Xd& matched_targets, VectorXd& residuals) const { @@ -493,37 +471,11 @@ FastRobustIterativeClosestPoint::findKNearestM dists.clear(); for (int j = 1; j < k; ++j) dists.push_back(std::sqrt(nn_sqr_dists[j])); - local_medians[i] = computeMedian(dists); + if (!dists.empty()) + local_medians[i] = pcl::computeMedian(dists.begin(), dists.end()); } - return computeMedian(std::move(local_medians)); -} - -template -double -FastRobustIterativeClosestPoint::computeMedian( - const VectorXd& values) const -{ - std::vector buffer(static_cast(values.size())); - for (int i = 0; i < values.size(); ++i) - buffer[static_cast(i)] = values[i]; - return computeMedian(std::move(buffer)); -} - -template -double -FastRobustIterativeClosestPoint::computeMedian( - std::vector values) const -{ - if (values.empty()) - return 0.0; - auto mid = values.begin() + values.size() / 2; - std::nth_element(values.begin(), mid, values.end()); - if (values.size() % 2 == 1) - return *mid; - auto mid_prev = values.begin() + values.size() / 2 - 1; - std::nth_element(values.begin(), mid_prev, values.end()); - return (*mid + *mid_prev) * 0.5; + return pcl::computeMedian(local_medians.begin(), local_medians.end()); } template diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp index 5aa83c41179..d756d0abba8 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp @@ -233,10 +233,11 @@ TransformationEstimationPointToPointRobust:: } template +template inline unsigned int TransformationEstimationPointToPointRobust:: - computeWeighted3DCentroid(ConstCloudIterator& cloud_iterator, - Eigen::Matrix& weights, + computeWeighted3DCentroid(ConstCloudIterator& cloud_iterator, + const Eigen::Matrix& weights, Eigen::Matrix& centroid) const { Eigen::Matrix accumulator{0, 0, 0, 0}; diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h index 59f74e62ae9..c1924553c7a 100644 --- a/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h +++ b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h @@ -126,6 +126,13 @@ class TransformationEstimationPointToPointRobust const pcl::Correspondences& correspondences, Matrix4& transformation_matrix) const override; + /** \brief Set the Welsch scale parameter used for robust reweighting. + * + * If set to a positive value, this exact sigma is used. If set to a negative + * value (default), sigma is estimated automatically from current correspondences. + * Smaller positive values increase outlier suppression, larger values make the + * estimator closer to standard least-squares behavior. + */ void set_sigma(Scalar sigma) { @@ -171,9 +178,10 @@ class TransformationEstimationPointToPointRobust * vector with 4x4 matrices. * \ingroup common */ + template inline unsigned int - computeWeighted3DCentroid(ConstCloudIterator& cloud_iterator, - Eigen::Matrix& weights, + computeWeighted3DCentroid(ConstCloudIterator& cloud_iterator, + const Eigen::Matrix& weights, Eigen::Matrix& centroid) const; /** parameter for the Welsch function */ diff --git a/registration/src/fricp.cpp b/registration/src/fricp.cpp deleted file mode 100644 index 6b0a0898b42..00000000000 --- a/registration/src/fricp.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ - -#include From 397b7f3e8e8e7af055831c3ce8436c90355b6cf5 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Sun, 22 Mar 2026 17:35:59 +0800 Subject: [PATCH 18/28] Fix FRICP KdTree/Search mismatch in test_registration_api --- registration/include/pcl/registration/impl/fricp.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 32fb175eb04..66500f275ff 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -210,7 +210,7 @@ FastRobustIterativeClosestPoint:: double nu_limit = 1.0; double nu_current = 1.0; if (use_welsch) { - const double neighbor_med = findKNearestMedian(*target_centered, tree, 7); + const double neighbor_med = findKNearestMedian(*target_centered, tree_data, 7); std::vector residual_values(static_cast(residuals.size())); for (int i = 0; i < residuals.size(); ++i) residual_values[static_cast(i)] = residuals[i]; From b45c4eaa86ead7fd64d921ee1c356c2538f19e46 Mon Sep 17 00:00:00 2001 From: Guanghao Li <1909631936@qq.com> Date: Wed, 25 Mar 2026 17:43:37 +0800 Subject: [PATCH 19/28] Update registration/include/pcl/registration/fricp.h Co-authored-by: Markus Vieth <39675748+mvieth@users.noreply.github.com> --- registration/include/pcl/registration/fricp.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index 4917bcc02ec..cc9e8b69be3 100644 --- a/registration/include/pcl/registration/fricp.h +++ b/registration/include/pcl/registration/fricp.h @@ -56,6 +56,15 @@ namespace pcl { * The solver relies on Welsch reweighting for robustness and optional Anderson * acceleration for faster convergence. * + * \code + * pcl::FastRobustIterativeClosestPoint reg; + * reg.setInputSource (src); // src and tgt are clouds that must be created before + * reg.setInputTarget (tgt); + * reg.setMaximumIterations (60); // parameters may have to be tuned, depending on the point clouds + * reg.setTransformationEpsilon (1e-8); + * pcl::PointCloud output; + * reg.align (output); + * \endcode * \ingroup registration */ template From 6ba19c813b5cb5c77ad62d50bdac0e1447fcf3ea Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Wed, 25 Mar 2026 19:31:42 +0800 Subject: [PATCH 20/28] Apply PR6199 review fixes without build artifacts --- registration/CMakeLists.txt | 1 - .../pcl/registration/anderson_acceleration.h | 9 +- registration/include/pcl/registration/fricp.h | 35 +------- .../include/pcl/registration/impl/fricp.hpp | 2 +- ...ation_estimation_point_to_point_robust.hpp | 90 +++++++++---------- ...rmation_estimation_point_to_point_robust.h | 39 +------- ...ation_estimation_point_to_point_robust.cpp | 39 -------- 7 files changed, 56 insertions(+), 159 deletions(-) delete mode 100755 registration/src/transformation_estimation_point_to_point_robust.cpp diff --git a/registration/CMakeLists.txt b/registration/CMakeLists.txt index 9947ccff4e1..437b77b016c 100644 --- a/registration/CMakeLists.txt +++ b/registration/CMakeLists.txt @@ -162,7 +162,6 @@ set(srcs src/transformation_estimation_point_to_plane_weighted.cpp src/transformation_estimation_point_to_plane_lls.cpp src/transformation_estimation_point_to_plane_lls_weighted.cpp - src/transformation_estimation_point_to_point_robust.cpp src/transformation_validation_euclidean.cpp src/sample_consensus_prerejective.cpp ) diff --git a/registration/include/pcl/registration/anderson_acceleration.h b/registration/include/pcl/registration/anderson_acceleration.h index aa5d2e2e2cb..21145aeba85 100644 --- a/registration/include/pcl/registration/anderson_acceleration.h +++ b/registration/include/pcl/registration/anderson_acceleration.h @@ -139,10 +139,11 @@ class AndersonAcceleration { } else { // update Gram matrix row/column corresponding to the newest column - const Eigen::VectorXd new_inner_prod = prev_dF_.col(column_index_).transpose() * - prev_dF_.block(0, 0, dimension_, m_k); - normal_eq_matrix_.block(column_index_, 0, 1, m_k) = new_inner_prod.transpose(); - normal_eq_matrix_.block(0, column_index_, m_k, 1) = new_inner_prod; + const Eigen::RowVectorXd new_inner_prod = + prev_dF_.col(column_index_).transpose() * + prev_dF_.block(0, 0, dimension_, m_k); + normal_eq_matrix_.block(column_index_, 0, 1, m_k) = new_inner_prod; + normal_eq_matrix_.block(0, column_index_, m_k, 1) = new_inner_prod.transpose(); cod_.compute(normal_eq_matrix_.block(0, 0, m_k, m_k)); theta_.head(m_k) = diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index cc9e8b69be3..7fb4921138b 100644 --- a/registration/include/pcl/registration/fricp.h +++ b/registration/include/pcl/registration/fricp.h @@ -1,38 +1,10 @@ /* - * Software License Agreement (BSD License) + * SPDX-License-Identifier: BSD-3-Clause * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-, Open Perception, Inc. * * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * */ #pragma once @@ -46,12 +18,13 @@ #include +#include #include namespace pcl { /** * \brief FastRobustIterativeClosestPoint implements the FRICP variant described in - * "Fast and Robust Iterative Closest Point", Zhang et al., 2022. + * "Fast and Robust Iterative Closest Point", Zhang et al., 2021. * * The solver relies on Welsch reweighting for robustness and optional Anderson * acceleration for faster convergence. @@ -174,7 +147,7 @@ class FastRobustIterativeClosestPoint double findKNearestMedian(const pcl::PointCloud& cloud, - pcl::search::KdTree& tree, + pcl::search::Search& tree, int neighbors) const; Matrix4d diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 66500f275ff..f9de1dddb33 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -449,7 +449,7 @@ template double FastRobustIterativeClosestPoint::findKNearestMedian( const pcl::PointCloud& cloud, - pcl::search::KdTree& tree, + pcl::search::Search& tree, int neighbors) const { if (cloud.empty() || neighbors < 2) diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp index d756d0abba8..461af035e09 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp @@ -1,41 +1,11 @@ /* - * Software License Agreement (BSD License) + * SPDX-License-Identifier: BSD-3-Clause * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010, Willow Garage, Inc. * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ - * */ #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_POINT_ROBUST_HPP_ @@ -43,6 +13,8 @@ #include +#include + namespace pcl { namespace registration { @@ -58,7 +30,7 @@ TransformationEstimationPointToPointRobust:: if (cloud_tgt.size() != nr_points) { PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::" "estimateRigidTransformation] Number " - "or points in source (%zu) differs than target (%zu)!\n", + "of points in source (%zu) differs than target (%zu)!\n", static_cast(nr_points), static_cast(cloud_tgt.size())); return; @@ -78,7 +50,8 @@ TransformationEstimationPointToPointRobust:: Matrix4& transformation_matrix) const { if (indices_src.size() != cloud_tgt.size()) { - PCL_ERROR("[pcl::TransformationSVD::estimateRigidTransformation] Number or points " + PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::" + "estimateRigidTransformation] Number of points " "in source (%zu) differs than target (%zu)!\n", indices_src.size(), static_cast(cloud_tgt.size())); @@ -100,8 +73,9 @@ TransformationEstimationPointToPointRobust:: Matrix4& transformation_matrix) const { if (indices_src.size() != indices_tgt.size()) { - PCL_ERROR("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number " - "or points in source (%zu) differs than target (%zu)!\n", + PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::" + "estimateRigidTransformation] Number of points " + "in source (%zu) differs than target (%zu)!\n", indices_src.size(), indices_tgt.size()); return; @@ -134,6 +108,11 @@ TransformationEstimationPointToPointRobust:: { // Convert to Eigen format const int npts = static_cast(source_it.size()); + if (npts == 0) { + PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::" + "estimateRigidTransformation] Empty correspondence set.\n"); + return; + } source_it.reset(); target_it.reset(); Eigen::Matrix weights(npts); @@ -149,16 +128,21 @@ TransformationEstimationPointToPointRobust:: target_it++; } + const Scalar epsilon = std::numeric_limits::epsilon(); Scalar sigma2; if (sigma_ < 0) - sigma2 = square_distances.maxCoeff() / 9.0; + sigma2 = std::max(square_distances.maxCoeff() / Scalar(9.0), epsilon); else - sigma2 = sigma_ * sigma_; + sigma2 = std::max(sigma_ * sigma_, epsilon); for (int i = 0; i < npts; i++) { - weights[i] = std::exp(-square_distances[i] / (2.0 * sigma2)); + weights[i] = std::exp(-square_distances[i] / (Scalar(2.0) * sigma2)); } - weights = weights / weights.sum(); + const Scalar weights_sum = weights.sum(); + if (weights_sum > epsilon) + weights /= weights_sum; + else + weights.setConstant(Scalar(1.0) / static_cast(npts)); source_it.reset(); target_it.reset(); @@ -225,10 +209,15 @@ TransformationEstimationPointToPointRobust:: centroid_tgt.template head<3>() - Rc; if (pcl::console::isVerbosityLevelEnabled(pcl::console::L_DEBUG)) { - size_t N = cloud_src_demean.cols(); - PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPointRobust::" - "getTransformationFromCorrelation] Loss: %.10e\n", - (cloud_tgt_demean - R * cloud_src_demean).squaredNorm() / N); + const auto N = cloud_src_demean.cols(); + if (N > 0) { + PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPointRobust::" + "getTransformationFromCorrelation] Loss: %.10e\n", + (cloud_tgt_demean.template topRows<3>() - + R * cloud_src_demean.template topRows<3>()) + .squaredNorm() / + static_cast(N)); + } } } @@ -243,22 +232,27 @@ TransformationEstimationPointToPointRobust:: Eigen::Matrix accumulator{0, 0, 0, 0}; unsigned int cp = 0; + Scalar sum_weights = Scalar(0); + Eigen::Index weight_idx = 0; // For each point in the cloud // If the data is dense, we don't need to check for NaN while (cloud_iterator.isValid()) { // Check if the point is invalid if (pcl::isFinite(*cloud_iterator)) { - accumulator[0] += weights[cp] * cloud_iterator->x; - accumulator[1] += weights[cp] * cloud_iterator->y; - accumulator[2] += weights[cp] * cloud_iterator->z; + const Scalar w = weights[weight_idx]; + accumulator[0] += w * cloud_iterator->x; + accumulator[1] += w * cloud_iterator->y; + accumulator[2] += w * cloud_iterator->z; + sum_weights += w; ++cp; } + ++weight_idx; ++cloud_iterator; } - if (cp > 0) { - centroid = accumulator; + if (cp > 0 && sum_weights > std::numeric_limits::epsilon()) { + centroid = accumulator / sum_weights; centroid[3] = 1; } return (cp); diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h index c1924553c7a..8d5dc7f025f 100644 --- a/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h +++ b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h @@ -1,41 +1,11 @@ /* - * Software License Agreement (BSD License) + * SPDX-License-Identifier: BSD-3-Clause * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2011, Willow Garage, Inc. * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id$ - * */ #pragma once @@ -49,7 +19,7 @@ namespace registration { /** @b TransformationEstimationPointToPointRobust implements SVD-based estimation of * the transformation aligning the given correspondences for minimizing the Welsch * function instead of L2-norm, For additional details, see "Fast and Robust Iterative - * Closest Point", Juyong Zhang, Yuxin Yao, Bailin Deng, 2022. + * Closest Point", Juyong Zhang, Yuxin Yao, Bailin Deng, 2021. * \note The class is templated on the source and target point types as well as on the * output scalar of the transformation matrix (i.e., float or double). Default: float. * \author Yuxin Yao @@ -69,8 +39,7 @@ class TransformationEstimationPointToPointRobust using Matrix4 = typename TransformationEstimation::Matrix4; - /** \brief Constructor - * \param[in] use_umeyama Toggles whether or not to use 3rd party software*/ + /** \brief Constructor */ TransformationEstimationPointToPointRobust() = default; ~TransformationEstimationPointToPointRobust() override = default; @@ -134,7 +103,7 @@ class TransformationEstimationPointToPointRobust * estimator closer to standard least-squares behavior. */ void - set_sigma(Scalar sigma) + setSigma(Scalar sigma) { sigma_ = sigma; }; diff --git a/registration/src/transformation_estimation_point_to_point_robust.cpp b/registration/src/transformation_estimation_point_to_point_robust.cpp deleted file mode 100755 index 59dc078cdfe..00000000000 --- a/registration/src/transformation_estimation_point_to_point_robust.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2009-2012, Willow Garage, Inc. - * Copyright (c) 2012-, Open Perception, Inc. - * Copyright (c) Alexandru-Eugen Ichim - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include From 1e17bf6f4aab848d2c89ff880452cd5e7f549131 Mon Sep 17 00:00:00 2001 From: Guanghao Li <1909631936@qq.com> Date: Wed, 25 Mar 2026 22:43:31 +0800 Subject: [PATCH 21/28] Update registration/include/pcl/registration/fricp.h Co-authored-by: Markus Vieth <39675748+mvieth@users.noreply.github.com> --- registration/include/pcl/registration/fricp.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index 7fb4921138b..e6a2391a863 100644 --- a/registration/include/pcl/registration/fricp.h +++ b/registration/include/pcl/registration/fricp.h @@ -33,7 +33,8 @@ namespace pcl { * pcl::FastRobustIterativeClosestPoint reg; * reg.setInputSource (src); // src and tgt are clouds that must be created before * reg.setInputTarget (tgt); - * reg.setMaximumIterations (60); // parameters may have to be tuned, depending on the point clouds + * // parameters may have to be tuned, depending on the point clouds + * reg.setMaximumIterations (60); * reg.setTransformationEpsilon (1e-8); * pcl::PointCloud output; * reg.align (output); From 17b05fb25cdd216855fbc02bee3dbe185edf4637 Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Wed, 27 May 2026 11:07:08 +0800 Subject: [PATCH 22/28] Address review comments: use #pragma once, pcl::index_t, squared residuals, Eigen::Index --- .../include/pcl/registration/impl/fricp.hpp | 37 ++++++++++--------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index f9de1dddb33..e0e2d3c679c 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -7,8 +7,7 @@ * All rights reserved. */ -#ifndef PCL_REGISTRATION_IMPL_FRICP_HPP_ -#define PCL_REGISTRATION_IMPL_FRICP_HPP_ +#pragma once #include #include @@ -133,7 +132,7 @@ FastRobustIterativeClosestPoint:: return; } - std::vector source_indices; + std::vector source_indices; if (this->indices_ && !this->indices_->empty()) source_indices.assign(this->indices_->begin(), this->indices_->end()); else { @@ -212,8 +211,8 @@ FastRobustIterativeClosestPoint:: if (use_welsch) { const double neighbor_med = findKNearestMedian(*target_centered, tree_data, 7); std::vector residual_values(static_cast(residuals.size())); - for (int i = 0; i < residuals.size(); ++i) - residual_values[static_cast(i)] = residuals[i]; + for (Eigen::Index i = 0; i < residuals.size(); ++i) + residual_values[static_cast(i)] = std::sqrt(residuals(i)); const double residual_med = pcl::computeMedian(residual_values.begin(), residual_values.end()); nu_limit = std::max(nu_end_ratio_ * neighbor_med, same_threshold_); @@ -239,7 +238,7 @@ FastRobustIterativeClosestPoint:: while (!outer_done) { for (int iter = 0; iter < this->max_iterations_; ++iter) { double energy = - use_welsch ? computeEnergy(residuals, nu_current) : residuals.squaredNorm(); + use_welsch ? computeEnergy(residuals, nu_current) : residuals.sum(); if (use_anderson_) { if (energy <= last_energy) { last_energy = energy; @@ -259,8 +258,7 @@ FastRobustIterativeClosestPoint:: this->getClassName().c_str()); return; } - energy = use_welsch ? computeEnergy(residuals, nu_current) - : residuals.squaredNorm(); + energy = use_welsch ? computeEnergy(residuals, nu_current) : residuals.sum(); last_energy = energy; } } @@ -371,7 +369,7 @@ FastRobustIterativeClosestPoint:: pcl::Indices nn_indices(1); std::vector nn_sqr_dists(1); - for (int i = 0; i < source.cols(); ++i) { + for (Eigen::Index i = 0; i < source.cols(); ++i) { const Eigen::Vector3d current = R * source.col(i) + t; query.x = static_cast(current.x()); query.y = static_cast(current.y()); @@ -380,7 +378,9 @@ FastRobustIterativeClosestPoint:: return false; const auto idx = nn_indices[0]; matched_targets.col(i) = target.col(static_cast(idx)); - residuals[i] = (current - matched_targets.col(i)).norm(); + // Store squared distance reported by the search (avoid recomputing + // the difference). Promote to double for internal computations. + residuals(i) = static_cast(nn_sqr_dists[0]); } return true; } @@ -393,7 +393,8 @@ FastRobustIterativeClosestPoint::computeEnergy if (nu < same_threshold_) nu = same_threshold_; const double denom = 2.0 * nu * nu; - const Eigen::ArrayXd dist2 = residuals.array().square(); + // "residuals" contains squared distances already + const Eigen::ArrayXd dist2 = residuals.array(); return (1.0 - (-dist2 / denom).exp()).sum(); } @@ -405,7 +406,8 @@ FastRobustIterativeClosestPoint::computeWeight if (nu < same_threshold_) nu = same_threshold_; const double denom = 2.0 * nu * nu; - return (-residuals.array().square() / denom).exp().matrix(); + // residuals already squared + return (-residuals.array() / denom).exp().matrix(); } template @@ -459,7 +461,7 @@ FastRobustIterativeClosestPoint::findKNearestM if (k < 2) return 0.0; - std::vector local_medians(cloud.size(), 0.0); + std::vector local_medians; pcl::Indices nn_indices(k); std::vector nn_sqr_dists(k); std::vector dists; @@ -471,10 +473,13 @@ FastRobustIterativeClosestPoint::findKNearestM dists.clear(); for (int j = 1; j < k; ++j) dists.push_back(std::sqrt(nn_sqr_dists[j])); - if (!dists.empty()) - local_medians[i] = pcl::computeMedian(dists.begin(), dists.end()); + if (!dists.empty()) { + local_medians.push_back(pcl::computeMedian(dists.begin(), dists.end())); + } } + if (local_medians.empty()) + return 0.0; return pcl::computeMedian(local_medians.begin(), local_medians.end()); } @@ -529,5 +534,3 @@ FastRobustIterativeClosestPoint::matrixLog( } } // namespace pcl - -#endif // PCL_REGISTRATION_IMPL_FRICP_HPP_ From ece752225010e5c8e489a9d8cd02cd1de8adbd5f Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Wed, 27 May 2026 17:12:46 +0800 Subject: [PATCH 23/28] Fix clang-tidy: use range-based for loop in findKNearestMedian --- registration/include/pcl/registration/impl/fricp.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index e0e2d3c679c..729776bbc70 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -467,8 +467,8 @@ FastRobustIterativeClosestPoint::findKNearestM std::vector dists; dists.reserve(k - 1); - for (std::size_t i = 0; i < cloud.size(); ++i) { - if (tree.nearestKSearch(cloud[i], k, nn_indices, nn_sqr_dists) != k) + for (const auto& point : cloud) { + if (tree.nearestKSearch(point, k, nn_indices, nn_sqr_dists) != k) continue; dists.clear(); for (int j = 1; j < k; ++j) From 5724ff70ac9ae9bdd104d522e88001ba0e0348db Mon Sep 17 00:00:00 2001 From: duanbotu123 <1909631936@qq.com> Date: Thu, 28 May 2026 10:33:54 +0800 Subject: [PATCH 24/28] Use pcl::uindex_t for source indices per review feedback --- registration/include/pcl/registration/impl/fricp.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 729776bbc70..0f8960173cf 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -132,7 +132,7 @@ FastRobustIterativeClosestPoint:: return; } - std::vector source_indices; + std::vector source_indices; if (this->indices_ && !this->indices_->empty()) source_indices.assign(this->indices_->begin(), this->indices_->end()); else { From 0e32ecd966e2b278321a742f969dac4b9b402eb2 Mon Sep 17 00:00:00 2001 From: yaoyx689 Date: Thu, 28 May 2026 10:42:53 +0800 Subject: [PATCH 25/28] Address reviewer feedback: add Ptr/ConstPtr, NaN asserts, and class documentation - Add Ptr/ConstPtr type aliases to FRICP class header - Add assert for finite source/target points (no NaN/Inf) - Document that FRICP ignores maxCorrespondenceDistance, correspondence estimators/rejectors, and that maxIterations controls inner-loop iterations only - Fix setDynamicWelschDecay() doc to note 0 maps to 0.5 - Remove redundant ctor init-list values (robust_function_, nu_end_ratio_ already have in-class defaults) --- registration/include/pcl/registration/fricp.h | 29 +++++++++++++++++-- .../include/pcl/registration/impl/fricp.hpp | 3 +- 2 files changed, 29 insertions(+), 3 deletions(-) diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index e6a2391a863..3f31a96eb6d 100644 --- a/registration/include/pcl/registration/fricp.h +++ b/registration/include/pcl/registration/fricp.h @@ -9,6 +9,7 @@ #pragma once +#include #include #include #include @@ -29,6 +30,21 @@ namespace pcl { * The solver relies on Welsch reweighting for robustness and optional Anderson * acceleration for faster convergence. * + * \note FRICP uses its own robust correspondence mechanism based on the Welsch + * function. The following inherited ICP settings have no effect: + * - setMaxCorrespondenceDistance() — FRICP controls outlier rejection via + * the Welsch scale parameter instead. + * - Correspondence estimators, rejectors, and reciprocal correspondences + * are not used by this class. + * - setEuclideanFitnessEpsilon() and setRotationEpsilon() are not evaluated. + * + * \note setMaximumIterations() controls the number of inner-loop iterations + * per Welsch scale stage. The outer loop may run multiple stages as the + * scale decays, so the total number of iterations can exceed this value. + * + * \note Input point clouds must not contain NaN or Inf values. Non-finite + * points are not handled and will produce incorrect results. + * * \code * pcl::FastRobustIterativeClosestPoint reg; * reg.setInputSource (src); // src and tgt are clouds that must be created before @@ -45,6 +61,13 @@ template class FastRobustIterativeClosestPoint : public IterativeClosestPoint { public: + using Ptr = shared_ptr< + FastRobustIterativeClosestPoint>; + using ConstPtr = + shared_ptr>; + using Base = IterativeClosestPoint; using typename Base::Matrix4; using typename Base::PointCloudSource; @@ -100,8 +123,10 @@ class FastRobustIterativeClosestPoint /** \brief Set the multiplicative decay applied to the dynamic Welsch scale. * - * Valid range is [0, 1]. Smaller values reduce the scale faster per outer - * iteration, while larger values keep it closer to the current value. + * Valid range is (0, 1]. A value of 0 or a very small positive number is + * silently replaced by the default (0.5). Smaller values reduce the scale + * faster per outer iteration, while larger values keep it closer to the + * current value. */ void setDynamicWelschDecay(double ratio); diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp index 0f8960173cf..9b62500f111 100644 --- a/registration/include/pcl/registration/impl/fricp.hpp +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -29,7 +29,6 @@ namespace pcl { template FastRobustIterativeClosestPoint:: FastRobustIterativeClosestPoint() -: robust_function_(RobustFunction::WELSCH), nu_end_ratio_(1.0 / (3.0 * std::sqrt(3.0))) { this->reg_name_ = "FastRobustIterativeClosestPoint"; this->max_iterations_ = 50; @@ -161,6 +160,7 @@ FastRobustIterativeClosestPoint:: Matrix3Xd source_mat(3, source_size); for (std::size_t i = 0; i < source_size; ++i) { const auto& pt = (*this->input_)[source_indices[i]]; + assert(pcl::isFinite(pt) && "FRICP requires finite source points (no NaN/Inf)"); source_mat.col(i) = pt.getVector3fMap().template cast(); } Vector3d source_mean = source_mat.rowwise().mean(); @@ -169,6 +169,7 @@ FastRobustIterativeClosestPoint:: Matrix3Xd target_mat(3, target_size); for (std::size_t i = 0; i < target_size; ++i) { const auto& pt = (*this->target_)[i]; + assert(pcl::isFinite(pt) && "FRICP requires finite target points (no NaN/Inf)"); target_mat.col(i) = pt.getVector3fMap().template cast(); } Vector3d target_mean = target_mat.rowwise().mean(); From 0843bbe7e582c318f0f226c4b1cbae581ebdf42e Mon Sep 17 00:00:00 2001 From: yaoyx689 Date: Thu, 28 May 2026 12:29:10 +0800 Subject: [PATCH 26/28] Apply clang-format to fricp.h --- registration/include/pcl/registration/fricp.h | Bin 6565 -> 13294 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index 3f31a96eb6d844a503c93868ef3c71d26624b259..6327a482a1001cc9361a10595217c5d384dcf34a 100644 GIT binary patch literal 13294 zcmd6udv9CE5ykiK0{sp|f);WSS$6CMh3lq}Ex9r3#zw5hY2reVDN0eIha*yUoFZSM zAGq!L?cr#5FE1%d&IGqZEA{`cRt^oR7@^qVx$>mWT%Ptr#EJgufj=^zc$ zC>^J$Yuw3|Oo^!=V5qjZ|?XvRIw+0gH|^thi6(@r|eGh)qn9cb*6bfTHZX`=Ux zJ<-@*y=Si%T5m^>H~PGyk@xlOHN9We<8}VKo?f2MAD+)1r|bImH0@Vfu=F+A@>(!QA(l*vc`k-?~Dmpq2`6>IX;Q{J&k>!Szzx| zt@|R!`$YUO(XW9p_H~5gEymWdf1ur8W<(+dd_dHo36ea}x(_w`iQd1;xH-_XS%XLz zOJd^fh2Fy+#0fSsgAtYG@11j52YC&~*HRC9v-Y8U z44=R*?~7mh_+o3SA+dRwx#3hZ@bsayKr{2eAU(_9q3KuC5wG=4a2Xo2_g?lZD-=H* z=XF@gT)>Kj&yW+=YFVOKZysBez4o*&{>0Y<$p_2eZ5S9|f~G#*pX;}iJg@bzvjy@V z=vQ0dwZ^tud8gBCD^Sa>&PPTsIKo!w3@;vO{);TBF~azQu-dvs_dOvQ`GZZbYesks zOT+Xh%0yv(UO_Ba#2#QPa0_-Oc_loBR>+2(2AXxEXCi4&&+K=ngI{u!7$e6y;;>6dUcL?V^XpK0kZn(< z5-vv5xJP5O3EvU#MlBwHkt-G~0`6Fu8fdJ)J`L^FP0eTALB=qWKG6G#UeR%!HYM#? z-+;H{sYgQ}uKZ zgs(x`NzNgl0ew$ZhoL7dWe(uGqFL&Lh-&jxzrqga zcbF~e+ytUyH9U(~gF}kFYShC>#K0SkBp1N*F@sn83shL1s z!CUzBV%B}AbzWvm5T8+{7_qLz(GM%2w|R=!@av8ci6oy$qEGZntigE4*`GxYkk2Tv zv8vygWri$PEBo8BEmF z4GEI`omU16nxH#TFi3yaV=u=6viLMqvh~}ZYS5d02wr%R?LE||Hd`(_71dttTA`qz zt8=X(Nbu>cmLY0O*DUZPk@7?quubf*sO(@8q7UTks(Zl6;Z#2R@wo7+x_xxcBCRI( zg-F+xMW=aMbai!RbGG~SXc$tGhr?g@4tQFg)h5%e3`zKUC0k`Km2;C z_y5x0V%vmFt`0cA%$JESZHdiRM|Y+MsqIK@N~WF6yTphqQ%hLBUDp`0TJ%P#j{19} z?kTy-X1%&272T90>HmF3d7PHxWD(~+I5m1`e$UB;jsba?=a9`6jWzD}Qm}+i0Iq`e z^a;lDNyrt?SQ^s-8Aw0k&RpSc3RZTd=h})@*vaFmusLytcicnsxrZ#)o};iH`}>@P zm8jZUJNtA()%J}>Q=_?lb4`b>$UD^UWEi^0$Qygra=3!Pa{ajehjt&$^s?6H_;f{k zaLVNL)!P0t2#+;FPNVsqBX{`l zUVYZ%3}Ixst4oFGKG*!{`c?>XO-c>`AI4}P-KRMa5?POL0+pNrIZrZ^$S8bg^n&c@ z_S+t%Bk))|E`37e0O#oCa}m*bGp~&V!TwaSJ`?#&QaZej?TgUb^~1+GdcuQG zr~cz?H<3o<(C{hO5$GSf)+T=5T#A+ZS8NfytaNXWTs(>Q=qwV8^Lsg^3sj?;`Ebq~ zz0zNVK{$<+e8Tm@Zm;~R*f!=)vj^xtfDo_0llcwQ>}qd1)~h}Hl8d-v;uBhQTiqdP zvFMt+i?;BRk!|F0wXh)9y&SNDO&xhi33jRFVZ739T@I@ojdg1e-P5XEc|2C#K&FAu zSF#0s>LFf=-@44&pS!!<=8n;?1szX29MN(gnS_j2umNH~Bj>Eh(QT)lp47`}1|;{o zez>kxmO%t8MRtw8d{h>{6e2dW1%5g-v|U-E-MBN-sYv+7xd5v=#!3|u8MDh6HL8gx zR&l*k$6WnB{6mFpl-tKq2d$_xL?7a&YGJ&&u17&jFu@Id>l%nScD3gW9=WJ5qT7Bk znV^*n6d^rVNe{C%h<~g>7Ixk$S1Md7wzVA6Ur!$>vmpCo)U$T!y@rL04O`jTiZy!p zW<*B_+{L?K6yJC8I_EsJ8e~1@)gPtKWF0do*Xtvv6D^jpm20RtbidfwmW!|2Y;jR$ zpU;*XlELU)jw9XG#uik?*tm3Wx;3zCc=!B9x=xAp`n!(YO%vVz{1A*7Ngr|7_|{qJG}q-7)=o)T3_A(V{!1f2D4{ zH}BN6B`zUi?Gxg@@4h2EwCsun8+fVf`-sr`EIdhnJxAM?JbzzO5V6EJF~WUa(F5$R z*kbj)iZ$7N*}r=Iu6@~o^LJdI;hrJREpS+9v+O<|chPQ~m(7I}b!cO^MV0w-R_@vr zTZ0R(yO0|f*?Bi%2P&VIg|2FYmVbVFn_s-&<*nZB6;SoLKf}FIumSj;y`zKc=`{wr z-up2vJ#QbgWEb04^XA2S)V96a9_5M|d7t>;ZXkGx(?xuaJ1F_SZ`UN?4Ubz}hx|!$ah{Z*F_W7so)?6O5?iTE(zv-HbBOOkisItHVzH_v- zb>WJ_0i`a!m?OAmOunaUA2{gh-2b`k{L8QueH%_l#I+-c9L4pJMNwH}v&O!8&DZ!I zX`THJOLXT2zqS>^qMa9AC2D(q+v>8lcW-W2XMxDX(MRO2`1V@Rwyfg&1E_THPN|#w z`=`-+h9QYl?zL}ww6W#~)mpQBxUF+c^whSsR-A!j_AQ;qZ(Ohf^)Q)bb~asVIu2`k ztmuC<{E7ecsI~f5GbiW2UpwzEgW^7kOV_1uPxlepY{PEWY^XI03vpK)XTbzCTDu-}@Bzz0BTyj}AQERdinde?NTvh5rxPlSRw_XV4Y% H-7fzJ0A*$D literal 6565 zcmbtZ>uwu27XI(2IOqa~9Oz=#X|YhV1{_;yjXI8Dd+TiLK%izQiJK8QB{_23qQE}H zKH)yeeuv~pGqzR7sS)@h@*JM?o!dud@M4Qz(B=8@``)R_q;|4*l1uH?R7pFe*O$k= z{oYX_Dkrg#_tLph+S5^Cs+>;M#0q;yo%5s99qQ5T?QOs0^^BKY-`H7@IWpy)RkOLL zPPRi|@9ypPy7aD;n$D%oWa*XBT{_WOzdq%#C@4rcveHSrlsT^rwzmFOS}|J)GCGs^ zOX;kra!IestQaih!q~h1{NUc_R?d|3R`59kp>t`S(PNQivXJr9CuCGsKHTA?u-Sa@ zG51!=jmvS@!)4ezUWm*Z_sAM!IQDm?42unKMrtPY;K*2c&~!dq2;cI0DKl>@_Gqz? zt`wOh97w-!4PJyW-b}2LQ+gwuzc7=^`4b)&GEslQi4ru9IIMsx75(OQmK(lSlSO=jgjD+&c zXq*8|Gg%*fqm8d!MczA7@ZQ|&bPhx;QnEDA4k=p53|=U=powt60(LS~(N;csTIq}v zuTSjM)D(reWz&_OORGFY7*r=cJLG6CmXfp~Ih`Vg!w77T*WZdy2z=x0Viqo8|68y zqtdCuXt>FzV8ey+p_Us9NwHjtq7neL9nFZIT2zaaj7B@CTVcg;=xQ;6`(_FRw6wj# zm=dv14I;QuVm&2cL=E`QWDG3e#%d5+h&!^CrbShFg#x0m4cQs0h#ig6xy-~JV00#= zjE7f6(~Q;GlnG7pQzn60fQ-Gom#`#7 zp5QYpISFVY(IA37O_f&y6^~g24LjjHXkFz$U_GircCZUogsMzAOr1U1`+MAnPCXn3Y|t>PJi&Cp6^$6L5E6x& zY3jsJqvQ=ttRrv(Tdk=|Msqd&VCZ1WOSE<1L$7$m@PvpFh){Q_{b@w2@#!TBTv+-C zO$#IZ!B(i>r$))vkjiROsBAqB(liK;MjvYfVqSzObtxWqfY}ulpe3Sza3Ib-TjA!& zf_z+h`wA=k^;v26XJ;J%<8;OmI)x7hCFPCdP*y0g=bV;wL^GfBy@|*dD@jPgyOn#d zF|&YS>Wb~@|8z{&Fs)LC6%L1Qg}3U{%Z=;nY#Lh$I_@AKf-ijLR9PQL>5#3Nz=IfmtVBW75cc%KYX}n z2GpRxwB_Qoaj0Ibf`n0GU?VLim=Ng7&=!6@OthuVS7IL&EIAMyQp-m(8K^t=hrJoCrn1SO=Fh98WO=QNO7nr1~je3Du1oe zkUii7j6AO~BQb?gUQDROM1@66(uiris=$aqAbtwdnA}1XxRJnC$nT};CKe@+eM3x+ z5#TnKwe>$l*Dm2%$N79XWFN=AcKIQDzPbtfj?@!j&rGi?q$}Fk#U5 zBTc6jci#*#;IbZs!_To>d;}ml_;+t5$s8@QTQHb}U8Dj5tcWlW6A@9+8V55GFh2)h znVW*?;R7g-qoL82*BF}mf-9_(8w@TLt}0Pu&V_5mLjX8iL_{H^)RzD-()lyoPgGZBKnyqsW=oc^y{o!qsRFmw0z6!T>SRm> z`L@B9#s1aV?SHe=r+bK_Z!x{)jcDx!m?L-8)G(R-BLMimx)pTkc8-$>e5lbPLNiq8 z0({*?H3xbhRb>@uGw#kI!cQjQF<%fKo&J%4g0_}IoUt82f3b)2RC zdj{*u93hSp3Db!*u#IJx!~OVI++LfyjUmhzobdwp-DgOlMPd#Hsb-6QXl-rgl{w5j zMpWC}e4Fqt7Ox|Jayq+?e0i?X55zWDod$bVf{V!ZdxQHDdz%qK2G$sFcJP5fG<35- zxT3|Uz2WdOJOuI!WV*J!|JTn446Kx!HLU7m8DHG#Gf5P^H({diJ}d* zhg6M~7!oyfgAtxFW_Jm|M`UrSjiXIhO*Qq-wW90r-d2aAUT;{jK~pOmgGo`7%rDeT z<*RPhit%R!tS+r*2d*u+b|sRF$Kk|m&wo5akn#5Q7R*-YWbiuT@uI%|J7xd|K$UL@ z=U>bqeuUQ?mIo&Ps2|bM zNT*0cj11hJ8oYD0A#v_8Hh#mKRw*~0FvS`NTE(mT{oVFD=+Am}ufIzJ>R=NFay;l) z>-p~;aI&V_z&+N2nDe1HKq@_0Xh<0Osbo$WXr06J$9TmH*$_O$d>cd_n+aiftegwm z@Sh*!w}(HD#)qdT-=B?+$M4QZ7l&8xF2-l0>n+?t|KMk<) Date: Thu, 28 May 2026 12:32:18 +0800 Subject: [PATCH 27/28] Fix file encoding: convert fricp.h from UTF-16 LE to UTF-8 --- registration/include/pcl/registration/fricp.h | Bin 13294 -> 6453 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index 6327a482a1001cc9361a10595217c5d384dcf34a..8b7afecf4fa4a1e8349dcbde59928224124decc9 100644 GIT binary patch literal 6453 zcmb_h?QR=47X9z1xab0g9O%QY(_*1`y>M)$HR?Eq?X9z|1A&^MByL9Jl;qe^i~ipi z*@xS=*mFsaG!t8K(yn00(vZBo_uQ{TYVd4}p3&9C$=lwU%A|I(cbZG>)l^A4q?cDG zz5U*CAu1;^k>}EdQQFgSVXBu5auttT{I2(yH0q zQzzS@uXp!$dtG`{N=+BiX0r6k=q{b=tY7bPR1_3eII_}7dna=q8*FX;rLF z@lENhsB%dMWmXKVoGIt6V0i2UgbnGpt--V4#@mTia!Rj+^Ot5)Ie+Q{LU$6Lb<+9hX0<2_xj;ZT@^eYAE>Djy z=uTKA5Cge%nN<^+lhVP>6kcDkPy+fCCDTeYM-9u4G4xfJewqtCBZ=8Xzl*r|dan+E zYwYGsaVIVOUnuF|Z@Q6%%jS4+FFBYL)=mv31Ov2mF6?STIEjLeQUI`S%@_!arce^X zGo!HwFwJDW^{qC(HWhL2NWpq@uhTwQViA(1fpn~*h0I`uatoRW2P~i`Llt%9t*4dF zI3WAPMompon0r=T>AAGZV+n)mq-TR1&BdK0ZAeb1@Zm55+vD-q;sYGt*!u**E}%na zhyMHT|NK3A2Nrp23aDIp0LS?K3xc2k-6>J~X{{o;OcaO?VG$_|iPAtugmi;Hd(_^O z;6H_EGuCyJ=P3PjAkT9ou ziX?^v*Qax%dsC%hC+iiKL(zmL0y{6TC&PZPP?F)@yh6~U>T)Dz9zJ)lGPsWg+-4?^ zlCokre4@M@gd%g@jrHLA5W;3JSPK64;j&=Tm8sAosN-z5^MVEg!iS)cpP_C*(PXTj z3m+|>;F*;i1T>K-5J8`&$ZLU&M=yeeovh>w@)j!A9=L_B)>I{FvR1Mct>iIt0XNi(?cDANFbPkoH7cA;JD1+pYF{i5>!EQ%DRz3mgt#3vbnj=Nre@!4`4(jEONUUTi^iiP9OXcxw5tQp=;?=={z3s7p7av#aCp z>Gv@G3BWoCWeB}9D#yQ2?U|s|GCPq~)L&^Dr~mx3Hn~DwwfVbuAE^N`$oKZTSp6Nc z38NrIlo-)SiwP<=swAX^k4K3{v^fN-DL0MSbD451)UP3*LkmFnA?C|?qnkN+F0u?_ zQAQ9mHQ@+V9R+3@sR+u7Gf2f;c=!yQV}g3lwP+$UeqNvxMu})A5Ek1`E~f$on9xU) z!e|kO-b-0xA#(b*!Vk)}h5(9nMj-I3fv6gW9RSFGPl-9{`*Zo`I{tir)i+xVwbb84uoc`G=l(kD3E#_(U3Ea zI=fxkd)KF&JN<=7z3RQrQpUCDwx1*@60*%F4 zHhB2SAUt8h{viHPdpxMH%$b>_s)~vYswUf1uk&%6-AgL;o`>S|7_{8pP zI^yL^m42{ngVw3BS0%WJY@h$#AJMnz5kz2(@n!=bEQpG3UJ#B<@d|D@yb2d~d?T2S zQE&hG(;)*Z8mhqv*9x;` z0`QTwIAq4wrmLoydgfZtO}PK6T~UuWwAdi26^%iqC`sm*YNqm4wQBkJvm92Z))zaj zy>M+xBo~jviQb<7a1JNqF6uRyt`02)A*Z*b?Ij3B-g*X))FuEUz% zYhaGJ!YS;`%v$(9Z{~b+W&6OA_}aL>P8@Sl4GbNZX=71ecJr|uVThK2vr~h2t|lbT zJ;uauc+x85#uKJk<3Ou;bicpb9tZtdkM8w%X+RxJ!VxEK^o#ZUeFvPZDK>DA^+NRd zkQ^YD9xOBjjQmhCrwp{t;W}cx;)SdTE=Il$OCIY9p?R!~3)App#Q62m52NwX+39!Z wqm%KQi_zuL^_$D_`RHZ~XVBmI8mVgkAhdr-;7jnq)P~;%%JOjQfBmIfX8-^I literal 13294 zcmd6udv9CE5ykiK0{sp|f);WSS$6CMh3lq}Ex9r3#zw5hY2reVDN0eIha*yUoFZSM zAGq!L?cr#5FE1%d&IGqZEA{`cRt^oR7@^qVx$>mWT%Ptr#EJgufj=^zc$ zC>^J$Yuw3|Oo^!=V5qjZ|?XvRIw+0gH|^thi6(@r|eGh)qn9cb*6bfTHZX`=Ux zJ<-@*y=Si%T5m^>H~PGyk@xlOHN9We<8}VKo?f2MAD+)1r|bImH0@Vfu=F+A@>(!QA(l*vc`k-?~Dmpq2`6>IX;Q{J&k>!Szzx| zt@|R!`$YUO(XW9p_H~5gEymWdf1ur8W<(+dd_dHo36ea}x(_w`iQd1;xH-_XS%XLz zOJd^fh2Fy+#0fSsgAtYG@11j52YC&~*HRC9v-Y8U z44=R*?~7mh_+o3SA+dRwx#3hZ@bsayKr{2eAU(_9q3KuC5wG=4a2Xo2_g?lZD-=H* z=XF@gT)>Kj&yW+=YFVOKZysBez4o*&{>0Y<$p_2eZ5S9|f~G#*pX;}iJg@bzvjy@V z=vQ0dwZ^tud8gBCD^Sa>&PPTsIKo!w3@;vO{);TBF~azQu-dvs_dOvQ`GZZbYesks zOT+Xh%0yv(UO_Ba#2#QPa0_-Oc_loBR>+2(2AXxEXCi4&&+K=ngI{u!7$e6y;;>6dUcL?V^XpK0kZn(< z5-vv5xJP5O3EvU#MlBwHkt-G~0`6Fu8fdJ)J`L^FP0eTALB=qWKG6G#UeR%!HYM#? z-+;H{sYgQ}uKZ zgs(x`NzNgl0ew$ZhoL7dWe(uGqFL&Lh-&jxzrqga zcbF~e+ytUyH9U(~gF}kFYShC>#K0SkBp1N*F@sn83shL1s z!CUzBV%B}AbzWvm5T8+{7_qLz(GM%2w|R=!@av8ci6oy$qEGZntigE4*`GxYkk2Tv zv8vygWri$PEBo8BEmF z4GEI`omU16nxH#TFi3yaV=u=6viLMqvh~}ZYS5d02wr%R?LE||Hd`(_71dttTA`qz zt8=X(Nbu>cmLY0O*DUZPk@7?quubf*sO(@8q7UTks(Zl6;Z#2R@wo7+x_xxcBCRI( zg-F+xMW=aMbai!RbGG~SXc$tGhr?g@4tQFg)h5%e3`zKUC0k`Km2;C z_y5x0V%vmFt`0cA%$JESZHdiRM|Y+MsqIK@N~WF6yTphqQ%hLBUDp`0TJ%P#j{19} z?kTy-X1%&272T90>HmF3d7PHxWD(~+I5m1`e$UB;jsba?=a9`6jWzD}Qm}+i0Iq`e z^a;lDNyrt?SQ^s-8Aw0k&RpSc3RZTd=h})@*vaFmusLytcicnsxrZ#)o};iH`}>@P zm8jZUJNtA()%J}>Q=_?lb4`b>$UD^UWEi^0$Qygra=3!Pa{ajehjt&$^s?6H_;f{k zaLVNL)!P0t2#+;FPNVsqBX{`l zUVYZ%3}Ixst4oFGKG*!{`c?>XO-c>`AI4}P-KRMa5?POL0+pNrIZrZ^$S8bg^n&c@ z_S+t%Bk))|E`37e0O#oCa}m*bGp~&V!TwaSJ`?#&QaZej?TgUb^~1+GdcuQG zr~cz?H<3o<(C{hO5$GSf)+T=5T#A+ZS8NfytaNXWTs(>Q=qwV8^Lsg^3sj?;`Ebq~ zz0zNVK{$<+e8Tm@Zm;~R*f!=)vj^xtfDo_0llcwQ>}qd1)~h}Hl8d-v;uBhQTiqdP zvFMt+i?;BRk!|F0wXh)9y&SNDO&xhi33jRFVZ739T@I@ojdg1e-P5XEc|2C#K&FAu zSF#0s>LFf=-@44&pS!!<=8n;?1szX29MN(gnS_j2umNH~Bj>Eh(QT)lp47`}1|;{o zez>kxmO%t8MRtw8d{h>{6e2dW1%5g-v|U-E-MBN-sYv+7xd5v=#!3|u8MDh6HL8gx zR&l*k$6WnB{6mFpl-tKq2d$_xL?7a&YGJ&&u17&jFu@Id>l%nScD3gW9=WJ5qT7Bk znV^*n6d^rVNe{C%h<~g>7Ixk$S1Md7wzVA6Ur!$>vmpCo)U$T!y@rL04O`jTiZy!p zW<*B_+{L?K6yJC8I_EsJ8e~1@)gPtKWF0do*Xtvv6D^jpm20RtbidfwmW!|2Y;jR$ zpU;*XlELU)jw9XG#uik?*tm3Wx;3zCc=!B9x=xAp`n!(YO%vVz{1A*7Ngr|7_|{qJG}q-7)=o)T3_A(V{!1f2D4{ zH}BN6B`zUi?Gxg@@4h2EwCsun8+fVf`-sr`EIdhnJxAM?JbzzO5V6EJF~WUa(F5$R z*kbj)iZ$7N*}r=Iu6@~o^LJdI;hrJREpS+9v+O<|chPQ~m(7I}b!cO^MV0w-R_@vr zTZ0R(yO0|f*?Bi%2P&VIg|2FYmVbVFn_s-&<*nZB6;SoLKf}FIumSj;y`zKc=`{wr z-up2vJ#QbgWEb04^XA2S)V96a9_5M|d7t>;ZXkGx(?xuaJ1F_SZ`UN?4Ubz}hx|!$ah{Z*F_W7so)?6O5?iTE(zv-HbBOOkisItHVzH_v- zb>WJ_0i`a!m?OAmOunaUA2{gh-2b`k{L8QueH%_l#I+-c9L4pJMNwH}v&O!8&DZ!I zX`THJOLXT2zqS>^qMa9AC2D(q+v>8lcW-W2XMxDX(MRO2`1V@Rwyfg&1E_THPN|#w z`=`-+h9QYl?zL}ww6W#~)mpQBxUF+c^whSsR-A!j_AQ;qZ(Ohf^)Q)bb~asVIu2`k ztmuC<{E7ecsI~f5GbiW2UpwzEgW^7kOV_1uPxlepY{PEWY^XI03vpK)XTbzCTDu-}@Bzz0BTyj}AQERdinde?NTvh5rxPlSRw_XV4Y% H-7fzJ0A*$D From 9ffa0110c7319ac7421bbc2c68089ab5a595fe8e Mon Sep 17 00:00:00 2001 From: yaoyx689 Date: Thu, 28 May 2026 14:38:39 +0800 Subject: [PATCH 28/28] Fix: add in-class default for robust_function_ after removing from init-list Previously removed robust_function_ from constructor init-list along with nu_end_ratio_, but robust_function_ had no in-class initializer, leading to default-initialization of an enum class (indeterminate value). Add explicit in-class default to WELSCH to preserve correct behavior. --- registration/include/pcl/registration/fricp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h index 8b7afecf4fa..58f0dcb3647 100644 --- a/registration/include/pcl/registration/fricp.h +++ b/registration/include/pcl/registration/fricp.h @@ -177,7 +177,7 @@ class FastRobustIterativeClosestPoint Matrix4d matrixLog(const Matrix4d& transform) const; - RobustFunction robust_function_; + RobustFunction robust_function_ = RobustFunction::WELSCH; bool use_anderson_ = false; std::size_t anderson_history_ = 5; double nu_begin_ratio_ = 3.0;