PCL - ICP代碼研讀（二五 ） - DefaultConvergenceCriteria實現

前言

接續PCL - ICP代碼研讀（二四 ） - DefaultConvergenceCriteria架構，本篇繼續介紹DefaultConvergenceCriteria中hasConverged函數的實現。

本篇對應到default_convergence_criteria.hpp這個檔案。

DefaultConvergenceCriteria

#ifndef PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_
#define PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_

#include <pcl/console/print.h>

namespace pcl {

namespace registration {

template <typename Scalar>
bool
DefaultConvergenceCriteria<Scalar>::hasConverged()
{

重設iterations_similar_transforms_和convergence_state_：

  if (convergence_state_ != CONVERGENCE_CRITERIA_NOT_CONVERGED) {
    // 為什麼要reset?
    // 這個函數要特定時機才能調用?
    // If it already converged or failed before, reset.
    iterations_similar_transforms_ = 0;
    convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED;
  }

當兩次估計出來的轉換夠相似（旋轉平移幅度夠小，或是相對/絕對rmse夠小）時，is_similar會被設為true：

  bool is_similar = false;

  PCL_DEBUG("[pcl::DefaultConvergenceCriteria::hasConverged] Iteration %d out of %d.\n",
            iterations_,
            max_iterations_);

第一種退出情況：迭代次數iterations_超過事先設定的最大迭代次數max_iterations_，並且當failure_after_max_iter_被設定為false的情況下，會認為校正算法成功收斂，收斂原因convergence_state_會被設為CONVERGENCE_CRITERIA_ITERATIONS並退出。

如果failure_after_max_iter_被設定為true，會暫時先將convergence_state_設為CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS，然後繼續檢查其他收斂條件。

  // 1. Number of iterations has reached the maximum user imposed number of iterations
  if (iterations_ >= max_iterations_) {
    if (!failure_after_max_iter_) {
      // failure_after_max_iter_為false,那麼在迭代次數iterations_達到max_iterations時,會認為是已經收斂了,否則繼續往下檢查
      convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS;
      return (true);
    }
    convergence_state_ = CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS;
  }

第二種退出情況：連續多次旋轉平移幅度都夠小。

  // 2. The epsilon (difference) between the previous transformation and the current
  // estimated transformation

計算旋轉角度的cos，套用由旋轉矩陣得到旋轉軸和旋轉角度公式：

  // https://math.stackexchange.com/a/2611955/621758
  // https://blog.csdn.net/keineahnung2345/article/details/120112413
  // 0.5*(旋轉矩陣的trace-1) = cos(旋轉角度)
  double cos_angle = 0.5 * (transformation_.coeff(0, 0) + transformation_.coeff(1, 1) +
                            transformation_.coeff(2, 2) - 1);

計算平移幅度，即轉換矩陣transformation_右上角3*1矩陣的squared norm：

  // sqr是平方,sqrt是平方根
  double translation_sqr = transformation_.coeff(0, 3) * transformation_.coeff(0, 3) +
                           transformation_.coeff(1, 3) * transformation_.coeff(1, 3) +
                           transformation_.coeff(2, 3) * transformation_.coeff(2, 3);
  PCL_DEBUG("[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation "
            "gave %f rotation (cosine) and %f translation.\n",
            cos_angle,
            translation_sqr);

如果旋轉角度夠小（cos夠大）並且平移幅度夠小：

  // 旋轉角度小(cos夠大),並且平移幅度小
  if (cos_angle >= rotation_threshold_ && translation_sqr <= translation_threshold_) {

並且如果連續max_iterations_similar_transforms_次迭代都是相似的（旋轉平移幅度夠小，或是相對/絕對rmse夠小），就會將收斂原因convergence_state_設為CONVERGENCE_CRITERIA_TRANSFORM並退出。

    // iterations_similar_transforms_表示連續多少次迭代時is_similar為true
    // 如果連續max_iterations_similar_transforms_次迭代is_similar都為true,就認為是收斂了
    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) {
      // 連續幾次迭代都是相似的(is_similar = true),就將收斂原因記為CONVERGENCE_CRITERIA_TRANSFORM然後退出
      convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM;
      return (true);
    }

如果旋轉角度夠小（cos夠大）並且平移幅度夠小，那麼該次迭代就會被當成是相似的：

    // 旋轉平移幅度小會被當成是相似的(is_similar = true)
    is_similar = true;
  }

計算本次迭代的mse，這是為第三種退出情況所準備：

  correspondences_cur_mse_ = calculateMSE(correspondences_);
  PCL_DEBUG("[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE "
            "for correspondences distances is: %f / %f.\n",
            correspondences_prev_mse_,
            correspondences_cur_mse_);

如果兩次迭代mse的差值夠小（小於mse_threshold_absolute_），則該次迭代就會被認為是相似的。如果連續max_iterations_similar_transforms_次迭代都是相似的，就會將收斂原因convergence_state_設為CONVERGENCE_CRITERIA_ABS_MSE並退出。

  // 3. The relative sum of Euclidean squared errors is smaller than a user defined
  // threshold Absolute
  // 兩次迭代mse的差值小於mse_threshold_absolute_
  if (std::abs(correspondences_cur_mse_ - correspondences_prev_mse_) <
      mse_threshold_absolute_) {
    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) {
      // 連續幾次迭代都是相似的(is_similar = true),就將收斂原因記為CONVERGENCE_CRITERIA_ABS_MSE然後退出
      convergence_state_ = CONVERGENCE_CRITERIA_ABS_MSE;
      return (true);
    }
    // mse差值夠小會被當成是相似的(is_similar = true)
    is_similar = true;
  }

如果兩次迭代mse的"相對"差值夠小（小於mse_threshold_relative_），則該次迭代就會被認為是相似的。如果連續max_iterations_similar_transforms_次都是相似的，就會將收斂原因convergence_state_設為CONVERGENCE_CRITERIA_REL_MSE並退出。

  // Relative
  // 當次mse與前次mse差值與前次mse的比值小於mse_threshold_relative_
  if (std::abs(correspondences_cur_mse_ - correspondences_prev_mse_) /
          correspondences_prev_mse_ <
      mse_threshold_relative_) {
    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) {
      // 連續幾次迭代都是相似的(is_similar = true),就將收斂原因記為CONVERGENCE_CRITERIA_REL_MSE;然後退出
      convergence_state_ = CONVERGENCE_CRITERIA_REL_MSE;
      return (true);
    }
    // mse的相對差值夠小會被當成是相似的(is_similar = true)
    is_similar = true;
  }

如果該次迭代是相似的（is_similar為true），就會遞增iterations_similar_transforms_，否則將它重置為0：

  if (is_similar) {
    // Increment the number of transforms that the thresholds are allowed to be similar
    // 如果當次迭代is_similar被設為true,就遞增iterations_similar_transforms_
    ++iterations_similar_transforms_;
  }
  else {
    // When the transform becomes large, reset.
    // 如果當次迭代is_similar被設為false,就重設iterations_similar_transforms_
    iterations_similar_transforms_ = 0;
  }

使用correspondences_prev_mse_變數記錄本次的mse，在下次迭代中會用到：

  correspondences_prev_mse_ = correspondences_cur_mse_;

如果三種收斂條件都不滿足，則回傳false，代表算法未收斂（此時convergence_state_等於CONVERGENCE_CRITERIA_NOT_CONVERGED）：

  return (false);
}

} // namespace registration
} // namespace pcl

#endif // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_