PCL - ICP代碼研讀(二四 ) - DefaultConvergenceCriteria架構
前言
DefaultConvergenceCriteria
顧名思義,也就是預設的收斂判斷條件。
本篇對應到default_convergence_criteria.h
這個檔案。
DefaultConvergenceCriteria
#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
定義名稱,方便後續使用:
using Ptr = shared_ptr<DefaultConvergenceCriteria<Scalar>>;
using ConstPtr = shared_ptr<const DefaultConvergenceCriteria<Scalar>>;
using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
收斂狀態
DefaultConvergenceCriteria
中依照收斂/未收斂的原因定義了以下幾種收斂狀態:
enum ConvergenceState {
// 未收斂
CONVERGENCE_CRITERIA_NOT_CONVERGED,
// 達到最大迭代次數,並當成收斂
CONVERGENCE_CRITERIA_ITERATIONS,
// 旋轉平移幅度夠小
CONVERGENCE_CRITERIA_TRANSFORM,
// 兩次迭代間的abs mse變化量夠小
CONVERGENCE_CRITERIA_ABS_MSE,
// 兩次迭代間的相對 mse變化量夠小
CONVERGENCE_CRITERIA_REL_MSE,
// 迭代過程中發現配對數量小於min_number_correspondences_(3)
CONVERGENCE_CRITERIA_NO_CORRESPONDENCES,
// 達到最大迭代次數,並當成未收斂
CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS
};
constructor和destructor
/** \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_是reference variable
: iterations_(iterations)
// 注意transformation_是reference variable
, transformation_(transform)
// 注意correspondences_是reference variable
, 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() {}
getter和setter
/** \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;
}
calculateMSE
計算多個配對的mse。
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)
// 這裡的distance是距離平方?
mse += correspondence.distance;
mse /= double(correspondences.size());
return (mse);
}
protected成員變數
hasConverged
函數中定義了三種退出迭代的條件,只要滿足其中之一就會退出迭代,並認定算法收斂或發散。
以下是三種退出條件以及它們各自涉及的成員變數:
- 第一種退出條件:迭代次數超過上限
iterations_
:當前迭代次數max_iterations_
:預先設定的最大迭代次數failure_after_max_iter_
:如果設為true,那麼迭代次數超過max_iterations_
後,會認定算法發散
- 第二種退出條件:連續多次旋轉平移幅度都夠小
transformation_
:本次迭代計算出來的轉換矩陣rotation_threshold_
:旋轉幅度閾值translation_threshold_
:平移幅度閾值
- 第三種退出條件:
correspondences_
:本次迭代估計出來的點對correspondences_prev_mse_
:使用上次迭代估計出來的點對計算出來的rmsecorrespondences_cur_mse_
:使用本次迭代估計出來的點對計算出來的rmsemse_threshold_relative_
:兩次rmse相對差值的閾值mse_threshold_absolute_
:兩次rmse差值絕對值的閾值
similar相關:
iterations_similar_transforms_
:如果在某次迭代中,旋轉平移幅度都夠小,或是相對/絕對rmse夠小,就會認為該次迭代是similar的。iterations_similar_transforms_
這個變數用於記錄有連續多少次迭代是similar的。max_iterations_similar_transforms_
:如果iterations_similar_transforms_
大於max_iterations_similar_transforms_
就會退出
/** \brief The number of iterations done by the registration loop so far. */
// 注意它是被宣告為reference variable?!
const int& iterations_;
/** \brief The current transformation obtained by the transformation estimation
* method. */
// 注意它是被宣告為reference variable?!
const Matrix4& transformation_;
/** \brief The current set of point correspondences between the source and the target.
*/
// 注意它是被宣告為reference variable?!
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. */
// 如果failure_after_max_iter_為false,那麼當達到最大迭代次數時,hasConverged會立即回傳true
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. */
// 用來記錄有連續多少次迭代時is_similar為true
int iterations_similar_transforms_;
/** \brief The maximum number of iterations that the internal rotation,
* translation, and MSE differences are allowed to be similar. */
// 如果連續max_iterations_similar_transforms_次迭代is_similar都為true,就認為是收斂了
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>