PCL - ICP代碼研讀（五 ） - align函數

前言

接續PCL - ICP代碼研讀（二 ） - Registration架構，本篇主要介紹Registration類別的align函數。

computeTransformation這個純虛擬函數用於估計兩點雲間的轉矩矩陣(final_transformation_)，並同時對輸入點雲input_做轉換，將結果儲存至output這個點雲中，可以說是校正算法的核心。

align函數則是它的wrapper，為computeTransformation函數做好初始化後，再呼叫該函數。

align函數有兩個版本，一個需要傳入4 * 4轉換矩陣作為初始解，另一個則假設初始解為單位矩陣，不需要傳入4 * 4矩陣。

align

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output,
                                                      const Matrix4& guess)
{

先透過initCompute函數設定好tree_及correspondence_estimation_。

  if (!initCompute())
    return;

準備好output：

  // Resize the output dataset
  output.resize(indices_->size());
  // Copy the header
  output.header = input_->header;
  // Check if the output will be computed for all points or only a subset
  if (indices_->size() != input_->size()) {
    // 只對輸入點雲中索引為indices_的點做align
    output.width = indices_->size();
    // 為何不是output.height = input_->height?
    output.height = 1;
  }
  else {
    output.width = static_cast<std::uint32_t>(input_->width);
    output.height = input_->height;
  }
  output.is_dense = input_->is_dense;

  // Copy the point data to output
  for (std::size_t i = 0; i < indices_->size(); ++i)
    output[i] = (*input_)[(*indices_)[i]];

目前與point_representation_相關的代碼還沒看明白。

  // ?
  // Set the internal point representation of choice unless otherwise noted
  if (point_representation_ && !force_no_recompute_)
    tree_->setPointRepresentation(point_representation_);

converged_，final_transformation_，transformation_及previous_transformation_這幾個protected成員變數在computeTransformation中會用到，這裡先將他們初始化。

  // Perform the actual transformation computation
  converged_ = false;
  final_transformation_ = transformation_ = previous_transformation_ =
      Matrix4::Identity();

computeTransformation函數中用到的轉換矩陣是4*4的，這裡用齊次法表示output，接下來就可以直接用一次矩陣乘法來做旋轉與平移。

  // Right before we estimate the transformation, we set all the point.data[3] values to
  // 1 to aid the rigid transformation
  // 齊次坐標的意思?
  for (std::size_t i = 0; i < indices_->size(); ++i)
    output[i].data[3] = 1.0;

計算final_transformation_並更新output：

  // 注意align函數會修改入參output
  computeTransformation(output, guess);

PCLBase::deinitCompute函數為空：

  deinitCompute();
}

align wrapper

這個函數與align(PointCloudSource& output, const Matrix4& guess)比起來少了一個參數guess。它是該函數的wrapper，默認guess為單位矩陣。

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output)
{
  align(output, Matrix4::Identity());
}