PCL - ICP代碼研讀(六 ) - IterativeClosestPoint架構
-
- 前言
- using
- public成員函數
-
- constructor和destructor
- copy constructor
- destructor
- getConvergeCriteria
- setInputSource
- setInputTarget
- use_reciprocal_correspondence_的setter和getter
- protected成員函數
- protected成員變數
前言
icp.h
裡宣告了
IterativeClosestPoint
和
IterativeClosestPointWithNormals
兩個類別,他們兩個都是
Registration
的子類別。
本篇只對
IterativeClosestPoint
類別著墨。
using
template <typename PointSource, typename PointTarget, typename Scalar = float>
class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar> {
public:
用
using
來定義名稱,方便後續使用。
using PointCloudSource =
typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
using PointCloudTarget =
typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
using PointIndicesPtr = PointIndices::Ptr;
using PointIndicesConstPtr = PointIndices::ConstPtr;
using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
using ConstPtr =
shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
沿用
Registration
中的定義,同時將原來是
Registration
的
protected
成員的導入
IterativeClosestPoint
的
public
權限區塊。
using Registration<PointSource, PointTarget, Scalar>::reg_name_;
using Registration<PointSource, PointTarget, Scalar>::getClassName;
using Registration<PointSource, PointTarget, Scalar>::input_;
using Registration<PointSource, PointTarget, Scalar>::indices_;
using Registration<PointSource, PointTarget, Scalar>::target_;
using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
using Registration<PointSource, PointTarget, Scalar>::transformation_;
using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
using Registration<PointSource, PointTarget, Scalar>::
transformation_rotation_epsilon_;
using Registration<PointSource, PointTarget, Scalar>::converged_;
using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
using Registration<PointSource, PointTarget, Scalar>::min_number_correspondences_;
using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
using Registration<PointSource, PointTarget, Scalar>::euclidean_fitness_epsilon_;
using Registration<PointSource, PointTarget, Scalar>::correspondences_;
using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_;
using Registration<PointSource, PointTarget, Scalar>::correspondence_estimation_;
using Registration<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
定義一個public成員變數
convergence_criteria_
,表示當前算法收斂的情況:
typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
convergence_criteria_;
沿用
Registration
中定義好的類別名稱
Matrix4
:
// 怎麼不寫using Registration<PointSource, PointTarget, Scalar>::Matrix4;?
using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;
public成員函數
constructor和destructor
/** \brief Empty constructor. */
IterativeClosestPoint()
: x_idx_offset_(0)
, y_idx_offset_(0)
, z_idx_offset_(0)
, nx_idx_offset_(0)
, ny_idx_offset_(0)
, nz_idx_offset_(0)
, use_reciprocal_correspondence_(false)
, source_has_normals_(false)
, target_has_normals_(false)
{
reg_name_ = "IterativeClosestPoint";
transformation_estimation_.reset(
new pcl::registration::
TransformationEstimationSVD<PointSource, PointTarget, Scalar>());
correspondence_estimation_.reset(
new pcl::registration::
CorrespondenceEstimation<PointSource, PointTarget, Scalar>);
convergence_criteria_.reset(
new pcl::registration::DefaultConvergenceCriteria<Scalar>(
nr_iterations_, transformation_, *correspondences_));
};
其中
transformation_estimation_
,
correspondence_estimation_
及
convergence_criteria_
皆是shared_ptr,調用
std::shared_ptr::reset
來替換他們所管理的物件。
注:
shared_ptr
相關的知識,可以通過C++ std::shared_ptr 用法與範例學習一下
copy constructor
目前
IterativeClosestPoint
的copy constructor是處於禁用的狀態:
/**
* \brief Due to `convergence_criteria_` holding references to the class members,
* it is tricky to correctly implement its copy and move operations correctly. This
* can result in subtle bugs and to prevent them, these operations for ICP have
* been disabled.
*
* \todo: remove deleted ctors and assignments operations after resolving the issue
*/
// 複製,移動IterativeClosestPoint會造成問題,所以目前先禁止使用
IterativeClosestPoint(const IterativeClosestPoint&) = delete;
IterativeClosestPoint(IterativeClosestPoint&&) = delete;
IterativeClosestPoint&
operator=(const IterativeClosestPoint&) = delete;
IterativeClosestPoint&
operator=(IterativeClosestPoint&&) = delete;
destructor
/** \brief Empty destructor */
~IterativeClosestPoint() {}
getConvergeCriteria
public成員變數
convergence_criteria_
的getter:
//注釋?
/** \brief Returns a pointer to the DefaultConvergenceCriteria used by the
* IterativeClosestPoint class. This allows to check the convergence state after the
* align() method as well as to configure DefaultConvergenceCriteria's parameters not
* available through the ICP API before the align() method is called. Please note that
* the align method sets max_iterations_, euclidean_fitness_epsilon_ and
* transformation_epsilon_ and therefore overrides the default / set values of the
* DefaultConvergenceCriteria instance. \return Pointer to the IterativeClosestPoint's
* DefaultConvergenceCriteria.
*/
inline typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
getConvergeCriteria()
{
return convergence_criteria_;
}
setInputSource
設定好source點雲本身,XYZ坐標偏移量
x_idx_offset_
,
y_idx_offset_
,
z_idx_offset_
,法向量XYZ坐標偏移量
nx_idx_offset_
,
ny_idx_offset_
,
nz_idx_offset_
及
source_has_normals_
。這些變數在
transformCloud
函數中會用到。
/**
* 設定好source點雲及x_idx_offset_,y_idx_offset_,z_idx_offset_,
* nx_idx_offset_,ny_idx_offset_,nz_idx_offset_及source_has_normals_
**/
// 這個函數在registration.h中是virtual的
/** \brief Provide a pointer to the input source
* (e.g., the point cloud that we want to align to the target)
*
* \param[in] cloud the input point cloud source
*/
void
setInputSource(const PointCloudSourceConstPtr& cloud) override
{
Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
const auto fields = pcl::getFields<PointSource>();
source_has_normals_ = false;
for (const auto& field : fields) {
if (field.name == "x")
x_idx_offset_ = field.offset;
else if (field.name == "y")
y_idx_offset_ = field.offset;
else if (field.name == "z")
z_idx_offset_ = field.offset;
else if (field.name == "normal_x") {
source_has_normals_ = true;
nx_idx_offset_ = field.offset;
}
else if (field.name == "normal_y") {
source_has_normals_ = true;
ny_idx_offset_ = field.offset;
}
else if (field.name == "normal_z") {
source_has_normals_ = true;
nz_idx_offset_ = field.offset;
}
}
}
setInputTarget
設定好target點雲本身及
target_has_normals_
。
target_has_normals_
在
transformCloud
函數中會用到。
//設定好target點雲及target_has_normals_
//沒有x_idx_offset_,nx_idx_offset_之類的?
// 這個函數在registration.h中是virtual的
/** \brief Provide a pointer to the input target
* (e.g., the point cloud that we want to align to the target)
*
* \param[in] cloud the input point cloud target
*/
void
setInputTarget(const PointCloudTargetConstPtr& cloud) override
{
Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
const auto fields = pcl::getFields<PointSource>();
target_has_normals_ = false;
for (const auto& field : fields) {
if (field.name == "normal_x" || field.name == "normal_y" ||
field.name == "normal_z") {
target_has_normals_ = true;
break;
}
}
}
use_reciprocal_correspondence_的setter和getter
/** \brief Set whether to use reciprocal correspondence or not
*
* \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence
* or not
*/
inline void
setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)
{
use_reciprocal_correspondence_ = use_reciprocal_correspondence;
}
/** \brief Obtain whether reciprocal correspondence are used or not */
inline bool
getUseReciprocalCorrespondences() const
{
return (use_reciprocal_correspondence_);
}
protected成員函數
transformCloud
函數用於對輸入點雲的XYZ坐標及法向量做剛體變換:
//對輸入點雲的XYZ坐標及法向量做剛體變換
/** \brief Apply a rigid transform to a given dataset. Here we check whether
* the dataset has surface normals in addition to XYZ, and rotate normals as well.
* \param[in] input the input point cloud
* \param[out] output the resultant output point cloud
* \param[in] transform a 4x4 rigid transformation
* \note Can be used with cloud_in equal to cloud_out
*/
virtual void
transformCloud(const PointCloudSource& input,
PointCloudSource& output,
const Matrix4& transform);
computeTransformation
函數是ICP算法的核心所在:
//計算輸入到輸出點雲的轉換,然後將output設為轉換後的點雲
/** \brief Rigid transformation computation method with initial guess.
* \param output the transformed input point cloud dataset using the rigid
* transformation found \param guess the initial guess of the transformation to
* compute
*/
void
computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
determineRequiredBlobData
是一個輔助函數,用於決定是否需要將輸入的
pcl::PointCloud
轉換為
pcl::PCLPointCloud2
:
//?
/** \brief Looks at the Estimators and Rejectors and determines whether their
* blob-setter methods need to be called */
virtual void
determineRequiredBlobData();
protected成員變數
在做校正算法時,輸入及輸出的點雲類型可能不同,底層的數據結構也不盡相同,所以在實際處理點雲內部的數據時(如
transformCloud
函數),需要知道XYZ坐標分別位於一個點的什麼位置。另外如果該點包含法向量,還需要知道該點法向量的XYZ坐標相對於點起始位置的偏移量。
以下六個成員變數便是用來表示XYZ和法向量XYZ的offset,在
setInputSource
函數中會對他們進行初始化:
//屬於source點雲
/** \brief XYZ fields offset. */
std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_;
//屬於source點雲
/** \brief Normal fields offset. */
std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_;
source點雲和target點雲可能帶有法向量也可能沒有,這兩個成員變數分別記錄兩點雲是否帶有法向量。主要在
transformCloud
函數中發揮作用。其中
source_has_normals_
在
setInputSource
函數中被初始化,
target_has_normals_
在
setInputTarget
函數中被初始化:
/** \brief Internal check whether source dataset has normals or not. */
bool source_has_normals_;
/** \brief Internal check whether target dataset has normals or not. */
bool target_has_normals_;
校正算法輸入的點雲是
pcl::PointCloud
類型的,但是其內部所用到的
pcl::registration::CorrespondenceEstimationBase::setSourceNormals
,
pcl::registration::CorrespondenceRejector::setSourcePoints
和
pcl::registration::CorrespondenceRejector::setSourceNormals
函數(對應的target版本為
pcl::registration::CorrespondenceEstimationBase::setTargetNormals
,
pcl::registration::CorrespondenceRejector::setTargetPoints
和
pcl::registration::CorrespondenceRejector::setTargetNormals
函數)接受的卻是
pcl::PCLPointCloud2
類型的點雲。
在
determineRequiredBlobData
成員函數中,會判定是否需要用到上述的幾個函數,如果需要,
need_source_blob_
或/和
need_target_blob_
就會被設為true。然後在
computeTransformation
函數中依據這兩個flag決定是否需要執行
pcl::toPCLPointCloud2
。
//?
/** \brief Checks for whether estimators and rejectors need various data */
bool need_source_blob_, need_target_blob_;
在ICP算法的第一步是尋找點對,依據判斷標準可分為以下兩種:
- 目標點雲中的點B是原始點雲中點A在目標點雲中的最近鄰
- 目標點雲中的點B是原始點雲中點A在目標點雲中的最近鄰,同時A也是B在原始點雲中的最近鄰
可以看到第二個標準是較為嚴格的。這個flag便是用來選擇該採用上述標準中的哪一種:
/** \brief The correspondence type used for correspondence estimation. */
bool use_reciprocal_correspondence_;