PCL - ICP代碼研讀（九 ） - computeTransform函數
-
- 前言
- computeTransform
前言
computeTransform
函數實現了ICP算法，是
icp.hpp
的精髓所在。
回顧ICP（Iterative Closest Point）算法推導中提到的ICP算法三步驟，第一步是在兩點雲間尋找點對，然後拒絕其中不合理的點對；第二步是估計變換矩陣；第三步是判斷是否收斂。
這三步分別對應到以下物件：
- 點對估計器
correspondence_estimation_
correspondence_rejectors_
- 轉換矩陣估計器
transformation_estimation_
- 收斂條件
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)
{
初始化：
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;
將
guess
累乘到
final_transformation_
上：
// Initialise final transformation to the guessed one
final_transformation_ = guess;
在此使用
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_;
因為
transformation_
表示當次估計出來的轉換矩陣，所以這邊不將它設為
guess
，而是設為單位矩陣：
determineRequiredBlobData
函數用於決定是否需要執行
pcl::toPCLPointCloud2
，詳見PCL - ICP代碼研讀（七 ） - determineRequiredBlobData函數：
// Make blobs if necessary
// ?
determineRequiredBlobData();
如果
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);
設定點對估計器的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);
設定（多個）點對拒絕器的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);
}
設定收斂條件，這裡的各種參數將留到後續章節繼續介紹：
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_);
以下過程會迭代進行直到確認為收斂或是發散為止：
// Repeat until convergence
do {
將
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);
}
接下來會更新
transformation_
，在此先儲存
previous_transformation_
：
// Save the previously estimated transformation
previous_transformation_ = transformation_;
設定點對估計器的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);
尋找點對
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_);
使用
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());
設定點對拒絕器的source點雲及法向量：
//rej:source點雲和法向量需要在每一個iteration都重設一遍
if (rej->requiresSourcePoints())
rej->setSourcePoints(input_transformed_blob);
if (rej->requiresSourceNormals() && source_has_normals_)
rej->setSourceNormals(input_transformed_blob);
拒絕
temp_correspondences
中不合理的點對，得到
correspondences_
：
rej->setInputCorrespondences(temp_correspondences);
rej->getCorrespondences(*correspondences_);
temp_correspondences
將變為下一次迭代中的輸入點對：
// 為下一個rejector準備輸入的temp_correspondences
// 那為何不把rej的輸入和輸出都設為correspondences_就好?
// Modify input for the next iteration
if (i < correspondence_rejectors_.size() - 1)
*temp_correspondences = *correspondences_;
}
至此
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;
}
estimateRigidTransformation
函數用於估計轉換矩陣，它也是ICP算法的核心所在：
// Estimate the transform
// input_transformed:經過guess和前面幾次算出來的轉換矩陣轉換過的source點雲
// correspondences_:僅使用這些點對進行轉換矩陣的估計?
transformation_estimation_->estimateRigidTransformation(
*input_transformed, *target_, *correspondences_, transformation_);
利用當次估計出來的
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_;
遞增迭代次數：
每次迭代結束後，設定
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_));
判斷算法是否收斂，如果尚未收斂，則繼續迭代過程：
} while (convergence_criteria_->getConvergenceState() ==
pcl::registration::DefaultConvergenceCriteria<
Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
輸出
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));
將
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_);
}
![C语言第四章自述2第四章 选择结构程序设计[图]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)