1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2012-, Open Perception, Inc. 6 * 7 * All rights reserved. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 13 * * Redistributions of source code must retain the above copyright 14 * notice, this list of conditions and the following disclaimer. 15 * * Redistributions in binary form must reproduce the above 16 * copyright notice, this list of conditions and the following 17 * disclaimer in the documentation and/or other materials provided 18 * with the distribution. 19 * * Neither the name of the copyright holder(s) nor the names of its 20 * contributors may be used to endorse or promote products derived 21 * from this software without specific prior written permission. 22 * 23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 * POSSIBILITY OF SUCH DAMAGE. 35 * 36 * $Id$ 37 * 38 */ 39 40 #pragma once 41 42 #include <pcl/registration/convergence_criteria.h> 43 #include <pcl/correspondence.h> 44 #include <pcl/memory.h> 45 #include <pcl/pcl_macros.h> 46 47 namespace pcl { 48 namespace registration { 49 /** \brief @b DefaultConvergenceCriteria represents an instantiation of 50 * ConvergenceCriteria, and implements the following criteria for registration loop 51 * evaluation: 52 * 53 * * a maximum number of iterations has been reached 54 * * the transformation (R, t) cannot be further updated (the difference between 55 * current and previous is smaller than a threshold) 56 * * the Mean Squared Error (MSE) between the current set of correspondences and the 57 * previous one is smaller than some threshold (both relative and absolute tests) 58 * 59 * \note Convergence is considered reached if ANY of the above criteria are met. 60 * 61 * \author Radu B. Rusu 62 * \ingroup registration 63 */ 64 template <typename Scalar = float> 65 class DefaultConvergenceCriteria : public ConvergenceCriteria { 66 public: 67 using Ptr = shared_ptr<DefaultConvergenceCriteria<Scalar>>; 68 using ConstPtr = shared_ptr<const DefaultConvergenceCriteria<Scalar>>; 69 70 using Matrix4 = Eigen::Matrix<Scalar, 4, 4>; 71 72 enum ConvergenceState { 73 CONVERGENCE_CRITERIA_NOT_CONVERGED, 74 CONVERGENCE_CRITERIA_ITERATIONS, 75 CONVERGENCE_CRITERIA_TRANSFORM, 76 CONVERGENCE_CRITERIA_ABS_MSE, 77 CONVERGENCE_CRITERIA_REL_MSE, 78 CONVERGENCE_CRITERIA_NO_CORRESPONDENCES, 79 CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS 80 }; 81 82 /** \brief Empty constructor. 83 * Sets: 84 * * the maximum number of iterations to 1000 85 * * the rotation threshold to 0.256 degrees (0.99999) 86 * * the translation threshold to 0.0003 meters (3e-4^2) 87 * * the MSE relative / absolute thresholds to 0.001% and 1e-12 88 * 89 * \param[in] iterations a reference to the number of iterations the loop has ran so 90 * far \param[in] transform a reference to the current transformation obtained by the 91 * transformation evaluation \param[in] correspondences a reference to the current set 92 * of point correspondences between source and target 93 */ DefaultConvergenceCriteria(const int & iterations,const Matrix4 & transform,const pcl::Correspondences & correspondences)94 DefaultConvergenceCriteria(const int& iterations, 95 const Matrix4& transform, 96 const pcl::Correspondences& correspondences) 97 : iterations_(iterations) 98 , transformation_(transform) 99 , correspondences_(correspondences) 100 , correspondences_prev_mse_(std::numeric_limits<double>::max()) 101 , correspondences_cur_mse_(std::numeric_limits<double>::max()) 102 , max_iterations_(100) // 100 iterations 103 , failure_after_max_iter_(false) 104 , rotation_threshold_(0.99999) // 0.256 degrees 105 , translation_threshold_(3e-4 * 3e-4) // 0.0003 meters 106 , mse_threshold_relative_(0.00001) // 0.001% of the previous MSE (relative error) 107 , mse_threshold_absolute_(1e-12) // MSE (absolute error) 108 , iterations_similar_transforms_(0) 109 , max_iterations_similar_transforms_(0) 110 , convergence_state_(CONVERGENCE_CRITERIA_NOT_CONVERGED) 111 {} 112 113 /** \brief Empty destructor */ ~DefaultConvergenceCriteria()114 ~DefaultConvergenceCriteria() {} 115 116 /** \brief Set the maximum number of consecutive iterations that the internal 117 * rotation, translation, and MSE differences are allowed to be similar. \param[in] 118 * nr_iterations the maximum number of iterations 119 */ 120 inline void setMaximumIterationsSimilarTransforms(const int nr_iterations)121 setMaximumIterationsSimilarTransforms(const int nr_iterations) 122 { 123 max_iterations_similar_transforms_ = nr_iterations; 124 } 125 126 /** \brief Get the maximum number of consecutive iterations that the internal 127 * rotation, translation, and MSE differences are allowed to be similar, as set by the 128 * user. 129 */ 130 inline int getMaximumIterationsSimilarTransforms()131 getMaximumIterationsSimilarTransforms() const 132 { 133 return (max_iterations_similar_transforms_); 134 } 135 136 /** \brief Set the maximum number of iterations the internal optimization should run 137 * for. \param[in] nr_iterations the maximum number of iterations the internal 138 * optimization should run for 139 */ 140 inline void setMaximumIterations(const int nr_iterations)141 setMaximumIterations(const int nr_iterations) 142 { 143 max_iterations_ = nr_iterations; 144 } 145 146 /** \brief Get the maximum number of iterations the internal optimization should run 147 * for, as set by the user. */ 148 inline int getMaximumIterations()149 getMaximumIterations() const 150 { 151 return (max_iterations_); 152 } 153 154 /** \brief Specifies if the registration fails or converges when the maximum number of 155 * iterations is reached. \param[in] failure_after_max_iter If true, the registration 156 * fails. If false, the registration is assumed to have converged. 157 */ 158 inline void setFailureAfterMaximumIterations(const bool failure_after_max_iter)159 setFailureAfterMaximumIterations(const bool failure_after_max_iter) 160 { 161 failure_after_max_iter_ = failure_after_max_iter; 162 } 163 164 /** \brief Get whether the registration will fail or converge when the maximum number 165 * of iterations is reached. */ 166 inline bool getFailureAfterMaximumIterations()167 getFailureAfterMaximumIterations() const 168 { 169 return (failure_after_max_iter_); 170 } 171 172 /** \brief Set the rotation threshold cosine angle (maximum allowable difference 173 * between two consecutive transformations) in order for an optimization to be 174 * considered as having converged to the final solution. \param[in] threshold the 175 * rotation threshold in order for an optimization to be considered as having 176 * converged to the final solution. 177 */ 178 inline void setRotationThreshold(const double threshold)179 setRotationThreshold(const double threshold) 180 { 181 rotation_threshold_ = threshold; 182 } 183 184 /** \brief Get the rotation threshold cosine angle (maximum allowable difference 185 * between two consecutive transformations) as set by the user. 186 */ 187 inline double getRotationThreshold()188 getRotationThreshold() const 189 { 190 return (rotation_threshold_); 191 } 192 193 /** \brief Set the translation threshold (maximum allowable difference between two 194 * consecutive transformations) in order for an optimization to be considered as 195 * having converged to the final solution. \param[in] threshold the translation 196 * threshold in order for an optimization to be considered as having converged to the 197 * final solution. 198 */ 199 inline void setTranslationThreshold(const double threshold)200 setTranslationThreshold(const double threshold) 201 { 202 translation_threshold_ = threshold; 203 } 204 205 /** \brief Get the rotation threshold cosine angle (maximum allowable difference 206 * between two consecutive transformations) as set by the user. 207 */ 208 inline double getTranslationThreshold()209 getTranslationThreshold() const 210 { 211 return (translation_threshold_); 212 } 213 214 /** \brief Set the relative MSE between two consecutive sets of correspondences. 215 * \param[in] mse_relative the relative MSE threshold 216 */ 217 inline void setRelativeMSE(const double mse_relative)218 setRelativeMSE(const double mse_relative) 219 { 220 mse_threshold_relative_ = mse_relative; 221 } 222 223 /** \brief Get the relative MSE between two consecutive sets of correspondences. */ 224 inline double getRelativeMSE()225 getRelativeMSE() const 226 { 227 return (mse_threshold_relative_); 228 } 229 230 /** \brief Set the absolute MSE between two consecutive sets of correspondences. 231 * \param[in] mse_absolute the relative MSE threshold 232 */ 233 inline void setAbsoluteMSE(const double mse_absolute)234 setAbsoluteMSE(const double mse_absolute) 235 { 236 mse_threshold_absolute_ = mse_absolute; 237 } 238 239 /** \brief Get the absolute MSE between two consecutive sets of correspondences. */ 240 inline double getAbsoluteMSE()241 getAbsoluteMSE() const 242 { 243 return (mse_threshold_absolute_); 244 } 245 246 /** \brief Check if convergence has been reached. */ 247 bool 248 hasConverged() override; 249 250 /** \brief Return the convergence state after hasConverged () */ 251 ConvergenceState getConvergenceState()252 getConvergenceState() 253 { 254 return (convergence_state_); 255 } 256 257 /** \brief Sets the convergence state externally (for example, when ICP does not find 258 * enough correspondences to estimate a transformation, the function is called setting 259 * the convergence state to ConvergenceState::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES) 260 * \param[in] c the convergence state 261 */ 262 inline void setConvergenceState(ConvergenceState c)263 setConvergenceState(ConvergenceState c) 264 { 265 convergence_state_ = c; 266 } 267 268 protected: 269 /** \brief Calculate the mean squared error (MSE) of the distance for a given set of 270 * correspondences. \param[in] correspondences the given set of correspondences 271 */ 272 inline double calculateMSE(const pcl::Correspondences & correspondences)273 calculateMSE(const pcl::Correspondences& correspondences) const 274 { 275 double mse = 0; 276 for (const auto& correspondence : correspondences) 277 mse += correspondence.distance; 278 mse /= double(correspondences.size()); 279 return (mse); 280 } 281 282 /** \brief The number of iterations done by the registration loop so far. */ 283 const int& iterations_; 284 285 /** \brief The current transformation obtained by the transformation estimation 286 * method. */ 287 const Matrix4& transformation_; 288 289 /** \brief The current set of point correspondences between the source and the target. 290 */ 291 const pcl::Correspondences& correspondences_; 292 293 /** \brief The MSE for the previous set of correspondences. */ 294 double correspondences_prev_mse_; 295 296 /** \brief The MSE for the current set of correspondences. */ 297 double correspondences_cur_mse_; 298 299 /** \brief The maximum nuyyGmber of iterations that the registration loop is to be 300 * executed. */ 301 int max_iterations_; 302 303 /** \brief Specifys if the registration fails or converges when the maximum number of 304 * iterations is reached. */ 305 bool failure_after_max_iter_; 306 307 /** \brief The rotation threshold is the relative rotation between two iterations (as 308 * angle cosine). */ 309 double rotation_threshold_; 310 311 /** \brief The translation threshold is the relative translation between two 312 * iterations (0 if no translation). */ 313 double translation_threshold_; 314 315 /** \brief The relative change from the previous MSE for the current set of 316 * correspondences, e.g. .1 means 10% change. */ 317 double mse_threshold_relative_; 318 319 /** \brief The absolute change from the previous MSE for the current set of 320 * correspondences. */ 321 double mse_threshold_absolute_; 322 323 /** \brief Internal counter for the number of iterations that the internal 324 * rotation, translation, and MSE differences are allowed to be similar. */ 325 int iterations_similar_transforms_; 326 327 /** \brief The maximum number of iterations that the internal rotation, 328 * translation, and MSE differences are allowed to be similar. */ 329 int max_iterations_similar_transforms_; 330 331 /** \brief The state of the convergence (e.g., why did the registration converge). */ 332 ConvergenceState convergence_state_; 333 334 public: 335 PCL_MAKE_ALIGNED_OPERATOR_NEW 336 }; 337 } // namespace registration 338 } // namespace pcl 339 340 #include <pcl/registration/impl/default_convergence_criteria.hpp> 341