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