341 lines
12 KiB
C++
341 lines
12 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2012-, Open Perception, Inc.
|
|
*
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of the copyright holder(s) nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
* $Id$
|
|
*
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <pcl/registration/convergence_criteria.h>
|
|
#include <pcl/correspondence.h>
|
|
#include <pcl/memory.h>
|
|
#include <pcl/pcl_macros.h>
|
|
|
|
namespace pcl {
|
|
namespace registration {
|
|
/** \brief @b DefaultConvergenceCriteria represents an instantiation of
|
|
* ConvergenceCriteria, and implements the following criteria for registration loop
|
|
* evaluation:
|
|
*
|
|
* * a maximum number of iterations has been reached
|
|
* * the transformation (R, t) cannot be further updated (the difference between
|
|
* current and previous is smaller than a threshold)
|
|
* * 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)
|
|
*
|
|
* \note Convergence is considered reached if ANY of the above criteria are met.
|
|
*
|
|
* \author Radu B. Rusu
|
|
* \ingroup registration
|
|
*/
|
|
template <typename Scalar = float>
|
|
class DefaultConvergenceCriteria : public ConvergenceCriteria {
|
|
public:
|
|
using Ptr = shared_ptr<DefaultConvergenceCriteria<Scalar>>;
|
|
using ConstPtr = shared_ptr<const DefaultConvergenceCriteria<Scalar>>;
|
|
|
|
using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
|
|
|
|
enum ConvergenceState {
|
|
CONVERGENCE_CRITERIA_NOT_CONVERGED,
|
|
CONVERGENCE_CRITERIA_ITERATIONS,
|
|
CONVERGENCE_CRITERIA_TRANSFORM,
|
|
CONVERGENCE_CRITERIA_ABS_MSE,
|
|
CONVERGENCE_CRITERIA_REL_MSE,
|
|
CONVERGENCE_CRITERIA_NO_CORRESPONDENCES,
|
|
CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS
|
|
};
|
|
|
|
/** \brief Empty constructor.
|
|
* Sets:
|
|
* * the maximum number of iterations to 1000
|
|
* * the rotation threshold to 0.256 degrees (0.99999)
|
|
* * the translation threshold to 0.0003 meters (3e-4^2)
|
|
* * the MSE relative / absolute thresholds to 0.001% and 1e-12
|
|
*
|
|
* \param[in] iterations a reference to the number of iterations the loop has ran so
|
|
* far \param[in] transform a reference to the current transformation obtained by the
|
|
* transformation evaluation \param[in] correspondences a reference to the current set
|
|
* of point correspondences between source and target
|
|
*/
|
|
DefaultConvergenceCriteria(const int& iterations,
|
|
const Matrix4& transform,
|
|
const pcl::Correspondences& correspondences)
|
|
: iterations_(iterations)
|
|
, transformation_(transform)
|
|
, correspondences_(correspondences)
|
|
, correspondences_prev_mse_(std::numeric_limits<double>::max())
|
|
, correspondences_cur_mse_(std::numeric_limits<double>::max())
|
|
, max_iterations_(100) // 100 iterations
|
|
, failure_after_max_iter_(false)
|
|
, rotation_threshold_(0.99999) // 0.256 degrees
|
|
, translation_threshold_(3e-4 * 3e-4) // 0.0003 meters
|
|
, mse_threshold_relative_(0.00001) // 0.001% of the previous MSE (relative error)
|
|
, mse_threshold_absolute_(1e-12) // MSE (absolute error)
|
|
, iterations_similar_transforms_(0)
|
|
, max_iterations_similar_transforms_(0)
|
|
, convergence_state_(CONVERGENCE_CRITERIA_NOT_CONVERGED)
|
|
{}
|
|
|
|
/** \brief Empty destructor */
|
|
~DefaultConvergenceCriteria() {}
|
|
|
|
/** \brief Set the maximum number of consecutive iterations that the internal
|
|
* rotation, translation, and MSE differences are allowed to be similar. \param[in]
|
|
* nr_iterations the maximum number of iterations
|
|
*/
|
|
inline void
|
|
setMaximumIterationsSimilarTransforms(const int nr_iterations)
|
|
{
|
|
max_iterations_similar_transforms_ = nr_iterations;
|
|
}
|
|
|
|
/** \brief Get the maximum number of consecutive iterations that the internal
|
|
* rotation, translation, and MSE differences are allowed to be similar, as set by the
|
|
* user.
|
|
*/
|
|
inline int
|
|
getMaximumIterationsSimilarTransforms() const
|
|
{
|
|
return (max_iterations_similar_transforms_);
|
|
}
|
|
|
|
/** \brief Set the maximum number of iterations the internal optimization should run
|
|
* for. \param[in] nr_iterations the maximum number of iterations the internal
|
|
* optimization should run for
|
|
*/
|
|
inline void
|
|
setMaximumIterations(const int nr_iterations)
|
|
{
|
|
max_iterations_ = nr_iterations;
|
|
}
|
|
|
|
/** \brief Get the maximum number of iterations the internal optimization should run
|
|
* for, as set by the user. */
|
|
inline int
|
|
getMaximumIterations() const
|
|
{
|
|
return (max_iterations_);
|
|
}
|
|
|
|
/** \brief Specifies if the registration fails or converges when the maximum number of
|
|
* iterations is reached. \param[in] failure_after_max_iter If true, the registration
|
|
* fails. If false, the registration is assumed to have converged.
|
|
*/
|
|
inline void
|
|
setFailureAfterMaximumIterations(const bool failure_after_max_iter)
|
|
{
|
|
failure_after_max_iter_ = failure_after_max_iter;
|
|
}
|
|
|
|
/** \brief Get whether the registration will fail or converge when the maximum number
|
|
* of iterations is reached. */
|
|
inline bool
|
|
getFailureAfterMaximumIterations() const
|
|
{
|
|
return (failure_after_max_iter_);
|
|
}
|
|
|
|
/** \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. \param[in] threshold the
|
|
* rotation threshold in order for an optimization to be considered as having
|
|
* converged to the final solution.
|
|
*/
|
|
inline void
|
|
setRotationThreshold(const double threshold)
|
|
{
|
|
rotation_threshold_ = threshold;
|
|
}
|
|
|
|
/** \brief Get the rotation threshold cosine angle (maximum allowable difference
|
|
* between two consecutive transformations) as set by the user.
|
|
*/
|
|
inline double
|
|
getRotationThreshold() const
|
|
{
|
|
return (rotation_threshold_);
|
|
}
|
|
|
|
/** \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. \param[in] threshold the translation
|
|
* threshold in order for an optimization to be considered as having converged to the
|
|
* final solution.
|
|
*/
|
|
inline void
|
|
setTranslationThreshold(const double threshold)
|
|
{
|
|
translation_threshold_ = threshold;
|
|
}
|
|
|
|
/** \brief Get the rotation threshold cosine angle (maximum allowable difference
|
|
* between two consecutive transformations) as set by the user.
|
|
*/
|
|
inline double
|
|
getTranslationThreshold() const
|
|
{
|
|
return (translation_threshold_);
|
|
}
|
|
|
|
/** \brief Set the relative MSE between two consecutive sets of correspondences.
|
|
* \param[in] mse_relative the relative MSE threshold
|
|
*/
|
|
inline void
|
|
setRelativeMSE(const double mse_relative)
|
|
{
|
|
mse_threshold_relative_ = mse_relative;
|
|
}
|
|
|
|
/** \brief Get the relative MSE between two consecutive sets of correspondences. */
|
|
inline double
|
|
getRelativeMSE() const
|
|
{
|
|
return (mse_threshold_relative_);
|
|
}
|
|
|
|
/** \brief Set the absolute MSE between two consecutive sets of correspondences.
|
|
* \param[in] mse_absolute the relative MSE threshold
|
|
*/
|
|
inline void
|
|
setAbsoluteMSE(const double mse_absolute)
|
|
{
|
|
mse_threshold_absolute_ = mse_absolute;
|
|
}
|
|
|
|
/** \brief Get the absolute MSE between two consecutive sets of correspondences. */
|
|
inline double
|
|
getAbsoluteMSE() const
|
|
{
|
|
return (mse_threshold_absolute_);
|
|
}
|
|
|
|
/** \brief Check if convergence has been reached. */
|
|
bool
|
|
hasConverged() override;
|
|
|
|
/** \brief Return the convergence state after hasConverged () */
|
|
ConvergenceState
|
|
getConvergenceState()
|
|
{
|
|
return (convergence_state_);
|
|
}
|
|
|
|
/** \brief Sets the convergence state externally (for example, when ICP does not find
|
|
* enough correspondences to estimate a transformation, the function is called setting
|
|
* the convergence state to ConvergenceState::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES)
|
|
* \param[in] c the convergence state
|
|
*/
|
|
inline void
|
|
setConvergenceState(ConvergenceState c)
|
|
{
|
|
convergence_state_ = c;
|
|
}
|
|
|
|
protected:
|
|
/** \brief Calculate the mean squared error (MSE) of the distance for a given set of
|
|
* correspondences. \param[in] correspondences the given set of correspondences
|
|
*/
|
|
inline double
|
|
calculateMSE(const pcl::Correspondences& correspondences) const
|
|
{
|
|
double mse = 0;
|
|
for (const auto& correspondence : correspondences)
|
|
mse += correspondence.distance;
|
|
mse /= double(correspondences.size());
|
|
return (mse);
|
|
}
|
|
|
|
/** \brief The number of iterations done by the registration loop so far. */
|
|
const int& iterations_;
|
|
|
|
/** \brief The current transformation obtained by the transformation estimation
|
|
* method. */
|
|
const Matrix4& transformation_;
|
|
|
|
/** \brief The current set of point correspondences between the source and the target.
|
|
*/
|
|
const pcl::Correspondences& correspondences_;
|
|
|
|
/** \brief The MSE for the previous set of correspondences. */
|
|
double correspondences_prev_mse_;
|
|
|
|
/** \brief The MSE for the current set of correspondences. */
|
|
double correspondences_cur_mse_;
|
|
|
|
/** \brief The maximum nuyyGmber of iterations that the registration loop is to be
|
|
* executed. */
|
|
int max_iterations_;
|
|
|
|
/** \brief Specifys if the registration fails or converges when the maximum number of
|
|
* iterations is reached. */
|
|
bool failure_after_max_iter_;
|
|
|
|
/** \brief The rotation threshold is the relative rotation between two iterations (as
|
|
* angle cosine). */
|
|
double rotation_threshold_;
|
|
|
|
/** \brief The translation threshold is the relative translation between two
|
|
* iterations (0 if no translation). */
|
|
double translation_threshold_;
|
|
|
|
/** \brief The relative change from the previous MSE for the current set of
|
|
* correspondences, e.g. .1 means 10% change. */
|
|
double mse_threshold_relative_;
|
|
|
|
/** \brief The absolute change from the previous MSE for the current set of
|
|
* correspondences. */
|
|
double mse_threshold_absolute_;
|
|
|
|
/** \brief Internal counter for the number of iterations that the internal
|
|
* rotation, translation, and MSE differences are allowed to be similar. */
|
|
int iterations_similar_transforms_;
|
|
|
|
/** \brief The maximum number of iterations that the internal rotation,
|
|
* translation, and MSE differences are allowed to be similar. */
|
|
int max_iterations_similar_transforms_;
|
|
|
|
/** \brief The state of the convergence (e.g., why did the registration converge). */
|
|
ConvergenceState convergence_state_;
|
|
|
|
public:
|
|
PCL_MAKE_ALIGNED_OPERATOR_NEW
|
|
};
|
|
} // namespace registration
|
|
} // namespace pcl
|
|
|
|
#include <pcl/registration/impl/default_convergence_criteria.hpp>
|