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