diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index d17e8a1048b..b94bed7a8b6 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -41,6 +41,7 @@ #pragma once #include +#include #include namespace pcl { @@ -76,6 +77,19 @@ 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 +116,8 @@ class GeneralizedIterativeClosestPoint using Vector6d = Eigen::Matrix; + typename pcl::registration::GICPConvergenceCriteria::Ptr convergence_criteria_; + /** \brief Empty constructor. */ GeneralizedIterativeClosestPoint() : k_correspondences_(20) @@ -125,6 +141,38 @@ 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..d8586ec51e5 --- /dev/null +++ b/registration/include/pcl/registration/gicp_convergence_criteria.h @@ -0,0 +1,371 @@ +/* + * 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..a302781b208 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -405,8 +405,8 @@ GeneralizedIterativeClosestPoint::computeTransformatio PointCloudSource& output, const Eigen::Matrix4f& guess) { pcl::IterativeClosestPoint::initComputeReciprocal(); - // Difference between consecutive transforms - double delta = 0; + // Point cloud containing the correspondences of each point in + PointCloudSourcePtr input_transformed(new PointCloudSource); // Get the size of the source point cloud const std::size_t N = indices_->size(); // Set the mahalanobis matrices to identity @@ -425,16 +425,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,88 +519,56 @@ 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", - getClassName().c_str()); + 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..1cafd551bfb --- /dev/null +++ b/registration/include/pcl/registration/impl/gicp_convergence_criteria.hpp @@ -0,0 +1,144 @@ +/* + * 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 */ + // Difference between consecutive transforms + 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_