From 400a104cb9aae2da3569ef6420b09c2840fd8bb9 Mon Sep 17 00:00:00 2001 From: keineahnung2345 Date: Tue, 7 Jun 2022 11:53:15 +0800 Subject: [PATCH 1/4] [feat] gicp: add convergence criteria and correspondence estimation --- registration/include/pcl/registration/gicp.h | 48 +++ .../registration/gicp_convergence_criteria.h | 369 ++++++++++++++++++ .../include/pcl/registration/impl/gicp.hpp | 184 +++++---- .../impl/gicp_convergence_criteria.hpp | 143 +++++++ 4 files changed, 674 insertions(+), 70 deletions(-) create mode 100644 registration/include/pcl/registration/gicp_convergence_criteria.h create mode 100644 registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index d17e8a1048b..f803d8930b9 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -42,6 +42,7 @@ #include #include +#include namespace pcl { /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the @@ -76,6 +77,17 @@ class GeneralizedIterativeClosestPoint using IterativeClosestPoint::inlier_threshold_; using IterativeClosestPoint::min_number_correspondences_; using IterativeClosestPoint::update_visualizer_; + using IterativeClosestPoint::correspondences_; + using IterativeClosestPoint::correspondence_estimation_; + using IterativeClosestPoint::correspondence_rejectors_; + using IterativeClosestPoint::setUseReciprocalCorrespondences; + using IterativeClosestPoint::getUseReciprocalCorrespondences; + using IterativeClosestPoint::determineRequiredBlobData; + using IterativeClosestPoint::use_reciprocal_correspondence_; + using IterativeClosestPoint::source_has_normals_; + using IterativeClosestPoint::target_has_normals_; + using IterativeClosestPoint::need_source_blob_; + using IterativeClosestPoint::need_target_blob_; using PointCloudSource = pcl::PointCloud; using PointCloudSourcePtr = typename PointCloudSource::Ptr; @@ -102,6 +114,9 @@ class GeneralizedIterativeClosestPoint using Vector6d = Eigen::Matrix; + typename pcl::registration::GICPConvergenceCriteria::Ptr + convergence_criteria_; + /** \brief Empty constructor. */ GeneralizedIterativeClosestPoint() : k_correspondences_(20) @@ -125,6 +140,39 @@ class GeneralizedIterativeClosestPoint estimateRigidTransformationBFGS( cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); }; + convergence_criteria_.reset( + new pcl::registration::GICPConvergenceCriteria( + nr_iterations_, transformation_, previous_transformation_, *correspondences_)); + } + + /** + * \brief Due to `convergence_criteria_` holding references to the class members, + * it is tricky to correctly implement its copy and move operations correctly. This + * can result in subtle bugs and to prevent them, these operations for GICP have + * been disabled. + * + * \todo: remove deleted ctors and assignments operations after resolving the issue + */ + GeneralizedIterativeClosestPoint(const GeneralizedIterativeClosestPoint&) = delete; + GeneralizedIterativeClosestPoint(GeneralizedIterativeClosestPoint&&) = delete; + GeneralizedIterativeClosestPoint& + operator=(const GeneralizedIterativeClosestPoint&) = delete; + GeneralizedIterativeClosestPoint& + operator=(GeneralizedIterativeClosestPoint&&) = delete; + + /** \brief Returns a pointer to the GICPConvergenceCriteria used by the + * GeneralizedIterativeClosestPoint class. This allows to check the convergence state after the + * align() method as well as to configure GICPConvergenceCriteria's parameters not + * available through the GICP API before the align() method is called. Please note that + * the align method sets max_iterations_, transformation_epsilon_ and + * rotation_epsilon_ and therefore overrides the default / set values of the + * GICPConvergenceCriteria instance. \return Pointer to the GeneralizedIterativeClosestPoint's + * GICPConvergenceCriteria. + */ + inline typename pcl::registration::GICPConvergenceCriteria::Ptr + getConvergeCriteria() + { + return convergence_criteria_; } /** \brief Provide a pointer to the input dataset diff --git a/registration/include/pcl/registration/gicp_convergence_criteria.h b/registration/include/pcl/registration/gicp_convergence_criteria.h new file mode 100644 index 00000000000..2b046a7e8bc --- /dev/null +++ b/registration/include/pcl/registration/gicp_convergence_criteria.h @@ -0,0 +1,369 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * 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 +#include + +namespace pcl { +namespace registration { +/** \brief @b GICPConvergenceCriteria represents an instantiation of + * ConvergenceCriteria, and implements the following criteria for registration loop + * evaluation: + * + * * a maximum number of iterations has been reached + * * the transformation (R, t) cannot be further updated (the difference between + * current and previous is smaller than a threshold) + * * the Mean Squared Error (MSE) between the current set of correspondences and the + * previous one is smaller than some threshold (both relative and absolute tests) + * + * \note Convergence is considered reached if ANY of the above criteria are met. + * + * \author Radu B. Rusu + * \ingroup registration + */ +template +class GICPConvergenceCriteria : public ConvergenceCriteria { +public: + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + using Matrix4 = Eigen::Matrix; + + enum ConvergenceState { + CONVERGENCE_CRITERIA_NOT_CONVERGED, + CONVERGENCE_CRITERIA_ITERATIONS, + CONVERGENCE_CRITERIA_TRANSFORM, + CONVERGENCE_CRITERIA_ABS_MSE, + CONVERGENCE_CRITERIA_REL_MSE, + CONVERGENCE_CRITERIA_NO_CORRESPONDENCES, + CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS + }; + + /** \brief Empty constructor. + * Sets: + * * the maximum number of iterations to 1000 + * * the rotation threshold to 0.256 degrees (0.99999) + * * the translation threshold to 0.0003 meters (3e-4^2) + * * the MSE relative / absolute thresholds to 0.001% and 1e-12 + * + * \param[in] iterations a reference to the number of iterations the loop has ran so + * far \param[in] transform a reference to the current transformation obtained by the + * transformation evaluation \param[in] correspondences a reference to the current set + * of point correspondences between source and target + */ + GICPConvergenceCriteria (const int& iterations, + const Matrix4& transform, + const Matrix4 &previous_transform, + const pcl::Correspondences& correspondences) + : iterations_(iterations) + , transformation_(transform) + , previous_transformation_ (previous_transform) + , correspondences_(correspondences) + , correspondences_prev_mse_(std::numeric_limits::max()) + , correspondences_cur_mse_(std::numeric_limits::max()) + , max_iterations_(100) // 100 iterations + , failure_after_max_iter_(false) + , rotation_threshold_(0.99999) // 0.256 degrees + , rotation_epsilon_ (2e-3) + , translation_threshold_(3e-4 * 3e-4) // 0.0003 meters + , mse_threshold_relative_(0.00001) // 0.001% of the previous MSE (relative error) + , mse_threshold_absolute_(1e-12) // MSE (absolute error) + , iterations_similar_transforms_(0) + , max_iterations_similar_transforms_(0) + , convergence_state_(CONVERGENCE_CRITERIA_NOT_CONVERGED) + {} + + /** \brief Empty destructor */ + ~GICPConvergenceCriteria () {} + + /** \brief Set the maximum number of consecutive iterations that the internal + * rotation, translation, and MSE differences are allowed to be similar. \param[in] + * nr_iterations the maximum number of iterations + */ + inline void + setMaximumIterationsSimilarTransforms(const int nr_iterations) + { + max_iterations_similar_transforms_ = nr_iterations; + } + + /** \brief Get the maximum number of consecutive iterations that the internal + * rotation, translation, and MSE differences are allowed to be similar, as set by the + * user. + */ + inline int + getMaximumIterationsSimilarTransforms() const + { + return (max_iterations_similar_transforms_); + } + + /** \brief Set the maximum number of iterations the internal optimization should run + * for. \param[in] nr_iterations the maximum number of iterations the internal + * optimization should run for + */ + inline void + setMaximumIterations(const int nr_iterations) + { + max_iterations_ = nr_iterations; + } + + /** \brief Get the maximum number of iterations the internal optimization should run + * for, as set by the user. */ + inline int + getMaximumIterations() const + { + return (max_iterations_); + } + + /** \brief Specifies if the registration fails or converges when the maximum number of + * iterations is reached. \param[in] failure_after_max_iter If true, the registration + * fails. If false, the registration is assumed to have converged. + */ + inline void + setFailureAfterMaximumIterations(const bool failure_after_max_iter) + { + failure_after_max_iter_ = failure_after_max_iter; + } + + /** \brief Get whether the registration will fail or converge when the maximum number + * of iterations is reached. */ + inline bool + getFailureAfterMaximumIterations() const + { + return (failure_after_max_iter_); + } + + /** \brief Set the rotation threshold cosine angle (maximum allowable difference + * between two consecutive transformations) in order for an optimization to be + * considered as having converged to the final solution. \param[in] threshold the + * rotation threshold in order for an optimization to be considered as having + * converged to the final solution. + */ + inline void + setRotationThreshold(const double threshold) + { + rotation_threshold_ = threshold; + } + + /** \brief Get the rotation threshold cosine angle (maximum allowable difference + * between two consecutive transformations) as set by the user. + */ + inline double + getRotationThreshold() const + { + return (rotation_threshold_); + } + + /** \brief Set the rotation epsilon (maximum allowable difference between two + * consecutive rotations) in order for an optimization to be considered as having + * converged to the final solution. + * \param[in] epsilon the rotation epsilon + */ + inline void + setRotationEpsilon(const double epsilon) + { + rotation_epsilon_ = epsilon; + } + + /** \brief Get the rotation epsilon (maximum allowable difference between two + * consecutive rotations) as set by the user. + */ + inline double + getRotationEpsilon () const + { + return (rotation_epsilon_); + } + + /** \brief Set the translation threshold (maximum allowable difference between two + * consecutive transformations) in order for an optimization to be considered as + * having converged to the final solution. \param[in] threshold the translation + * threshold in order for an optimization to be considered as having converged to the + * final solution. + */ + inline void + setTranslationThreshold(const double threshold) + { + translation_threshold_ = threshold; + } + + /** \brief Get the rotation threshold cosine angle (maximum allowable difference + * between two consecutive transformations) as set by the user. + */ + inline double + getTranslationThreshold() const + { + return (translation_threshold_); + } + + /** \brief Set the relative MSE between two consecutive sets of correspondences. + * \param[in] mse_relative the relative MSE threshold + */ + inline void + setRelativeMSE(const double mse_relative) + { + mse_threshold_relative_ = mse_relative; + } + + /** \brief Get the relative MSE between two consecutive sets of correspondences. */ + inline double + getRelativeMSE() const + { + return (mse_threshold_relative_); + } + + /** \brief Set the absolute MSE between two consecutive sets of correspondences. + * \param[in] mse_absolute the relative MSE threshold + */ + inline void + setAbsoluteMSE(const double mse_absolute) + { + mse_threshold_absolute_ = mse_absolute; + } + + /** \brief Get the absolute MSE between two consecutive sets of correspondences. */ + inline double + getAbsoluteMSE() const + { + return (mse_threshold_absolute_); + } + + /** \brief Check if convergence has been reached. */ + bool + hasConverged() override; + + /** \brief Return the convergence state after hasConverged () */ + ConvergenceState + getConvergenceState() + { + return (convergence_state_); + } + + /** \brief Sets the convergence state externally (for example, when ICP does not find + * enough correspondences to estimate a transformation, the function is called setting + * the convergence state to ConvergenceState::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES) + * \param[in] c the convergence state + */ + inline void + setConvergenceState(ConvergenceState c) + { + convergence_state_ = c; + } + +protected: + /** \brief Calculate the mean squared error (MSE) of the distance for a given set of + * correspondences. \param[in] correspondences the given set of correspondences + */ + inline double + calculateMSE(const pcl::Correspondences& correspondences) const + { + double mse = 0; + for (const auto& correspondence : correspondences) + mse += correspondence.distance; + mse /= double(correspondences.size()); + return (mse); + } + + /** \brief The number of iterations done by the registration loop so far. */ + const int& iterations_; + + /** \brief The current transformation obtained by the transformation estimation + * method. */ + const Matrix4& transformation_; + + /** \brief The previous transformation obtained by the transformation estimation method. */ + const Matrix4& previous_transformation_; + + /** \brief The current set of point correspondences between the source and the target. + */ + const pcl::Correspondences& correspondences_; + + /** \brief The MSE for the previous set of correspondences. */ + double correspondences_prev_mse_; + + /** \brief The MSE for the current set of correspondences. */ + double correspondences_cur_mse_; + + /** \brief The maximum nuyyGmber of iterations that the registration loop is to be + * executed. */ + int max_iterations_; + + /** \brief Specifys if the registration fails or converges when the maximum number of + * iterations is reached. */ + bool failure_after_max_iter_; + + /** \brief The rotation threshold is the relative rotation between two iterations (as + * angle cosine). */ + double rotation_threshold_; + + /** \brief The rotation epsilon is the maximum allowable difference between two consecutive rotations. */ + double rotation_epsilon_; + + /** \brief The translation threshold is the relative translation between two + * iterations (0 if no translation). */ + double translation_threshold_; + + /** \brief The relative change from the previous MSE for the current set of + * correspondences, e.g. .1 means 10% change. */ + double mse_threshold_relative_; + + /** \brief The absolute change from the previous MSE for the current set of + * correspondences. */ + double mse_threshold_absolute_; + + /** \brief Internal counter for the number of iterations that the internal + * rotation, translation, and MSE differences are allowed to be similar. */ + int iterations_similar_transforms_; + + /** \brief The maximum number of iterations that the internal rotation, + * translation, and MSE differences are allowed to be similar. */ + int max_iterations_similar_transforms_; + + /** \brief The state of the convergence (e.g., why did the registration converge). */ + ConvergenceState convergence_state_; + +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace registration +} // namespace pcl + +#include diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index 1866724d968..b3a66dca9fd 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -405,6 +405,8 @@ GeneralizedIterativeClosestPoint::computeTransformatio PointCloudSource& output, const Eigen::Matrix4f& guess) { pcl::IterativeClosestPoint::initComputeReciprocal(); + // Point cloud containing the correspondences of each point in + PointCloudSourcePtr input_transformed(new PointCloudSource); // Difference between consecutive transforms double delta = 0; // Get the size of the source point cloud @@ -425,16 +427,90 @@ GeneralizedIterativeClosestPoint::computeTransformatio base_transformation_ = Eigen::Matrix4f::Identity(); nr_iterations_ = 0; converged_ = false; - double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; - pcl::Indices nn_indices(1); - std::vector nn_dists(1); pcl::transformPointCloud(output, output, guess); + pcl::transformPointCloud(output, *input_transformed, transformation_); + + // Make blobs if necessary + determineRequiredBlobData(); + PCLPointCloud2::Ptr target_blob(new PCLPointCloud2); + if (need_target_blob_) + pcl::toPCLPointCloud2(*target_, *target_blob); + + // Pass in the default target for the Correspondence Estimation/Rejection code + correspondence_estimation_->setInputTarget(target_); + if (correspondence_estimation_->requiresTargetNormals()) + correspondence_estimation_->setTargetNormals(target_blob); + // Correspondence Rejectors need a binary blob + for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + if (rej->requiresTargetPoints()) + rej->setTargetPoints(target_blob); + if (rej->requiresTargetNormals() && target_has_normals_) + rej->setTargetNormals(target_blob); + } + + convergence_criteria_->setMaximumIterations(max_iterations_); + //convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_); + convergence_criteria_->setTranslationThreshold(transformation_epsilon_); + convergence_criteria_->setRotationThreshold(rotation_epsilon_); + + do { + // Get blob data if needed + PCLPointCloud2::Ptr input_transformed_blob; + if (need_source_blob_) { + input_transformed_blob.reset(new PCLPointCloud2); + toPCLPointCloud2(*input_transformed, *input_transformed_blob); + } + + // Set the source each iteration, to ensure the dirty flag is updated + correspondence_estimation_->setInputSource(input_transformed); + if (correspondence_estimation_->requiresSourceNormals()) + correspondence_estimation_->setSourceNormals(input_transformed_blob); + // Estimate correspondences + if (use_reciprocal_correspondence_) + correspondence_estimation_->determineReciprocalCorrespondences( + *correspondences_, corr_dist_threshold_); + else + correspondence_estimation_->determineCorrespondences(*correspondences_, + corr_dist_threshold_); + + // if (correspondence_rejectors_.empty ()) + CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_)); + for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + PCL_DEBUG("Applying a correspondence rejector method: %s.\n", + rej->getClassName().c_str()); + if (rej->requiresSourcePoints()) + rej->setSourcePoints(input_transformed_blob); + if (rej->requiresSourceNormals() && source_has_normals_) + rej->setSourceNormals(input_transformed_blob); + rej->setInputCorrespondences(temp_correspondences); + rej->getCorrespondences(*correspondences_); + // Modify input for the next iteration + if (i < correspondence_rejectors_.size() - 1) + *temp_correspondences = *correspondences_; + } - while (!converged_) { - std::size_t cnt = 0; - pcl::Indices source_indices(indices_->size()); - pcl::Indices target_indices(indices_->size()); + // Check whether we have enough correspondences + if (correspondences_->size() < min_number_correspondences_) { + PCL_ERROR("[pcl::%s::computeTransformation] Not enough correspondences found. " + "Relax your threshold parameters.\n", + getClassName().c_str()); + convergence_criteria_->setConvergenceState( + pcl::registration::GICPConvergenceCriteria< + float>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES); + converged_ = false; + break; + } + + std::size_t cnt = correspondences_->size(); + pcl::Indices source_indices(cnt); + pcl::Indices target_indices(cnt); + for(size_t i = 0; i < cnt; ++i) { + source_indices[i] = (*correspondences_)[i].index_query; + target_indices[i] = (*correspondences_)[i].index_match; + } // guess corresponds to base_t and transformation_ to t Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero(); @@ -445,87 +521,55 @@ GeneralizedIterativeClosestPoint::computeTransformatio Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>(); - for (std::size_t i = 0; i < N; i++) { - PointSource query = output[i]; - query.getVector4fMap() = transformation_ * query.getVector4fMap(); - - if (!searchForNeighbors(query, nn_indices, nn_dists)) { - PCL_ERROR("[pcl::%s::computeTransformation] Unable to find a nearest neighbor " - "in the target dataset for point %d in the source!\n", - getClassName().c_str(), - (*indices_)[i]); - return; - } - - // Check if the distance to the nearest neighbor is smaller than the user imposed - // threshold - if (nn_dists[0] < dist_threshold) { - Eigen::Matrix3d& C1 = (*input_covariances_)[i]; - Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]]; - Eigen::Matrix3d& M = mahalanobis_[i]; - // M = R*C1 - M = R * C1; - // temp = M*R' + C2 = R*C1*R' + C2 - Eigen::Matrix3d temp = M * R.transpose(); - temp += C2; - // M = temp^-1 - M = temp.inverse(); - source_indices[cnt] = static_cast(i); - target_indices[cnt] = nn_indices[0]; - cnt++; - } + for (std::size_t i = 0; i < cnt; i++) { + Eigen::Matrix3d& C1 = (*input_covariances_)[source_indices[i]]; + Eigen::Matrix3d& C2 = (*target_covariances_)[target_indices[i]]; + Eigen::Matrix3d& M = mahalanobis_[source_indices[i]]; + // M = R*C1 + M = R * C1; + // temp = M*R' + C2 = R*C1*R' + C2 + Eigen::Matrix3d temp = M * R.transpose(); + temp += C2; + // M = temp^-1 + M = temp.inverse(); } - // Resize to the actual number of valid correspondences - source_indices.resize(cnt); - target_indices.resize(cnt); + /* optimize transformation using the current assignment and Mahalanobis metrics*/ previous_transformation_ = transformation_; // optimization right here try { rigid_transformation_estimation_( output, source_indices, *target_, target_indices, transformation_); - /* compute the delta from this iteration */ - delta = 0.; - for (int k = 0; k < 4; k++) { - for (int l = 0; l < 4; l++) { - double ratio = 1; - if (k < 3 && l < 3) // rotation part of the transform - ratio = 1. / rotation_epsilon_; - else - ratio = 1. / transformation_epsilon_; - double c_delta = - ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l)); - if (c_delta > delta) - delta = c_delta; - } - } } catch (PCLException& e) { PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName().c_str(), e.what()); break; } + + // Transform the data + pcl::transformPointCloud(output, *input_transformed, transformation_); + nr_iterations_++; if (update_visualizer_ != 0) { - PointCloudSourcePtr input_transformed(new PointCloudSource); - pcl::transformPointCloud(output, *input_transformed, transformation_); update_visualizer_(*input_transformed, source_indices, *target_, target_indices); } - - // Check for convergence - if (nr_iterations_ >= max_iterations_ || delta < 1) { - converged_ = true; - PCL_DEBUG("[pcl::%s::computeTransformation] Convergence reached. Number of " - "iterations: %d out of %d. Transformation difference: %f\n", - getClassName().c_str(), - nr_iterations_, - max_iterations_, - (transformation_ - previous_transformation_).array().abs().sum()); - previous_transformation_ = transformation_; - } - else - PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n", + converged_ = static_cast((*convergence_criteria_)); + } while (convergence_criteria_->getConvergenceState() == + pcl::registration::GICPConvergenceCriteria< + float>::CONVERGENCE_CRITERIA_NOT_CONVERGED); + if(converged_) { + PCL_DEBUG("[pcl::%s::computeTransformation] Convergence reached. Number of " + "iterations: %d out of %d. Transformation difference: %f\n", + getClassName().c_str(), + nr_iterations_, + max_iterations_, + (transformation_ - previous_transformation_).array().abs().sum()); + previous_transformation_ = transformation_; + } + else { + PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n", getClassName().c_str()); } final_transformation_ = previous_transformation_ * guess; diff --git a/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp b/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp new file mode 100644 index 00000000000..85297ab880a --- /dev/null +++ b/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp @@ -0,0 +1,143 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * 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_GICP_CONVERGENCE_CRITERIA_HPP_ +#define PCL_REGISTRATION_GICP_CONVERGENCE_CRITERIA_HPP_ + +#include + +namespace pcl { + +namespace registration { + +template +bool +GICPConvergenceCriteria::hasConverged() +{ + if (convergence_state_ != CONVERGENCE_CRITERIA_NOT_CONVERGED) { + // If it already converged or failed before, reset. + iterations_similar_transforms_ = 0; + convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED; + } + + bool is_similar = false; + + PCL_DEBUG("[pcl::GICPConvergenceCriteria::hasConverged] Iteration %d out of %d.\n", + iterations_, + max_iterations_); + // 1. Number of iterations has reached the maximum user imposed number of iterations + if (iterations_ >= max_iterations_) { + if (!failure_after_max_iter_) { + convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS; + return (true); + } + convergence_state_ = CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS; + } + + // 2. The epsilon (difference) between the previous transformation and the current + // estimated transformation + /* compute the delta from this iteration */ + double delta = 0.; + for (int k = 0; k < 4; k++) { + for (int l = 0; l < 4; l++) { + double ratio = 1; + if (k < 3 && l < 3) // rotation part of the transform + ratio = 1. / rotation_epsilon_; + else + ratio = 1. / translation_threshold_; + double c_delta = + ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l)); + if (c_delta > delta) + delta = c_delta; + } + } + if(delta < 1) { + if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) { + convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM; + return (true); + } + is_similar = true; + } + + correspondences_cur_mse_ = calculateMSE(correspondences_); + PCL_DEBUG("[pcl::GICPConvergenceCriteria::hasConverged] Previous / Current MSE " + "for correspondences distances is: %f / %f.\n", + correspondences_prev_mse_, + correspondences_cur_mse_); + + // 3. The relative sum of Euclidean squared errors is smaller than a user defined + // threshold Absolute + if (std::abs(correspondences_cur_mse_ - correspondences_prev_mse_) < + mse_threshold_absolute_) { + if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) { + convergence_state_ = CONVERGENCE_CRITERIA_ABS_MSE; + return (true); + } + is_similar = true; + } + + // Relative + if (std::abs(correspondences_cur_mse_ - correspondences_prev_mse_) / + correspondences_prev_mse_ < + mse_threshold_relative_) { + if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) { + convergence_state_ = CONVERGENCE_CRITERIA_REL_MSE; + return (true); + } + is_similar = true; + } + + if (is_similar) { + // Increment the number of transforms that the thresholds are allowed to be similar + ++iterations_similar_transforms_; + } + else { + // When the transform becomes large, reset. + iterations_similar_transforms_ = 0; + } + + correspondences_prev_mse_ = correspondences_cur_mse_; + + return (false); +} + +} // namespace registration +} // namespace pcl + +#endif // PCL_REGISTRATION_GICP_CONVERGENCE_CRITERIA_HPP_ \ No newline at end of file From 73c414edb8e789c6b32a856bd76ec321dcd3bd06 Mon Sep 17 00:00:00 2001 From: keineahnung2345 Date: Tue, 7 Jun 2022 12:08:31 +0800 Subject: [PATCH 2/4] [style] formatting --- registration/include/pcl/registration/gicp.h | 28 ++++++++--------- .../registration/gicp_convergence_criteria.h | 30 ++++++++++--------- .../include/pcl/registration/impl/gicp.hpp | 8 ++--- .../impl/gicp_convergence_criteria.hpp | 2 +- 4 files changed, 35 insertions(+), 33 deletions(-) diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index f803d8930b9..a39d0ff6c11 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -41,8 +41,8 @@ #pragma once #include -#include #include +#include namespace pcl { /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the @@ -80,8 +80,10 @@ class GeneralizedIterativeClosestPoint using IterativeClosestPoint::correspondences_; using IterativeClosestPoint::correspondence_estimation_; using IterativeClosestPoint::correspondence_rejectors_; - using IterativeClosestPoint::setUseReciprocalCorrespondences; - using IterativeClosestPoint::getUseReciprocalCorrespondences; + using IterativeClosestPoint::setUseReciprocalCorrespondences; + using IterativeClosestPoint::getUseReciprocalCorrespondences; using IterativeClosestPoint::determineRequiredBlobData; using IterativeClosestPoint::use_reciprocal_correspondence_; using IterativeClosestPoint::source_has_normals_; @@ -114,8 +116,7 @@ class GeneralizedIterativeClosestPoint using Vector6d = Eigen::Matrix; - typename pcl::registration::GICPConvergenceCriteria::Ptr - convergence_criteria_; + typename pcl::registration::GICPConvergenceCriteria::Ptr convergence_criteria_; /** \brief Empty constructor. */ GeneralizedIterativeClosestPoint() @@ -140,9 +141,8 @@ class GeneralizedIterativeClosestPoint estimateRigidTransformationBFGS( cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); }; - convergence_criteria_.reset( - new pcl::registration::GICPConvergenceCriteria( - nr_iterations_, transformation_, previous_transformation_, *correspondences_)); + convergence_criteria_.reset(new pcl::registration::GICPConvergenceCriteria( + nr_iterations_, transformation_, previous_transformation_, *correspondences_)); } /** @@ -161,13 +161,13 @@ class GeneralizedIterativeClosestPoint operator=(GeneralizedIterativeClosestPoint&&) = delete; /** \brief Returns a pointer to the GICPConvergenceCriteria used by the - * GeneralizedIterativeClosestPoint class. This allows to check the convergence state after the - * align() method as well as to configure GICPConvergenceCriteria's parameters not - * available through the GICP API before the align() method is called. Please note that - * the align method sets max_iterations_, transformation_epsilon_ and + * GeneralizedIterativeClosestPoint class. This allows to check the convergence state + * after the align() method as well as to configure GICPConvergenceCriteria's + * parameters not available through the GICP API before the align() method is called. + * Please note that the align method sets max_iterations_, transformation_epsilon_ and * rotation_epsilon_ and therefore overrides the default / set values of the - * GICPConvergenceCriteria instance. \return Pointer to the GeneralizedIterativeClosestPoint's - * GICPConvergenceCriteria. + * GICPConvergenceCriteria instance. \return Pointer to the + * GeneralizedIterativeClosestPoint's GICPConvergenceCriteria. */ inline typename pcl::registration::GICPConvergenceCriteria::Ptr getConvergeCriteria() diff --git a/registration/include/pcl/registration/gicp_convergence_criteria.h b/registration/include/pcl/registration/gicp_convergence_criteria.h index 2b046a7e8bc..d8586ec51e5 100644 --- a/registration/include/pcl/registration/gicp_convergence_criteria.h +++ b/registration/include/pcl/registration/gicp_convergence_criteria.h @@ -62,10 +62,10 @@ namespace registration { * \ingroup registration */ template -class GICPConvergenceCriteria : public ConvergenceCriteria { +class GICPConvergenceCriteria : public ConvergenceCriteria { public: - using Ptr = shared_ptr>; - using ConstPtr = shared_ptr>; + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; using Matrix4 = Eigen::Matrix; @@ -91,20 +91,20 @@ class GICPConvergenceCriteria : public ConvergenceCriteria { * transformation evaluation \param[in] correspondences a reference to the current set * of point correspondences between source and target */ - GICPConvergenceCriteria (const int& iterations, - const Matrix4& transform, - const Matrix4 &previous_transform, - const pcl::Correspondences& correspondences) + GICPConvergenceCriteria(const int& iterations, + const Matrix4& transform, + const Matrix4& previous_transform, + const pcl::Correspondences& correspondences) : iterations_(iterations) , transformation_(transform) - , previous_transformation_ (previous_transform) + , previous_transformation_(previous_transform) , correspondences_(correspondences) , correspondences_prev_mse_(std::numeric_limits::max()) , correspondences_cur_mse_(std::numeric_limits::max()) , max_iterations_(100) // 100 iterations , failure_after_max_iter_(false) - , rotation_threshold_(0.99999) // 0.256 degrees - , rotation_epsilon_ (2e-3) + , rotation_threshold_(0.99999) // 0.256 degrees + , rotation_epsilon_(2e-3) , translation_threshold_(3e-4 * 3e-4) // 0.0003 meters , mse_threshold_relative_(0.00001) // 0.001% of the previous MSE (relative error) , mse_threshold_absolute_(1e-12) // MSE (absolute error) @@ -114,7 +114,7 @@ class GICPConvergenceCriteria : public ConvergenceCriteria { {} /** \brief Empty destructor */ - ~GICPConvergenceCriteria () {} + ~GICPConvergenceCriteria() {} /** \brief Set the maximum number of consecutive iterations that the internal * rotation, translation, and MSE differences are allowed to be similar. \param[in] @@ -208,7 +208,7 @@ class GICPConvergenceCriteria : public ConvergenceCriteria { * consecutive rotations) as set by the user. */ inline double - getRotationEpsilon () const + getRotationEpsilon() const { return (rotation_epsilon_); } @@ -309,7 +309,8 @@ class GICPConvergenceCriteria : public ConvergenceCriteria { * method. */ const Matrix4& transformation_; - /** \brief The previous transformation obtained by the transformation estimation method. */ + /** \brief The previous transformation obtained by the transformation estimation + * method. */ const Matrix4& previous_transformation_; /** \brief The current set of point correspondences between the source and the target. @@ -334,7 +335,8 @@ class GICPConvergenceCriteria : public ConvergenceCriteria { * angle cosine). */ double rotation_threshold_; - /** \brief The rotation epsilon is the maximum allowable difference between two consecutive rotations. */ + /** \brief The rotation epsilon is the maximum allowable difference between two + * consecutive rotations. */ double rotation_epsilon_; /** \brief The translation threshold is the relative translation between two diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index b3a66dca9fd..b3a6ddb7048 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -451,7 +451,7 @@ GeneralizedIterativeClosestPoint::computeTransformatio } convergence_criteria_->setMaximumIterations(max_iterations_); - //convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_); + // convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_); convergence_criteria_->setTranslationThreshold(transformation_epsilon_); convergence_criteria_->setRotationThreshold(rotation_epsilon_); @@ -507,7 +507,7 @@ GeneralizedIterativeClosestPoint::computeTransformatio std::size_t cnt = correspondences_->size(); pcl::Indices source_indices(cnt); pcl::Indices target_indices(cnt); - for(size_t i = 0; i < cnt; ++i) { + for (size_t i = 0; i < cnt; ++i) { source_indices[i] = (*correspondences_)[i].index_query; target_indices[i] = (*correspondences_)[i].index_match; } @@ -559,7 +559,7 @@ GeneralizedIterativeClosestPoint::computeTransformatio } while (convergence_criteria_->getConvergenceState() == pcl::registration::GICPConvergenceCriteria< float>::CONVERGENCE_CRITERIA_NOT_CONVERGED); - if(converged_) { + if (converged_) { PCL_DEBUG("[pcl::%s::computeTransformation] Convergence reached. Number of " "iterations: %d out of %d. Transformation difference: %f\n", getClassName().c_str(), @@ -570,7 +570,7 @@ GeneralizedIterativeClosestPoint::computeTransformatio } else { PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n", - getClassName().c_str()); + getClassName().c_str()); } final_transformation_ = previous_transformation_ * guess; diff --git a/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp b/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp index 85297ab880a..93035bb76c7 100644 --- a/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp +++ b/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp @@ -87,7 +87,7 @@ GICPConvergenceCriteria::hasConverged() delta = c_delta; } } - if(delta < 1) { + if (delta < 1) { if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) { convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM; return (true); From 425d383b4b45a6d63818132d11761aedacc120b5 Mon Sep 17 00:00:00 2001 From: keineahnung2345 Date: Tue, 7 Jun 2022 12:47:11 +0800 Subject: [PATCH 3/4] [fix] remove unused "delta" --- registration/include/pcl/registration/impl/gicp.hpp | 2 -- .../pcl/registration/impl/gicp_convergence_criteria.hpp | 3 ++- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index b3a6ddb7048..a302781b208 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -407,8 +407,6 @@ GeneralizedIterativeClosestPoint::computeTransformatio pcl::IterativeClosestPoint::initComputeReciprocal(); // Point cloud containing the correspondences of each point in PointCloudSourcePtr input_transformed(new PointCloudSource); - // Difference between consecutive transforms - double delta = 0; // Get the size of the source point cloud const std::size_t N = indices_->size(); // Set the mahalanobis matrices to identity diff --git a/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp b/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp index 93035bb76c7..1cafd551bfb 100644 --- a/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp +++ b/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp @@ -73,6 +73,7 @@ GICPConvergenceCriteria::hasConverged() // 2. The epsilon (difference) between the previous transformation and the current // estimated transformation /* compute the delta from this iteration */ + // Difference between consecutive transforms double delta = 0.; for (int k = 0; k < 4; k++) { for (int l = 0; l < 4; l++) { @@ -140,4 +141,4 @@ GICPConvergenceCriteria::hasConverged() } // namespace registration } // namespace pcl -#endif // PCL_REGISTRATION_GICP_CONVERGENCE_CRITERIA_HPP_ \ No newline at end of file +#endif // PCL_REGISTRATION_GICP_CONVERGENCE_CRITERIA_HPP_ From 29b85dbab5057d267b3fcc218a21f32427cf64f4 Mon Sep 17 00:00:00 2001 From: keineahnung2345 Date: Tue, 7 Jun 2022 13:39:12 +0800 Subject: [PATCH 4/4] [fix] getConvergeCriteria return type --- registration/include/pcl/registration/gicp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index a39d0ff6c11..b94bed7a8b6 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -169,7 +169,7 @@ class GeneralizedIterativeClosestPoint * GICPConvergenceCriteria instance. \return Pointer to the * GeneralizedIterativeClosestPoint's GICPConvergenceCriteria. */ - inline typename pcl::registration::GICPConvergenceCriteria::Ptr + inline typename pcl::registration::GICPConvergenceCriteria::Ptr getConvergeCriteria() { return convergence_criteria_;