PCL - ICP代碼研讀（九 ） - computeTransform函數_previous_transformation_

前言

computeTransform函數實現了ICP算法，是icp.hpp的精髓所在。

回顧ICP（Iterative Closest Point）算法推導中提到的ICP算法三步驟，第一步是在兩點雲間尋找點對，然後拒絕其中不合理的點對；第二步是估計變換矩陣；第三步是判斷是否收斂。

這三步分別對應到以下物件：

1. 點對估計器correspondence_estimation_，點對拒絕器（多個）correspondence_rejectors_
2. 轉換矩陣估計器transformation_estimation_
3. 收斂條件convergence_criteria_

在以下代碼中，會時不時看到他們的身影。

computeTransform

computeTransform函數首先在兩點雲間尋找點對correspondences_，然後根據兩點雲間的點對估計出transformation_。在迭代的過程中，會將transformation_累乘至final_transformation_，final_transformation_即為最終的轉換矩陣。算法收斂後，對輸入點雲input_做變換，最後將結果存入output點雲中。

// 這個函數在registration.h中是virtual的
template <typename PointSource, typename PointTarget, typename Scalar>
void
IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation(
    PointCloudSource& output, const Matrix4& guess)
{
1
2
3
4
5
6

初始化：input_transformed用來儲存轉換後的點雲；將當前迭代次數nr_iterations_設為0；將收斂與否converged_設為false。

  // 這個注釋講的好像是CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_));才對?
  // Point cloud containing the correspondences of each point in <input, indices>
  PointCloudSourcePtr input_transformed(new PointCloudSource);

  nr_iterations_ = 0;
  converged_ = false;
1
2
3
4
5
6

將guess累乘到final_transformation_上：

  // Initialise final transformation to the guessed one
  final_transformation_ = guess;
1
2

在此使用guess（如果有）對input_做轉換，得到input_transformed：

  // 如果輸入的guess非identify,則對input_做轉換後存入input_transformed,然後將transformation設為identity
  // If the guessed transformation is non identity
  if (guess != Matrix4::Identity()) {
    input_transformed->resize(input_->size());
    // Apply guessed transformation prior to search for neighbours
    transformCloud(*input_, *input_transformed, guess);
  }
  else
    *input_transformed = *input_;
1
2
3
4
5
6
7
8
9

因為transformation_表示當次估計出來的轉換矩陣，所以這邊不將它設為guess，而是設為單位矩陣：

  transformation_ = Matrix4::Identity();
1

determineRequiredBlobData函數用於決定是否需要執行pcl::toPCLPointCloud2，詳見PCL - ICP代碼研讀（七 ） - determineRequiredBlobData函數：

  // Make blobs if necessary
  // ?
  determineRequiredBlobData();
1
2
3

如果need_target_blob_為true，代表我們需要pcl::PCLPointCloud2類型的target點雲。這邊對target_點雲執行pcl::toPCLPointCloud2，得到pcl::PCLPointCloud2類型的target_blob，待會作為pcl::registration::CorrespondenceEstimationBase::setTargetNormals等函數的參數：

  PCLPointCloud2::Ptr target_blob(new PCLPointCloud2);
  if (need_target_blob_)
    pcl::toPCLPointCloud2(*target_, *target_blob);
1
2
3

設定點對估計器的target點雲及法向量：

  // Pass in the default target for the Correspondence Estimation/Rejection code
  // 目標點雲和法向量可以在循環之前設好,source點雲和法向量則需要在每一個iteration都重設一遍
  correspondence_estimation_->setInputTarget(target_);
  if (correspondence_estimation_->requiresTargetNormals())
    correspondence_estimation_->setTargetNormals(target_blob);
1
2
3
4
5

設定（多個）點對拒絕器的target點雲及法向量：

  // Correspondence Rejectors need a binary blob
  // 為correspondence_rejectors_中的每一個rejector設置目標點雲和法向量
  // 目標點雲和法向量可以在循環之前設好,source點雲和法向量則需要在每一個iteration都重設一遍
  for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
    registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
    if (rej->requiresTargetPoints())
      rej->setTargetPoints(target_blob);
    if (rej->requiresTargetNormals() && target_has_normals_)
      rej->setTargetNormals(target_blob);
  }
1
2
3
4
5
6
7
8
9
10

設定收斂條件，這裡的各種參數將留到PCL - ICP代碼研讀（二四 ） - DefaultConvergenceCriteria架構中繼續介紹：

  convergence_criteria_->setMaximumIterations(max_iterations_);
  //euclidean_fitness_epsilon_怎麼會跟relative mse扯在一起?
  convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_);
  convergence_criteria_->setTranslationThreshold(transformation_epsilon_);
  if (transformation_rotation_epsilon_ > 0)
    // cos(角度)要小於transformation_rotation_epsilon_
    // 小於還是大於?
    convergence_criteria_->setRotationThreshold(transformation_rotation_epsilon_);
  else
    // 應該是1.0 - transformation_rotation_epsilon_,但如果改成這樣閾值就會大於1?
    // transformation_rotation_epsilon_在-1到0之間仍然是有意義的,所以可以把else branch刪除? 
    convergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);
1
2
3
4
5
6
7
8
9
10
11
12

