Point Cloud Library (PCL)  1.11.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 1000
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 */
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 /= 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>
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES
@ CONVERGENCE_CRITERIA_NO_CORRESPONDENCES
Definition: default_convergence_criteria.h:78
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
Definition: convolution.h:46
pcl::registration::DefaultConvergenceCriteria::setTranslationThreshold
void setTranslationThreshold(const double threshold)
Set the translation threshold (maximum allowable difference between two consecutive transformations) ...
Definition: default_convergence_criteria.h:200
pcl::registration::DefaultConvergenceCriteria::setMaximumIterations
void setMaximumIterations(const int nr_iterations)
Set the maximum number of iterations the internal optimization should run for.
Definition: default_convergence_criteria.h:141
pcl::registration::DefaultConvergenceCriteria::hasConverged
bool hasConverged() override
Check if convergence has been reached.
Definition: default_convergence_criteria.hpp:51
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_ABS_MSE
@ CONVERGENCE_CRITERIA_ABS_MSE
Definition: default_convergence_criteria.h:76
pcl::registration::DefaultConvergenceCriteria::translation_threshold_
double translation_threshold_
The translation threshold is the relative translation between two iterations (0 if no translation).
Definition: default_convergence_criteria.h:313
pcl::registration::DefaultConvergenceCriteria::calculateMSE
double calculateMSE(const pcl::Correspondences &correspondences) const
Calculate the mean squared error (MSE) of the distance for a given set of correspondences.
Definition: default_convergence_criteria.h:273
pcl::registration::DefaultConvergenceCriteria::max_iterations_
int max_iterations_
The maximum nuyyGmber of iterations that the registration loop is to be executed.
Definition: default_convergence_criteria.h:301
pcl::registration::ConvergenceCriteria::Ptr
shared_ptr< ConvergenceCriteria > Ptr
Definition: convergence_criteria.h:66
pcl::registration::DefaultConvergenceCriteria
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
Definition: default_convergence_criteria.h:65
pcl::registration::DefaultConvergenceCriteria::max_iterations_similar_transforms_
int max_iterations_similar_transforms_
The maximum number of iterations that the internal rotation, translation, and MSE differences are all...
Definition: default_convergence_criteria.h:329
pcl::registration::DefaultConvergenceCriteria::setRelativeMSE
void setRelativeMSE(const double mse_relative)
Set the relative MSE between two consecutive sets of correspondences.
Definition: default_convergence_criteria.h:218
pcl::registration::DefaultConvergenceCriteria::getFailureAfterMaximumIterations
bool getFailureAfterMaximumIterations() const
Get whether the registration will fail or converge when the maximum number of iterations is reached.
Definition: default_convergence_criteria.h:167
pcl::registration::DefaultConvergenceCriteria::convergence_state_
ConvergenceState convergence_state_
The state of the convergence (e.g., why did the registration converge).
Definition: default_convergence_criteria.h:332
pcl::registration::DefaultConvergenceCriteria::iterations_
const int & iterations_
The number of iterations done by the registration loop so far.
Definition: default_convergence_criteria.h:283
pcl::registration::DefaultConvergenceCriteria::correspondences_
const pcl::Correspondences & correspondences_
The current set of point correspondences between the source and the target.
Definition: default_convergence_criteria.h:291
pcl::registration::ConvergenceCriteria::ConstPtr
shared_ptr< const ConvergenceCriteria > ConstPtr
Definition: convergence_criteria.h:67
pcl::registration::DefaultConvergenceCriteria::ConvergenceState
ConvergenceState
Definition: default_convergence_criteria.h:72
pcl::registration::DefaultConvergenceCriteria::setMaximumIterationsSimilarTransforms
void setMaximumIterationsSimilarTransforms(const int nr_iterations)
Set the maximum number of consecutive iterations that the internal rotation, translation,...
Definition: default_convergence_criteria.h:121
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_NOT_CONVERGED
@ CONVERGENCE_CRITERIA_NOT_CONVERGED
Definition: default_convergence_criteria.h:73
pcl::registration::DefaultConvergenceCriteria::getTranslationThreshold
double getTranslationThreshold() const
Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transfo...
Definition: default_convergence_criteria.h:209
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_REL_MSE
@ CONVERGENCE_CRITERIA_REL_MSE
Definition: default_convergence_criteria.h:77
pcl::registration::DefaultConvergenceCriteria::setConvergenceState
void setConvergenceState(ConvergenceState c)
Sets the convergence state externally (for example, when ICP does not find enough correspondences to ...
Definition: default_convergence_criteria.h:263
pcl::registration::DefaultConvergenceCriteria::getMaximumIterations
int getMaximumIterations() const
Get the maximum number of iterations the internal optimization should run for, as set by the user.
Definition: default_convergence_criteria.h:149
pcl::registration::DefaultConvergenceCriteria::getConvergenceState
ConvergenceState getConvergenceState()
Return the convergence state after hasConverged ()
Definition: default_convergence_criteria.h:252
pcl::registration::DefaultConvergenceCriteria::~DefaultConvergenceCriteria
~DefaultConvergenceCriteria()
Empty destructor.
Definition: default_convergence_criteria.h:114
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_ITERATIONS
@ CONVERGENCE_CRITERIA_ITERATIONS
Definition: default_convergence_criteria.h:74
pcl::registration::DefaultConvergenceCriteria::setAbsoluteMSE
void setAbsoluteMSE(const double mse_absolute)
Set the absolute MSE between two consecutive sets of correspondences.
Definition: default_convergence_criteria.h:234
pcl::registration::DefaultConvergenceCriteria::setRotationThreshold
void setRotationThreshold(const double threshold)
Set the rotation threshold cosine angle (maximum allowable difference between two consecutive transfo...
Definition: default_convergence_criteria.h:179
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS
@ CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS
Definition: default_convergence_criteria.h:79
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
pcl::registration::DefaultConvergenceCriteria::mse_threshold_relative_
double mse_threshold_relative_
The relative change from the previous MSE for the current set of correspondences, e....
Definition: default_convergence_criteria.h:317
pcl::registration::DefaultConvergenceCriteria::failure_after_max_iter_
bool failure_after_max_iter_
Specifys if the registration fails or converges when the maximum number of iterations is reached.
Definition: default_convergence_criteria.h:305
pcl::registration::DefaultConvergenceCriteria::getMaximumIterationsSimilarTransforms
int getMaximumIterationsSimilarTransforms() const
Get the maximum number of consecutive iterations that the internal rotation, translation,...
Definition: default_convergence_criteria.h:131
pcl::registration::DefaultConvergenceCriteria::getRotationThreshold
double getRotationThreshold() const
Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transfo...
Definition: default_convergence_criteria.h:188
pcl::registration::DefaultConvergenceCriteria::getAbsoluteMSE
double getAbsoluteMSE() const
Get the absolute MSE between two consecutive sets of correspondences.
Definition: default_convergence_criteria.h:241
pcl::registration::DefaultConvergenceCriteria::correspondences_prev_mse_
double correspondences_prev_mse_
The MSE for the previous set of correspondences.
Definition: default_convergence_criteria.h:294
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_TRANSFORM
@ CONVERGENCE_CRITERIA_TRANSFORM
Definition: default_convergence_criteria.h:75
pcl::registration::DefaultConvergenceCriteria::transformation_
const Matrix4 & transformation_
The current transformation obtained by the transformation estimation method.
Definition: default_convergence_criteria.h:287
pcl::registration::DefaultConvergenceCriteria< float >::Matrix4
Eigen::Matrix< float, 4, 4 > Matrix4
Definition: default_convergence_criteria.h:70
pcl::registration::DefaultConvergenceCriteria::correspondences_cur_mse_
double correspondences_cur_mse_
The MSE for the current set of correspondences.
Definition: default_convergence_criteria.h:297
pcl::registration::DefaultConvergenceCriteria::setFailureAfterMaximumIterations
void setFailureAfterMaximumIterations(const bool failure_after_max_iter)
Specifies if the registration fails or converges when the maximum number of iterations is reached.
Definition: default_convergence_criteria.h:159
pcl::registration::DefaultConvergenceCriteria::iterations_similar_transforms_
int iterations_similar_transforms_
Internal counter for the number of iterations that the internal rotation, translation,...
Definition: default_convergence_criteria.h:325
pcl::registration::DefaultConvergenceCriteria::getRelativeMSE
double getRelativeMSE() const
Get the relative MSE between two consecutive sets of correspondences.
Definition: default_convergence_criteria.h:225
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:89
pcl::registration::ConvergenceCriteria
ConvergenceCriteria represents an abstract base class for different convergence criteria used in regi...
Definition: convergence_criteria.h:64
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::registration::DefaultConvergenceCriteria::rotation_threshold_
double rotation_threshold_
The rotation threshold is the relative rotation between two iterations (as angle cosine).
Definition: default_convergence_criteria.h:309
pcl::registration::DefaultConvergenceCriteria::mse_threshold_absolute_
double mse_threshold_absolute_
The absolute change from the previous MSE for the current set of correspondences.
Definition: default_convergence_criteria.h:321
pcl::registration::DefaultConvergenceCriteria::DefaultConvergenceCriteria
DefaultConvergenceCriteria(const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences)
Empty constructor.
Definition: default_convergence_criteria.h:94