Point Cloud Library (PCL)  1.13.1-dev
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 #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>
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 
80  };
81 
82  /** \brief Empty constructor.
83  * Sets:
84  * * the maximum number of iterations to 100
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  */
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)
111  {}
112 
113  /** \brief Empty destructor */
114  ~DefaultConvergenceCriteria() override = default;
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
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
132  {
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
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
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
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
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
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
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
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
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
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
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
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
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 () */
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
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
273  calculateMSE(const pcl::Correspondences& correspondences) const
274  {
275  double mse = 0;
276  for (const auto& correspondence : correspondences)
277  mse += correspondence.distance;
278  mse /= static_cast<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. */
288 
289  /** \brief The current set of point correspondences between the source and the target.
290  */
292 
293  /** \brief The MSE for the previous set of correspondences. */
295 
296  /** \brief The MSE for the current set of correspondences. */
298 
299  /** \brief The maximum nuyyGmber of iterations that the registration loop is to be
300  * executed. */
302 
303  /** \brief Specifys if the registration fails or converges when the maximum number of
304  * iterations is reached. */
306 
307  /** \brief The rotation threshold is the relative rotation between two iterations (as
308  * angle cosine). */
310 
311  /** \brief The translation threshold is the relative translation between two
312  * iterations (0 if no translation). */
314 
315  /** \brief The relative change from the previous MSE for the current set of
316  * correspondences, e.g. .1 means 10% change. */
318 
319  /** \brief The absolute change from the previous MSE for the current set of
320  * correspondences. */
322 
323  /** \brief Internal counter for the number of iterations that the internal
324  * rotation, translation, and MSE differences are allowed to be similar. */
326 
327  /** \brief The maximum number of iterations that the internal rotation,
328  * translation, and MSE differences are allowed to be similar. */
330 
331  /** \brief The state of the convergence (e.g., why did the registration converge). */
333 
334 public:
336 };
337 } // namespace registration
338 } // namespace pcl
339 
340 #include <pcl/registration/impl/default_convergence_criteria.hpp>
ConvergenceCriteria represents an abstract base class for different convergence criteria used in regi...
shared_ptr< ConvergenceCriteria > Ptr
shared_ptr< const ConvergenceCriteria > ConstPtr
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
ConvergenceState convergence_state_
The state of the convergence (e.g., why did the registration converge).
int iterations_similar_transforms_
Internal counter for the number of iterations that the internal rotation, translation,...
int getMaximumIterations() const
Get the maximum number of iterations the internal optimization should run for, as set by the user.
void setConvergenceState(ConvergenceState c)
Sets the convergence state externally (for example, when ICP does not find enough correspondences to ...
double correspondences_cur_mse_
The MSE for the current set of correspondences.
DefaultConvergenceCriteria(const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences)
Empty constructor.
const Matrix4 & transformation_
The current transformation obtained by the transformation estimation method.
void setAbsoluteMSE(const double mse_absolute)
Set the absolute MSE between two consecutive sets of correspondences.
void setMaximumIterations(const int nr_iterations)
Set the maximum number of iterations the internal optimization should run for.
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.
void setTranslationThreshold(const double threshold)
Set the translation threshold (maximum allowable difference between two consecutive transformations) ...
bool getFailureAfterMaximumIterations() const
Get whether the registration will fail or converge when the maximum number of iterations is reached.
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.
void setMaximumIterationsSimilarTransforms(const int nr_iterations)
Set the maximum number of consecutive iterations that the internal rotation, translation,...
double getAbsoluteMSE() const
Get 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).
~DefaultConvergenceCriteria() override=default
Empty destructor.
void setRotationThreshold(const double threshold)
Set the rotation threshold cosine angle (maximum allowable difference between two consecutive transfo...
double mse_threshold_absolute_
The absolute change from the previous MSE for the current set of correspondences.
double getTranslationThreshold() const
Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transfo...
ConvergenceState getConvergenceState()
Return the convergence state after hasConverged ()
bool hasConverged() override
Check if convergence has been reached.
int getMaximumIterationsSimilarTransforms() const
Get the maximum number of consecutive iterations that the internal rotation, translation,...
double calculateMSE(const pcl::Correspondences &correspondences) const
Calculate the mean squared error (MSE) of the distance for a given set of correspondences.
void setFailureAfterMaximumIterations(const bool failure_after_max_iter)
Specifies if the registration fails or converges when the maximum number of iterations is reached.
double getRelativeMSE() const
Get the relative MSE between two consecutive sets 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).
double mse_threshold_relative_
The relative change from the previous MSE for the current set of correspondences, e....
double correspondences_prev_mse_
The MSE for the previous set of correspondences.
int max_iterations_similar_transforms_
The maximum number of iterations that the internal rotation, translation, and MSE differences are all...
double getRotationThreshold() const
Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transfo...
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Defines all the PCL and non-PCL macros used.