Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/default_convergence_criteria.h
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