以下過程會迭代進行直到確認為收斂或是發散為止：

  // Repeat until convergence
  do {
1
2

將input_transformed轉換為pcl::PCLPointCloud2型別的input_transformed_blob：

    // Get blob data if needed
    // ?
    PCLPointCloud2::Ptr input_transformed_blob;
    if (need_source_blob_) {
      input_transformed_blob.reset(new PCLPointCloud2);
      toPCLPointCloud2(*input_transformed, *input_transformed_blob);
    }
1
2
3
4
5
6
7

接下來會更新transformation_，在此先儲存previous_transformation_：

    // Save the previously estimated transformation
    previous_transformation_ = transformation_;
1
2

設定點對估計器的source點雲及法向量：

    // Set the source each iteration, to ensure the dirty flag is updated
    // 目標點雲和法向量可以在循環之前設好,source點雲和法向量則需要在每一個iteration都重設一遍
    correspondence_estimation_->setInputSource(input_transformed);
    if (correspondence_estimation_->requiresSourceNormals())
      correspondence_estimation_->setSourceNormals(input_transformed_blob);
1
2
3
4
5

尋找點對correspondences_，有兩種方法 - reciprocal或非reciprocal：

    // Estimate correspondences
    // 依據use_reciprocal_correspondence_決定要用determineCorrespondences還是determineReciprocalCorrespondences
    if (use_reciprocal_correspondence_)
      correspondence_estimation_->determineReciprocalCorrespondences(
          *correspondences_, corr_dist_threshold_);
    else
      correspondence_estimation_->determineCorrespondences(*correspondences_,
                                                           corr_dist_threshold_);
1
2
3
4
5
6
7
8

使用correspondence_rejectors_裡的多個rejector拒絕掉不合理的點對：

    // if (correspondence_rejectors_.empty ())
    CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_));
    
    //多個rejector連環reject?
    for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
      registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
      PCL_DEBUG("Applying a correspondence rejector method: %s.\n",
                rej->getClassName().c_str());
1
2
3
4
5
6
7
8

設定點對拒絕器的source點雲及法向量：

      //rej:source點雲和法向量需要在每一個iteration都重設一遍
      if (rej->requiresSourcePoints())
        rej->setSourcePoints(input_transformed_blob);
      if (rej->requiresSourceNormals() && source_has_normals_)
        rej->setSourceNormals(input_transformed_blob);
1
2
3
4
5

拒絕temp_correspondences中不合理的點對，得到correspondences_：

      rej->setInputCorrespondences(temp_correspondences);
      rej->getCorrespondences(*correspondences_);
1
2

temp_correspondences將變為下一次迭代中的輸入點對：

      // 為下一個rejector準備輸入的temp_correspondences
      // 那為何不把rej的輸入和輸出都設為correspondences_就好?
      // Modify input for the next iteration
      if (i < correspondence_rejectors_.size() - 1)
        *temp_correspondences = *correspondences_;
    }
1
2
3
4
5
6

至此correspondences_表示通過所有點對拒絕器考驗的點對。

如果點對數量過少，則認為算法無法收斂並退出：

    std::size_t cnt = correspondences_->size();
    // Check whether we have enough correspondences
    if (static_cast<int>(cnt) < min_number_correspondences_) {
      PCL_ERROR("[pcl::%s::computeTransformation] Not enough correspondences found. "
                "Relax your threshold parameters.\n",
                getClassName().c_str());
      convergence_criteria_->setConvergenceState(
          pcl::registration::DefaultConvergenceCriteria<
              Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
      converged_ = false;
      break;
    }
1
2
3
4
5
6
7
8
9
10
11
12

estimateRigidTransformation函數用於估計轉換矩陣，它也是ICP算法的核心所在：

    // Estimate the transform
    // input_transformed:經過guess和前面幾次算出來的轉換矩陣轉換過的source點雲
    // correspondences_:僅使用這些點對進行轉換矩陣的估計?
    transformation_estimation_->estimateRigidTransformation(
        *input_transformed, *target_, *correspondences_, transformation_);
1
2
3
4
5

利用當次估計出來的transformation_更新input_transformed及final_transformation_：

    //使用當次算出來的轉換矩陣transformation_對input_transformed做轉換
    // Transform the data
    transformCloud(*input_transformed, *input_transformed, transformation_);

    //然後將transformation_累乘到final_transformation_上
    // Obtain the final transformation
    final_transformation_ = transformation_ * final_transformation_;
1
2
3
4
5
6
7

遞增迭代次數：

    ++nr_iterations_;
1

每次迭代結束後，設定converged_成員變數：

    // Update the vizualization of icp convergence
    // if (update_visualizer_ != 0)
    //  update_visualizer_(output, source_indices_good, *target_, target_indices_good );

    // 為什麼可以直接轉?
    converged_ = static_cast<bool>((*convergence_criteria_));
1
2
3
4
5
6

判斷算法是否收斂，如果尚未收斂，則繼續迭代過程：

  } while (convergence_criteria_->getConvergenceState() ==
           pcl::registration::DefaultConvergenceCriteria<
               Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
1
2
3

輸出final_transformation_矩陣：

  // Transform the input cloud using the final transformation
  PCL_DEBUG("Transformation "
            "is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%"
            "5f\t%5f\t%5f\t%5f\n",
            final_transformation_(0, 0),
            final_transformation_(0, 1),
            final_transformation_(0, 2),
            final_transformation_(0, 3),
            final_transformation_(1, 0),
            final_transformation_(1, 1),
            final_transformation_(1, 2),
            final_transformation_(1, 3),
            final_transformation_(2, 0),
            final_transformation_(2, 1),
            final_transformation_(2, 2),
            final_transformation_(2, 3),
            final_transformation_(3, 0),
            final_transformation_(3, 1),
            final_transformation_(3, 2),
            final_transformation_(3, 3));
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

將input_使用final_transformation_做轉換，把結果存到output中。

  // Copy all the values
  // input_是PointCloudConstPtr,是指標
  // output是PointCloudSource
  // 既然有下面的transformCloud,那這句的意義何在?應該是複製xyz,normal以外的欄位?
  output = *input_;
  // Transform the XYZ + normals
  transformCloud(*input_, output, final_transformation_);
}
1
2
3
4
5
6
7
8