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_：使用上次迭代估計出來的點對計算出來的rmse
  • correspondences_cur_mse_：使用本次迭代估計出來的點對計算出來的rmse
  • mse_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>