Point Cloud Library (PCL)  1.7.0
default_convergence_criteria.h
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 #ifndef PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_H_
41 #define PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_H_
42 
43 #include <pcl/registration/eigen.h>
44 #include <pcl/correspondence.h>
45 #include <pcl/registration/convergence_criteria.h>
46 
47 namespace pcl
48 {
49  namespace registration
50  {
51  /** \brief @b DefaultConvergenceCriteria represents an instantiation of
52  * ConvergenceCriteria, and implements the following criteria for registration loop
53  * evaluation:
54  *
55  * * a maximum number of iterations has been reached
56  * * the transformation (R, t) cannot be further updated (the difference between current and previous is smaller than a threshold)
57  * * 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)
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>
66  {
67  public:
68  typedef boost::shared_ptr<DefaultConvergenceCriteria<Scalar> > Ptr;
69  typedef boost::shared_ptr<const DefaultConvergenceCriteria<Scalar> > ConstPtr;
70 
71  typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
72 
74  {
81  };
82 
83  /** \brief Empty constructor.
84  * Sets:
85  * * the maximum number of iterations to 1000
86  * * the rotation threshold to 0.256 degrees (0.99999)
87  * * the translation threshold to 0.0003 meters (3e-4^2)
88  * * the MSE relative / absolute thresholds to 0.001% and 1e-12
89  *
90  * \param[in] iterations a reference to the number of iterations the loop has ran so far
91  * \param[in] transform a reference to the current transformation obtained by the transformation evaluation
92  * \param[in] correspondences a reference to the current set of point correspondences between source and target
93  */
94  DefaultConvergenceCriteria (const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences)
95  : iterations_ (iterations)
96  , transformation_ (transform)
97  , correspondences_ (correspondences)
98  , correspondences_prev_mse_ (std::numeric_limits<double>::max ())
99  , correspondences_cur_mse_ (std::numeric_limits<double>::max ())
100  , max_iterations_ (100) // 100 iterations
101  , failure_after_max_iter_ (false)
102  , rotation_threshold_ (0.99999) // 0.256 degrees
103  , translation_threshold_ (3e-4 * 3e-4) // 0.0003 meters
104  , mse_threshold_relative_ (0.00001) // 0.001% of the previous MSE (relative error)
105  , mse_threshold_absolute_ (1e-12) // MSE (absolute error)
109  {
110  }
111 
112  /** \brief Empty destructor */
114 
115  /** \brief Set the maximum number of iterations that the internal rotation,
116  * translation, and MSE differences are allowed to be similar.
117  * \param[in] nr_iterations the maximum number of iterations
118  */
119  inline void
120  setMaximumIterationsSimilarTransforms (const int nr_iterations) { max_iterations_similar_transforms_ = nr_iterations; }
121 
122  /** \brief Get the maximum number of iterations that the internal rotation,
123  * translation, and MSE differences are allowed to be similar, as set by the user.
124  */
125  inline int
127 
128  /** \brief Set the maximum number of iterations the internal optimization should run for.
129  * \param[in] nr_iterations the maximum number of iterations the internal optimization should run for
130  */
131  inline void
132  setMaximumIterations (const int nr_iterations) { max_iterations_ = nr_iterations; }
133 
134  /** \brief Get the maximum number of iterations the internal optimization should run for, as set by the user. */
135  inline int
136  getMaximumIterations () const { return (max_iterations_); }
137 
138  /** \brief Specifies if the registration fails or converges when the maximum number of iterations is reached.
139  * \param[in] failure_after_max_iter If true, the registration fails. If false, the registration is assumed to have converged.
140  */
141  inline void
142  setFailureAfterMaximumIterations (const bool failure_after_max_iter) { failure_after_max_iter_ = failure_after_max_iter; }
143 
144  /** \brief Get whether the registration will fail or converge when the maximum number of iterations is reached. */
145  inline bool
147 
148  /** \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.
149  * \param[in] threshold the rotation threshold in order for an optimization to be considered as having converged to the final solution.
150  */
151  inline void
152  setRotationThreshold (const double threshold) { rotation_threshold_ = threshold; }
153 
154  /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user.
155  */
156  inline double
158 
159  /** \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.
160  * \param[in] threshold the translation threshold in order for an optimization to be considered as having converged to the final solution.
161  */
162  inline void
163  setTranslationThreshold (const double threshold) { translation_threshold_ = threshold; }
164 
165  /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user.
166  */
167  inline double
169 
170  /** \brief Set the relative MSE between two consecutive sets of correspondences.
171  * \param[in] mse_relative the relative MSE threshold
172  */
173  inline void
174  setRelativeMSE (const double mse_relative) { mse_threshold_relative_ = mse_relative; }
175 
176  /** \brief Get the relative MSE between two consecutive sets of correspondences. */
177  inline double
179 
180  /** \brief Set the absolute MSE between two consecutive sets of correspondences.
181  * \param[in] mse_absolute the relative MSE threshold
182  */
183  inline void
184  setAbsoluteMSE (const double mse_absolute) { mse_threshold_absolute_ = mse_absolute; }
185 
186  /** \brief Get the absolute MSE between two consecutive sets of correspondences. */
187  inline double
189 
190 
191  /** \brief Check if convergence has been reached. */
192  virtual bool
193  hasConverged ();
194 
195  /** \brief Return the convergence state after hasConverged () */
198  {
199  return (convergence_state_);
200  }
201 
202  /** \brief Sets the convergence state externally (for example, when ICP does not find
203  * enough correspondences to estimate a transformation, the function is called setting
204  * the convergence state to ConvergenceState::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES)
205  * \param[in] c the convergence state
206  */
207  inline void
209  {
210  convergence_state_ = c;
211  }
212 
213  protected:
214 
215  /** \brief Calculate the mean squared error (MSE) of the distance for a given set of correspondences.
216  * \param[in] correspondences the given set of correspondences
217  */
218  inline double
219  calculateMSE (const pcl::Correspondences &correspondences) const
220  {
221  double mse = 0;
222  for (size_t i = 0; i < correspondences.size (); ++i)
223  mse += correspondences[i].distance;
224  mse /= double (correspondences.size ());
225  return (mse);
226  }
227 
228  /** \brief The number of iterations done by the registration loop so far. */
229  const int &iterations_;
230 
231  /** \brief The current transformation obtained by the transformation estimation method. */
233 
234  /** \brief The current set of point correspondences between the source and the target. */
236 
237  /** \brief The MSE for the previous set of correspondences. */
239 
240  /** \brief The MSE for the current set of correspondences. */
242 
243  /** \brief The maximum nuyyGmber of iterations that the registration loop is to be executed. */
245 
246  /** \brief Specifys if the registration fails or converges when the maximum number of iterations is reached. */
248 
249  /** \brief The rotation threshold is the relative rotation between two iterations (as angle cosine). */
251 
252  /** \brief The translation threshold is the relative translation between two iterations (0 if no translation). */
254 
255  /** \brief The relative change from the previous MSE for the current set of correspondences, e.g. .1 means 10% change. */
257 
258  /** \brief The absolute change from the previous MSE for the current set of correspondences. */
260 
261  /** \brief Internal counter for the number of iterations that the internal
262  * rotation, translation, and MSE differences are allowed to be similar. */
264 
265  /** \brief The maximum number of iterations that the internal rotation,
266  * translation, and MSE differences are allowed to be similar. */
268 
269  /** \brief The state of the convergence (e.g., why did the registration converge). */
271 
272  public:
273  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
274  };
275  }
276 }
277 
278 #include <pcl/registration/impl/default_convergence_criteria.hpp>
279 
280 #endif // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_H_
281 
double correspondences_cur_mse_
The MSE for the current set of correspondences.
const Matrix4 & transformation_
The current transformation obtained by the transformation estimation method.
ConvergenceCriteria represents an abstract base class for different convergence criteria used in regi...
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
void setFailureAfterMaximumIterations(const bool failure_after_max_iter)
Specifies if the registration fails or converges when the maximum number of iterations is reached...
int getMaximumIterations() const
Get the maximum number of iterations the internal optimization should run for, as set by the user...
int iterations_similar_transforms_
Internal counter for the number of iterations that the internal rotation, translation, and MSE differences are allowed to be similar.
double mse_threshold_absolute_
The absolute change from the previous MSE for the current set of correspondences. ...
bool failure_after_max_iter_
Specifys if the registration fails or converges when the maximum number of iterations is reached...
double rotation_threshold_
The rotation threshold is the relative rotation between two iterations (as angle cosine).
bool getFailureAfterMaximumIterations() const
Get whether the registration will fail or converge when the maximum number of iterations is reached...
double mse_threshold_relative_
The relative change from the previous MSE for the current set of correspondences, e...
void setMaximumIterations(const int nr_iterations)
Set the maximum number of iterations the internal optimization should run for.
double getRotationThreshold() const
Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transfo...
virtual bool hasConverged()
Check if convergence has been reached.
DefaultConvergenceCriteria(const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences)
Empty constructor.
void setRelativeMSE(const double mse_relative)
Set the relative MSE between two consecutive sets of correspondences.
int max_iterations_
The maximum nuyyGmber of iterations that the registration loop is to be executed. ...
const pcl::Correspondences & correspondences_
The current set of point correspondences between the source and the target.
const int & iterations_
The number of iterations done by the registration loop so far.
double correspondences_prev_mse_
The MSE for the previous set of correspondences.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void setConvergenceState(ConvergenceState c)
Sets the convergence state externally (for example, when ICP does not find enough correspondences to ...
ConvergenceState getConvergenceState()
Return the convergence state after hasConverged ()
double getAbsoluteMSE() const
Get the absolute MSE between two consecutive sets of correspondences.
int getMaximumIterationsSimilarTransforms() const
Get the maximum number of iterations that the internal rotation, translation, and MSE differences are...
boost::shared_ptr< const DefaultConvergenceCriteria< Scalar > > ConstPtr
boost::shared_ptr< DefaultConvergenceCriteria< Scalar > > Ptr
double getRelativeMSE() const
Get the relative MSE between two consecutive sets of correspondences.
void setTranslationThreshold(const double threshold)
Set the translation threshold (maximum allowable difference between two consecutive transformations) ...
double calculateMSE(const pcl::Correspondences &correspondences) const
Calculate the mean squared error (MSE) of the distance for a given set of correspondences.
void setRotationThreshold(const double threshold)
Set the rotation threshold cosine angle (maximum allowable difference between two consecutive transfo...
void setAbsoluteMSE(const double mse_absolute)
Set the absolute MSE between two consecutive sets of correspondences.
double translation_threshold_
The translation threshold is the relative translation between two iterations (0 if no translation)...
ConvergenceState convergence_state_
The state of the convergence (e.g., why did the registration converge).
int max_iterations_similar_transforms_
The maximum number of iterations that the internal rotation, translation, and MSE differences are all...
void setMaximumIterationsSimilarTransforms(const int nr_iterations)
Set the maximum number of iterations that the internal rotation, translation, and MSE differences are...
double getTranslationThreshold() const
Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transfo...