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

83 阅读 0 评论 55 点赞

我是靠谱客的博主欢喜书包，这篇文章主要介绍PCL - ICP代碼研讀（五） - align函數，现在分享给大家，希望可以做个参考。

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

复制代码

1
2
3
4
5
6
template <typename PointSource, typename PointTarget, typename Scalar>
inline void
Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output,
const Matrix4& guess)
{

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

复制代码

1
2
3
4

if (!initCompute())
return;

準備好output：

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

// 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_相關的代碼還沒看明白。

复制代码

1
2
3
4
5
6

// ?
// 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中會用到，這裡先將他們初始化。

复制代码

1
2
3
4
5
6

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

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

复制代码

1
2
3
4
5
6
7

// 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：

复制代码

1
2
3
4

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

PCLBase::deinitCompute函數為空：

复制代码

1
2
3
4

deinitCompute();
}

align wrapper

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

复制代码

1
2
3
4
5
6
7
template <typename PointSource, typename PointTarget, typename Scalar>
inline void
Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output)
{
align(output, Matrix4::Identity());
}