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/memory.h>
43 #include <pcl/pcl_macros.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  using Ptr = shared_ptr<DefaultConvergenceCriteria<Scalar> >;
69  using ConstPtr = shared_ptr<const DefaultConvergenceCriteria<Scalar> >;
70 
71  using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
72 
74  {
82  };
83 
84  /** \brief Empty constructor.
85  * Sets:
86  * * the maximum number of iterations to 1000
87  * * the rotation threshold to 0.256 degrees (0.99999)
88  * * the translation threshold to 0.0003 meters (3e-4^2)
89  * * the MSE relative / absolute thresholds to 0.001% and 1e-12
90  *
91  * \param[in] iterations a reference to the number of iterations the loop has ran so far
92  * \param[in] transform a reference to the current transformation obtained by the transformation evaluation
93  * \param[in] correspondences a reference to the current set of point correspondences between source and target
94  */
95  DefaultConvergenceCriteria (const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences)
96  : iterations_ (iterations)
97  , transformation_ (transform)
98  , correspondences_ (correspondences)
99  , correspondences_prev_mse_ (std::numeric_limits<double>::max ())
100  , correspondences_cur_mse_ (std::numeric_limits<double>::max ())
101  , max_iterations_ (100) // 100 iterations
102  , failure_after_max_iter_ (false)
103  , rotation_threshold_ (0.99999) // 0.256 degrees
104  , translation_threshold_ (3e-4 * 3e-4) // 0.0003 meters
105  , mse_threshold_relative_ (0.00001) // 0.001% of the previous MSE (relative error)
106  , mse_threshold_absolute_ (1e-12) // MSE (absolute error)
110  {
111  }
112 
113  /** \brief Empty destructor */
115 
116  /** \brief Set the maximum number of consecutive iterations that the internal rotation,
117  * translation, and MSE differences are allowed to be similar.
118  * \param[in] nr_iterations the maximum number of iterations
119  */
120  inline void
121  setMaximumIterationsSimilarTransforms (const int nr_iterations) { max_iterations_similar_transforms_ = nr_iterations; }
122 
123  /** \brief Get the maximum number of consecutive iterations that the internal rotation,
124  * translation, and MSE differences are allowed to be similar, as set by the user.
125  */
126  inline int
128 
129  /** \brief Set the maximum number of iterations the internal optimization should run for.
130  * \param[in] nr_iterations the maximum number of iterations the internal optimization should run for
131  */
132  inline void
133  setMaximumIterations (const int nr_iterations) { max_iterations_ = nr_iterations; }
134 
135  /** \brief Get the maximum number of iterations the internal optimization should run for, as set by the user. */
136  inline int
137  getMaximumIterations () const { return (max_iterations_); }
138 
139  /** \brief Specifies if the registration fails or converges when the maximum number of iterations is reached.
140  * \param[in] failure_after_max_iter If true, the registration fails. If false, the registration is assumed to have converged.
141  */
142  inline void
143  setFailureAfterMaximumIterations (const bool failure_after_max_iter) { failure_after_max_iter_ = failure_after_max_iter; }
144 
145  /** \brief Get whether the registration will fail or converge when the maximum number of iterations is reached. */
146  inline bool
148 
149  /** \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.
150  * \param[in] threshold the rotation threshold in order for an optimization to be considered as having converged to the final solution.
151  */
152  inline void
153  setRotationThreshold (const double threshold) { rotation_threshold_ = threshold; }
154 
155  /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user.
156  */
157  inline double
159 
160  /** \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.
161  * \param[in] threshold the translation threshold in order for an optimization to be considered as having converged to the final solution.
162  */
163  inline void
164  setTranslationThreshold (const double threshold) { translation_threshold_ = threshold; }
165 
166  /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user.
167  */
168  inline double
170 
171  /** \brief Set the relative MSE between two consecutive sets of correspondences.
172  * \param[in] mse_relative the relative MSE threshold
173  */
174  inline void
175  setRelativeMSE (const double mse_relative) { mse_threshold_relative_ = mse_relative; }
176 
177  /** \brief Get the relative MSE between two consecutive sets of correspondences. */
178  inline double
180 
181  /** \brief Set the absolute MSE between two consecutive sets of correspondences.
182  * \param[in] mse_absolute the relative MSE threshold
183  */
184  inline void
185  setAbsoluteMSE (const double mse_absolute) { mse_threshold_absolute_ = mse_absolute; }
186 
187  /** \brief Get the absolute MSE between two consecutive sets of correspondences. */
188  inline double
190 
191 
192  /** \brief Check if convergence has been reached. */
193  bool
194  hasConverged () override;
195 
196  /** \brief Return the convergence state after hasConverged () */
199  {
200  return (convergence_state_);
201  }
202 
203  /** \brief Sets the convergence state externally (for example, when ICP does not find
204  * enough correspondences to estimate a transformation, the function is called setting
205  * the convergence state to ConvergenceState::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES)
206  * \param[in] c the convergence state
207  */
208  inline void
210  {
211  convergence_state_ = c;
212  }
213 
214  protected:
215 
216  /** \brief Calculate the mean squared error (MSE) of the distance for a given set of correspondences.
217  * \param[in] correspondences the given set of correspondences
218  */
219  inline double
220  calculateMSE (const pcl::Correspondences &correspondences) const
221  {
222  double mse = 0;
223  for (const auto &correspondence : correspondences)
224  mse += correspondence.distance;
225  mse /= double (correspondences.size ());
226  return (mse);
227  }
228 
229  /** \brief The number of iterations done by the registration loop so far. */
230  const int &iterations_;
231 
232  /** \brief The current transformation obtained by the transformation estimation method. */
234 
235  /** \brief The current set of point correspondences between the source and the target. */
237 
238  /** \brief The MSE for the previous set of correspondences. */
240 
241  /** \brief The MSE for the current set of correspondences. */
243 
244  /** \brief The maximum nuyyGmber of iterations that the registration loop is to be executed. */
246 
247  /** \brief Specifys if the registration fails or converges when the maximum number of iterations is reached. */
249 
250  /** \brief The rotation threshold is the relative rotation between two iterations (as angle cosine). */
252 
253  /** \brief The translation threshold is the relative translation between two iterations (0 if no translation). */
255 
256  /** \brief The relative change from the previous MSE for the current set of correspondences, e.g. .1 means 10% change. */
258 
259  /** \brief The absolute change from the previous MSE for the current set of correspondences. */
261 
262  /** \brief Internal counter for the number of iterations that the internal
263  * rotation, translation, and MSE differences are allowed to be similar. */
265 
266  /** \brief The maximum number of iterations that the internal rotation,
267  * translation, and MSE differences are allowed to be similar. */
269 
270  /** \brief The state of the convergence (e.g., why did the registration converge). */
272 
273  public:
275  };
276  }
277 }
278 
279 #include <pcl/registration/impl/default_convergence_criteria.hpp>
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES
@ CONVERGENCE_CRITERIA_NO_CORRESPONDENCES
Definition: default_convergence_criteria.h:80
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:164
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:133
pcl::registration::DefaultConvergenceCriteria::hasConverged
bool hasConverged() override
Check if convergence has been reached.
Definition: default_convergence_criteria.hpp:53
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_ABS_MSE
@ CONVERGENCE_CRITERIA_ABS_MSE
Definition: default_convergence_criteria.h:78
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:254
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:220
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:245
pcl::registration::ConvergenceCriteria::Ptr
shared_ptr< ConvergenceCriteria > Ptr
Definition: convergence_criteria.h:67
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:268
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:175
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:147
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:271
pcl::registration::DefaultConvergenceCriteria::iterations_
const int & iterations_
The number of iterations done by the registration loop so far.
Definition: default_convergence_criteria.h:230
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:236
pcl::registration::ConvergenceCriteria::ConstPtr
shared_ptr< const ConvergenceCriteria > ConstPtr
Definition: convergence_criteria.h:68
pcl::registration::DefaultConvergenceCriteria::ConvergenceState
ConvergenceState
Definition: default_convergence_criteria.h:73
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:75
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:169
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_REL_MSE
@ CONVERGENCE_CRITERIA_REL_MSE
Definition: default_convergence_criteria.h:79
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:209
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:137
pcl::registration::DefaultConvergenceCriteria::getConvergenceState
ConvergenceState getConvergenceState()
Return the convergence state after hasConverged ()
Definition: default_convergence_criteria.h:198
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:76
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:185
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:153
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS
@ CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS
Definition: default_convergence_criteria.h:81
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:257
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:248
pcl::registration::DefaultConvergenceCriteria::getMaximumIterationsSimilarTransforms
int getMaximumIterationsSimilarTransforms() const
Get the maximum number of consecutive iterations that the internal rotation, translation,...
Definition: default_convergence_criteria.h:127
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:158
pcl::registration::DefaultConvergenceCriteria::getAbsoluteMSE
double getAbsoluteMSE() const
Get the absolute MSE between two consecutive sets of correspondences.
Definition: default_convergence_criteria.h:189
pcl::registration::DefaultConvergenceCriteria::correspondences_prev_mse_
double correspondences_prev_mse_
The MSE for the previous set of correspondences.
Definition: default_convergence_criteria.h:239
pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_TRANSFORM
@ CONVERGENCE_CRITERIA_TRANSFORM
Definition: default_convergence_criteria.h:77
pcl::registration::DefaultConvergenceCriteria::transformation_
const Matrix4 & transformation_
The current transformation obtained by the transformation estimation method.
Definition: default_convergence_criteria.h:233
pcl::registration::DefaultConvergenceCriteria< float >::Matrix4
Eigen::Matrix< float, 4, 4 > Matrix4
Definition: default_convergence_criteria.h:71
pcl::registration::DefaultConvergenceCriteria::correspondences_cur_mse_
double correspondences_cur_mse_
The MSE for the current set of correspondences.
Definition: default_convergence_criteria.h:242
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:143
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:264
pcl::registration::DefaultConvergenceCriteria::getRelativeMSE
double getRelativeMSE() const
Get the relative MSE between two consecutive sets of correspondences.
Definition: default_convergence_criteria.h:179
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:251
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:260
pcl::registration::DefaultConvergenceCriteria::DefaultConvergenceCriteria
DefaultConvergenceCriteria(const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences)
Empty constructor.
Definition: default_convergence_criteria.h:95