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

• • 前言
  • align
  • align wrapper

前言

接續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());
}