From aefe5db5f79ec69c75350dfcf9d6b62ec7adc9d5 Mon Sep 17 00:00:00 2001 From: yaoyx689 Date: Wed, 11 Dec 2024 19:36:34 +0800 Subject: [PATCH 01/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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::