pcl库中ICP算法是通过icp.h、icp.hpp两个文件实现的,icp.h为声明文件,icp.hpp是函数的具体实现文件。本文将解析ICP算法的源代码逻辑实现,ICP算法本身的原理请自行阅读相关文献。
由于算法实现的源代码继承于Registration类,所以阅读本部分前请先阅读Registration类的源代码解读:PCL库—配准模块(Registration)源代码阅读—Registration类
文中有任何错误或疑惑的地方可在评论区留言,看到均会回复!!!
icp.h文件
IterativeClosestPoint类继承于Registration类,其实现了经典的ICP算法,变换矩阵的求解基于SVD方法,算法终止的条件为:最大迭代次数,变换矩阵的变化值,点云欧氏距离误差。
class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>
{...}
该类引用了 Registration类中大量的变量,可直接在本类中使用。
public:
using Registration<PointSource, PointTarget, Scalar>::reg_name_;
using Registration<PointSource, PointTarget, Scalar>::getClassName;
using... 引用各种变量,方便本类直接使用
构造函数及初始化列表,此处x_idx_offset_这些变量与字段偏移量有关,无需特别关注。
reset函数是C++智能指针的一个函数,用来重新指向一个新对象并释放原有指向对象的内存。
IterativeClosestPoint():… 构造函数及初始化列表
{
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_));
};
设置源点云和目标点云是通过 Registration类的相应函数进行设置。此外,本文为了快速阅读,简化了函数的书写方式,不常用的函数本部分将略去。
setInputSource(& ) override; 设置源点云,扩展了Registration类的功能
setInputTarget(& ) override; 设置目标点云
setUseReciprocalCorrespondences(bool); 设置是否使用相互对应关系
transformCloud()函数是icp算法中的内部函数,实现了点云在变换矩阵下变换的功能。
protected:
transformCloud(& ,&, & ); 将点云在变换矩阵T下变换,输出点云结果
computeTransformation()函数是icp算法中的核心函数,它实现了经典ICP算法计算变换矩阵,具体的代码逻辑见下部分。
void computeTransformation(,) override; 实现ICP算法的变换矩阵求解
std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_;定义XYZ字段信息的偏移量
std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_;定义法向量字段信息的偏移量
bool use_reciprocal_correspondence_;定义对应类型是否是相互对应关系
bool source_has_normals_; 定义源点云是否有法向量
bool target_has_normals_; 定义目标点云是否有法向量
IterativeClosestPointWithNormals类继承于IterativeClosestPoint类,它是IterativeClosestPoint的一种特例,默认情况下基于点到面的距离进行变换矩阵的估计。本文不研究点到面的ICP变体算法,故不做过多介绍。
class IterativeClosestPointWithNormals
: public IterativeClosestPoint<PointSource, PointTarget, Scalar>
{...}
icp.hpp文件
本文件中的computeTransformation函数实现了经典ICP算法的变换矩阵估计,因此本部分重点介绍computeTransformation函数的实现逻辑。output和guess参数的含义请参考Registration类中的align函数。
template <typename PointSource, typename PointTarget, typename Scalar>
void
IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation(
PointCloudSource& output, const Matrix4& guess)
{...}
下面将对 computeTransformation函数的内部核心部分进行讲解,非重要部分将简略带过。
input_transformed的作用是计算变换矩阵,如果直接使用input_计算变换矩阵的话会改变用户输入的源点云数据。
PointCloudSourcePtr input_transformed(new PointCloudSource);定义了点云数据指针变量,用来计算变换矩阵
nr_iterations_ = 0; 当前迭代次数设为0
converged_ = false; 收敛状态为假
final_transformation_ = guess; 最终变换矩阵初值设为guess
/ guess是否等于单位矩阵 /
if (guess != Matrix4::Identity()) {
input_transformed->resize(input_->size());
transformCloud(*input_, *input_transformed, guess);
}
else
*input_transformed = *input_;
transformation_ = Matrix4::Identity(); 当前变换矩阵设为单位矩阵
PCLPointCloud2是一种特殊的数据类型,使用频率很低,无需特别关注。
最大迭代次数,点云间欧氏距离误差阈值,平移矩阵变化阈值,旋转矩阵变化阈值参数最终是通过收敛标准对象convergence_criteria_来判断是否收敛的,具体的代码阅读可参考PCL库中的ConvergenceCriteria模块:PCL库源代码阅读—配准模块(Registration)—收敛标准(ConvergenceCriteria)
对应关系的确定可参考:PCL库源代码阅读—配准模块(Registration)—对应关系模块(Correspondence)
correspondence_estimation_->setInputTarget(target_);为对应关系估计对象设置目标点云
/ 对应关系估计需要法向量的情况 /
if (correspondence_estimation_->requiresTargetNormals())
correspondence_estimation_->setTargetNormals(target_blob);
/ 对应关系拒绝的情况 无需特别关注 /
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_); 设置最大迭代次数
convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_);设置点云间欧式距离误差阈值
convergence_criteria_->setTranslationThreshold(transformation_epsilon_);设置平移矩阵变化平方阈值
/ 设置旋转矩阵变化阈值 /
if (transformation_rotation_epsilon_ > 0)
convergence_criteria_->setRotationThreshold(transformation_rotation_epsilon_);
else
convergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);
do-while()循环实现了变换矩阵的迭代求解
do
{...}while();
本部分将介绍do-while循环中是如何实现变换矩阵的迭代求解,由于PCLPointCloud2的数据类型不常见,对应关系拒绝器correspondence_rejectors_与经典ICP算法关系不大,故无需关注与此相关的代码。
previous_transformation_ = transformation_;将当前变换矩阵赋给前一次变换矩阵
correspondence_estimation_->setInputSource(input_transformed);为对应关系估计对象设置源点云
if (correspondence_estimation_->requiresSourceNormals())
correspondence_estimation_->setSourceNormals(input_transformed_blob);
/ 相互对应关系的情况 /
if (use_reciprocal_correspondence_)
correspondence_estimation_->determineReciprocalCorrespondences(
*correspondences_, corr_dist_threshold_); 确定对应关系,具体可参考Correspondence模块讲解
/ 普通的对应关系情况 /
else
correspondence_estimation_->determineCorrespondences(*correspondences_,
corr_dist_threshold_);
对应关系中的点对数量小于阈值时,不收敛并跳出循环。
if (correspondences_->size() < min_number_correspondences_) {
...
convergence_criteria_->setConvergenceState(
pcl::registration::DefaultConvergenceCriteria<
Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
converged_ = false;
break;
}
变换矩阵估计的关键函数estimateRigidTransformation,方法为SVD。
/ 变换矩阵的估计 得到最新的transformation_ /
transformation_estimation_->estimateRigidTransformation(
*input_transformed, *target_, *correspondences_, transformation_);
/ 更新input_transformed中的点云 /
transformCloud(*input_transformed, *input_transformed, transformation_);
/ 更新最终变换矩阵 /
final_transformation_ = transformation_ * final_transformation_;
++nr_iterations_; 当前迭代次数+1
更新收敛状态,此处用到了C++中的operator类型转换语法,将类对象隐式转换为bool类型,具体逻辑可以参考ConvergenceCriteria模块讲解。
converged_ = static_cast<bool>((*convergence_criteria_));每次迭代更新收敛状态
判断是否达到了收敛的条件。
while (convergence_criteria_->getConvergenceState() ==
pcl::registration::DefaultConvergenceCriteria<
Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
迭代求解出最终变换矩阵后计算得到变换后的源点云数据,并存储于output中。
output = *input_;
// Transform the XYZ + normals
transformCloud(*input_, output, final_transformation_);
总结
IterativeClosestPoint类是PCL库中用来实现经典ICP算法的类,它实现了源点云、目标点云的设置,并通过computeTransformation函数实现了变换矩阵的估计。
computeTransformation函数实现了迭代求解最优变换矩阵,通过correspondence_estimation_估计对应关系,通过convergence_criteria_判断收敛,通过transformation_estimation_求解变换矩阵。收敛条件的参数设置在do-while迭代求解前已通过convergence_criteria_->进行设置。
每次迭代时correspondence_estimation_->determineCorrespondences函数均需要更新对应点对关系。