Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 00040 #ifndef PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_H_ 00041 #define PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_H_ 00042 00043 #include <pcl/registration/eigen.h> 00044 #include <pcl/correspondence.h> 00045 #include <pcl/registration/convergence_criteria.h> 00046 00047 namespace pcl 00048 { 00049 namespace registration 00050 { 00051 /** \brief @b DefaultConvergenceCriteria represents an instantiation of 00052 * ConvergenceCriteria, and implements the following criteria for registration loop 00053 * evaluation: 00054 * 00055 * * a maximum number of iterations has been reached 00056 * * the transformation (R, t) cannot be further updated (the difference between current and previous is smaller than a threshold) 00057 * * 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) 00058 * 00059 * \note Convergence is considered reached if ANY of the above criteria are met. 00060 * 00061 * \author Radu B. Rusu 00062 * \ingroup registration 00063 */ 00064 template <typename Scalar = float> 00065 class DefaultConvergenceCriteria : public ConvergenceCriteria 00066 { 00067 public: 00068 typedef boost::shared_ptr<DefaultConvergenceCriteria<Scalar> > Ptr; 00069 typedef boost::shared_ptr<const DefaultConvergenceCriteria<Scalar> > ConstPtr; 00070 00071 typedef Eigen::Matrix<Scalar, 4, 4> Matrix4; 00072 00073 enum ConvergenceState 00074 { 00075 CONVERGENCE_CRITERIA_NOT_CONVERGED, 00076 CONVERGENCE_CRITERIA_ITERATIONS, 00077 CONVERGENCE_CRITERIA_TRANSFORM, 00078 CONVERGENCE_CRITERIA_ABS_MSE, 00079 CONVERGENCE_CRITERIA_REL_MSE, 00080 CONVERGENCE_CRITERIA_NO_CORRESPONDENCES 00081 }; 00082 00083 /** \brief Empty constructor. 00084 * Sets: 00085 * * the maximum number of iterations to 1000 00086 * * the rotation threshold to 0.256 degrees (0.99999) 00087 * * the translation threshold to 0.0003 meters (3e-4^2) 00088 * * the MSE relative / absolute thresholds to 0.001% and 1e-12 00089 * 00090 * \param[in] iterations a reference to the number of iterations the loop has ran so far 00091 * \param[in] transform a reference to the current transformation obtained by the transformation evaluation 00092 * \param[in] correspondences a reference to the current set of point correspondences between source and target 00093 */ 00094 DefaultConvergenceCriteria (const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences) 00095 : iterations_ (iterations) 00096 , transformation_ (transform) 00097 , correspondences_ (correspondences) 00098 , correspondences_prev_mse_ (std::numeric_limits<double>::max ()) 00099 , correspondences_cur_mse_ (std::numeric_limits<double>::max ()) 00100 , max_iterations_ (100) // 100 iterations 00101 , failure_after_max_iter_ (false) 00102 , rotation_threshold_ (0.99999) // 0.256 degrees 00103 , translation_threshold_ (3e-4 * 3e-4) // 0.0003 meters 00104 , mse_threshold_relative_ (0.00001) // 0.001% of the previous MSE (relative error) 00105 , mse_threshold_absolute_ (1e-12) // MSE (absolute error) 00106 , iterations_similar_transforms_ (0) 00107 , max_iterations_similar_transforms_ (0) 00108 , convergence_state_ (CONVERGENCE_CRITERIA_NOT_CONVERGED) 00109 { 00110 } 00111 00112 /** \brief Empty destructor */ 00113 virtual ~DefaultConvergenceCriteria () {} 00114 00115 /** \brief Set the maximum number of iterations that the internal rotation, 00116 * translation, and MSE differences are allowed to be similar. 00117 * \param[in] nr_iterations the maximum number of iterations 00118 */ 00119 inline void 00120 setMaximumIterationsSimilarTransforms (const int nr_iterations) { max_iterations_similar_transforms_ = nr_iterations; } 00121 00122 /** \brief Get the maximum number of iterations that the internal rotation, 00123 * translation, and MSE differences are allowed to be similar, as set by the user. 00124 */ 00125 inline int 00126 getMaximumIterationsSimilarTransforms () const { return (max_iterations_similar_transforms_); } 00127 00128 /** \brief Set the maximum number of iterations the internal optimization should run for. 00129 * \param[in] nr_iterations the maximum number of iterations the internal optimization should run for 00130 */ 00131 inline void 00132 setMaximumIterations (const int nr_iterations) { max_iterations_ = nr_iterations; } 00133 00134 /** \brief Get the maximum number of iterations the internal optimization should run for, as set by the user. */ 00135 inline int 00136 getMaximumIterations () const { return (max_iterations_); } 00137 00138 /** \brief Specifies if the registration fails or converges when the maximum number of iterations is reached. 00139 * \param[in] failure_after_max_iter If true, the registration fails. If false, the registration is assumed to have converged. 00140 */ 00141 inline void 00142 setFailureAfterMaximumIterations (const bool failure_after_max_iter) { failure_after_max_iter_ = failure_after_max_iter; } 00143 00144 /** \brief Get whether the registration will fail or converge when the maximum number of iterations is reached. */ 00145 inline bool 00146 getFailureAfterMaximumIterations () const { return (failure_after_max_iter_); } 00147 00148 /** \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. 00149 * \param[in] threshold the rotation threshold in order for an optimization to be considered as having converged to the final solution. 00150 */ 00151 inline void 00152 setRotationThreshold (const double threshold) { rotation_threshold_ = threshold; } 00153 00154 /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user. 00155 */ 00156 inline double 00157 getRotationThreshold () const { return (rotation_threshold_); } 00158 00159 /** \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. 00160 * \param[in] threshold the translation threshold in order for an optimization to be considered as having converged to the final solution. 00161 */ 00162 inline void 00163 setTranslationThreshold (const double threshold) { translation_threshold_ = threshold; } 00164 00165 /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user. 00166 */ 00167 inline double 00168 getTranslationThreshold () const { return (translation_threshold_); } 00169 00170 /** \brief Set the relative MSE between two consecutive sets of correspondences. 00171 * \param[in] mse_relative the relative MSE threshold 00172 */ 00173 inline void 00174 setRelativeMSE (const double mse_relative) { mse_threshold_relative_ = mse_relative; } 00175 00176 /** \brief Get the relative MSE between two consecutive sets of correspondences. */ 00177 inline double 00178 getRelativeMSE () const { return (mse_threshold_relative_); } 00179 00180 /** \brief Set the absolute MSE between two consecutive sets of correspondences. 00181 * \param[in] mse_absolute the relative MSE threshold 00182 */ 00183 inline void 00184 setAbsoluteMSE (const double mse_absolute) { mse_threshold_absolute_ = mse_absolute; } 00185 00186 /** \brief Get the absolute MSE between two consecutive sets of correspondences. */ 00187 inline double 00188 getAbsoluteMSE () const { return (mse_threshold_absolute_); } 00189 00190 00191 /** \brief Check if convergence has been reached. */ 00192 virtual bool 00193 hasConverged (); 00194 00195 /** \brief Return the convergence state after hasConverged () */ 00196 ConvergenceState 00197 getConvergenceState () 00198 { 00199 return (convergence_state_); 00200 } 00201 00202 /** \brief Sets the convergence state externally (for example, when ICP does not find 00203 * enough correspondences to estimate a transformation, the function is called setting 00204 * the convergence state to ConvergenceState::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES) 00205 * \param[in] c the convergence state 00206 */ 00207 inline void 00208 setConvergenceState(ConvergenceState c) 00209 { 00210 convergence_state_ = c; 00211 } 00212 00213 protected: 00214 00215 /** \brief Calculate the mean squared error (MSE) of the distance for a given set of correspondences. 00216 * \param[in] correspondences the given set of correspondences 00217 */ 00218 inline double 00219 calculateMSE (const pcl::Correspondences &correspondences) const 00220 { 00221 double mse = 0; 00222 for (size_t i = 0; i < correspondences.size (); ++i) 00223 mse += correspondences[i].distance; 00224 mse /= double (correspondences.size ()); 00225 return (mse); 00226 } 00227 00228 /** \brief The number of iterations done by the registration loop so far. */ 00229 const int &iterations_; 00230 00231 /** \brief The current transformation obtained by the transformation estimation method. */ 00232 const Matrix4 &transformation_; 00233 00234 /** \brief The current set of point correspondences between the source and the target. */ 00235 const pcl::Correspondences &correspondences_; 00236 00237 /** \brief The MSE for the previous set of correspondences. */ 00238 double correspondences_prev_mse_; 00239 00240 /** \brief The MSE for the current set of correspondences. */ 00241 double correspondences_cur_mse_; 00242 00243 /** \brief The maximum nuyyGmber of iterations that the registration loop is to be executed. */ 00244 int max_iterations_; 00245 00246 /** \brief Specifys if the registration fails or converges when the maximum number of iterations is reached. */ 00247 bool failure_after_max_iter_; 00248 00249 /** \brief The rotation threshold is the relative rotation between two iterations (as angle cosine). */ 00250 double rotation_threshold_; 00251 00252 /** \brief The translation threshold is the relative translation between two iterations (0 if no translation). */ 00253 double translation_threshold_; 00254 00255 /** \brief The relative change from the previous MSE for the current set of correspondences, e.g. .1 means 10% change. */ 00256 double mse_threshold_relative_; 00257 00258 /** \brief The absolute change from the previous MSE for the current set of correspondences. */ 00259 double mse_threshold_absolute_; 00260 00261 /** \brief Internal counter for the number of iterations that the internal 00262 * rotation, translation, and MSE differences are allowed to be similar. */ 00263 int iterations_similar_transforms_; 00264 00265 /** \brief The maximum number of iterations that the internal rotation, 00266 * translation, and MSE differences are allowed to be similar. */ 00267 int max_iterations_similar_transforms_; 00268 00269 /** \brief The state of the convergence (e.g., why did the registration converge). */ 00270 ConvergenceState convergence_state_; 00271 00272 public: 00273 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00274 }; 00275 } 00276 } 00277 00278 #include <pcl/registration/impl/default_convergence_criteria.hpp> 00279 00280 #endif // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_H_ 00281