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