PCL库源代码阅读—配准模块(Registration)—ICP算法

于 2024-05-23 19:45:00 发布
阅读量742 收藏 27
点赞数 13
文章标签： 人工智能 自动驾驶
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/mazheng11111111/article/details/138394629
版权

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函数均需要更新对应点对关系。

想躺平的点云工程师
关注
  • 13
    点赞
  • 踩
  • 27
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
pcl-registration:使用PCL注册点云
05-13
pcl注册 该程序包将两个点云作为输入参数，并尝试使用特征匹配来对齐它们。 运行可执行文件的语法是 ./pipeline <source> <target> 首先，使用PCL正常估算对象估算每个点云的法线。 然后，使用这些法线作为输入，使用PCL SIFT关键点对象来计算每个输入云的SIFT关键点。 PCL FPFH估计对象用于估计两个云的关键点处的FPFH功能。 请注意，必须将搜索表面设置为原始点云，但是将输入云设置为关键点云。 估计关键点和功能后，它们将用作computeInitialAlignment()方法的输入，该方法在include/pipeline.hpp文件中定义。 此方法使用PCL样本共识初始对准对象返回4 x 4转换矩阵。 最后，将此矩阵应用于源云以使其与目标云对齐。 该程序将打开带有两个视口的PCL可视化器。 左视口显示两个输入云及其SIFT关
4pcs算法在pcl上的实现完整工程
12-26
在提供的"4pcs_registration_carlos202012"压缩包中，包含了完整的4PCS算法实现工程，包括源代码、数据集和可能的输出结果。通过分析和运行这个工程，我们可以深入理解4PCS算法的每个步骤，并学习如何在实际项目中...
【点云配准】迭代最近点(ICP)配准算法
点云兔子的博客
09-23 3011
点云配准（Point Cloud Registration）指的是输入两幅点云(source)和(target) ，输出一个变换T使得T(Ps)和T(Pt)的重合程度尽可能高；Ps和Pt是源点云和目标点云中的对应点。变换T可以是刚性的(rigid)，也可以不是，一般只考虑刚性变换，即变换只包括旋转、平移。点云配准可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。
ICP介绍
liliemily的专栏
11-01 1317
转：如何用PCL做点云之间的匹配   2013-08-19 07:16:35|  分类： 默认分类 |  标签： |字号大中小 订阅 http://blog.csdn.net/u010696366/article/details/8941938 PCL Registration API Registration：不断调整，把不同角度的3D
PCL - 3D点云配准(registration)介绍
jiugeshao的专栏
11-05 1893
代码会加载5张点云数据，A,B,C,D,E分别代表这5个点云，然后AB,BC,CD,DE分别两两配准，这样就可以把B,C,D,E都变换到A空间中去， 向A对齐，完毕后将这5个点云数据做融合（相加），就这样这，可以实现一个拼接或者融合的功能，后续任务就可以基于这份包含更细致信息的点云做文章。运行效果如下（左边显示的是对齐后的点云和目标点云，右边是源点云和目标点云），左边两组点云的一致性很好，而右边两组由于未对齐，可看到是错乱分布的。这里需要清楚的一点，是将源点云向目标点云对齐，让其变换到目标点云的样子。
（一）PCL库 ICP配准
伍心的专栏
08-31 1113
简介：通过ICP配准，对齐CBCT提取牙齿与口内扫描获取牙冠，结果失败。原因：可能是因为两者重叠点云数据少，而不重叠部分过多，待后续分析。 1、CBCT提取牙齿 与口内扫描获取数据在同坐标系下显示 2、PCL库调用ICP进行配准 void registCTInteroral() { char strST[256] = "E:\\CBCT\\lower.ply"; char strCT[256] = "E:\\CBCT\\lower-CT.ply"; pcl::Po.
pcl点云库(1):迭代最近点算法(icp)算法实践及解析
biter0088的博客
07-23 3797
本次icp匹配效果不好，主要因为icp是一种贪心算法，这里可能陷入局部最优----解决办法一般是先进行比较好的粗匹配。彩色是输入点云即待匹配点云、白色是目标点云、红色是进过icp匹配后的点云。（注基于ubuntu、ros的c++代码）..................
3d-ndt算法在pcl上的实现完整项目文件
12-26
2. 源代码：可能是一个完整的C++项目，包括NDT配准的主要函数，如初始化、迭代优化和结果输出。 3. 配置文件：可能包含了用于设置NDT参数的文件，如细胞大小、最大迭代次数、收敛阈值等。 4. 可执行文件：编译后的...
4PCS配准算法实现.zip
08-26
4PCS目录可能包含了源代码、头文件和其他支持文件，这些文件通常会包含实现K4PCS算法的具体函数和类，以及与PCL库的接口。x64目录则表明这是为64位操作系统编译的版本。 理解并实现K4PCS算法需要掌握以下几个关键...
NDT.zip_8MF_NDT点云配准_NDT算法_ndt配准 c++_点云ndt
09-21
对于NDT算法，你需要包含必要的库，比如PCL（Point Cloud Library），这是一个强大的开源点云处理库，内置了NDT模块。 在PCL中，NDT配准主要通过`pcl::registration::NDT`类来实现。使用时，你需要创建一个NDT对象...
PCL点云库IterativeClosestPoint用法demo
07-30
首先，`icp.cpp`是实现ICP算法的源代码文件。在VS2010中，你可以使用C++语言编写并编译这个程序。`CMakeLists.txt`是CMake构建系统配置文件，用于指导编译过程，确保所有依赖项正确链接。 1. **PCL ICP算法**：ICP...
PCL库源代码阅读—配准模块(Registration)—Registration类
最新发布
不知名的代码小白
05-22 1750
Registration类是PCL库中用来配准的基类，它建立了配准的框架。两点云配准所需的源点云、目标点云源的设置，用来判断是否收敛的各种阈值参数的设置都是在该类中实现。而变换矩阵的迭代求解则通过computeTransformation()纯虚函数让子类进行重载，以实现具体的配准算法。
PCL - ICP代碼研讀（三 ） - Registration初始化
keineahnung2345的博客
10-03 782
接續PCL - ICP代碼研讀（二 ） - Registration架構，這邊繼續來看Registration成員函數的實現。本篇主要關注的是registration/include/pcl/registration/impl/registration.hpp這份文件中與初始化相關的函數。
PCL学习九：Registration-配准
qq_42994487的博客
05-10 750
PCL学习九：Registration-配准
【PCL自学：Registration 1】PCL点云配准一般流程介绍
斯坦福的兔子的博客
03-13 4234
PCL点云配准一般流程
PCL点云库：ICP算法原理及应用
weixin_44849088的博客
12-03 2321
ICP算法 在点云数据处理过程中，往往需要将不同视角即不同参考坐标下的两组或者多组点云统一到统一坐标系下，进行点云的配准。在配准算法中，使用最多的是ICP算法。 基本思想： 将目标点云P与源点云Q进行比较，匹配得到最接近的点云对。 对这些点云对进行相关运算得到误差函数，并计算使得误差函数最小的参数R，t。其中R 为旋转矩阵，t为平移向量。 在此基础上，使用上一步求得的旋转矩阵R和平移矩阵 t...
PCL之——点云配准算法ICP
热门推荐
qq_34782535的博客
01-04 1万+
PCL之——点云配准算法ICP一、ICP介绍：1.ICP用途2.ICP原理3.PCL的ICP实现二、ICP使用：1.ICP配准步骤2.官方例程——采坑 一、ICP介绍： 1.ICP用途 ICP原名IterativeClosestPoint（迭代最近点算法），主要用于点云配准，找到两部分点云之间的刚体变换关系。可用于SLAM、位姿估计等 2.ICP原理 刚体变换不理解先看： https://blog...
pcl的ICP算法
baidu_16271159的博客
06-29 376
本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对，计算最优刚体变换这一过程，直到满足正确配准的收敛精度要求。ICP是一个广泛使用的配准算法，主要目的就是找到旋转和平移参数，将两个不同坐标系下的点云，以其中一个点云坐标系为全局坐标系，另一个点云经过旋转和平移后两组点云重合部分完全重叠。算法的输入：参考点云和目标点云，停止迭代的标准。算法的输出：旋转和平移矩阵，即转换矩阵。
python pcl库使用icp配准
07-28
Python pcl库可以使用ICP算法进行点云配准。ICP算法本质上是基于最小二乘法的最优配准方法，它通过选择对应关系点对，计算最优刚体变换的过程来实现配准。在Python pcl库中，可以使用`pcl.registration.ICP`类来进行ICP配准。 首先，需要导入相应的库和模块： ```python import pcl from pcl.registration import icp, TransformationEstimationPointToPlane ``` 然后，可以加载需要配准的点云数据： ```python cloud_source = pcl.load("source_cloud.pcd") cloud_target = pcl.load("target_cloud.pcd") ``` 接下来，创建一个ICP对象，并设置一些参数： ```python icp = icp.IterativeClosestPoint() icp.setMaximumIterations(50) # 设置最大迭代次数 icp.setTransformationEpsilon(1e-8) # 设置收敛精度 icp.setEuclideanFitnessEpsilon(1e-6) # 设置收敛条件 ``` 然后，可以进行配准： ```python icp.setInputSource(cloud_source) icp.setInputTarget(cloud_target) cloud_aligned = pcl.PointCloud() icp.align(cloud_aligned) ``` 最后，可以获取配准后的点云结果： ```python transformation_matrix = icp.getFinalTransformation() ``` 这样，就完成了使用Python pcl库进行ICP配准的过程。请注意，ICP算法的配准结果可能受到初始迭代值的影响，因此在实际应用中，需要根据具体情况选择合适的初始值来获得更好的配准结果。 #### 引用[.reference_title] - *1* *2* *3* [PCL中的点云ICP配准（附源代码和数据）](https://blog.csdn.net/qq_29462849/article/details/85080518)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v91^insert_down28v1,239^v3^insert_chatgpt"}} ] [.reference_item] [ .reference_list ]
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

想躺平的点云工程师

感谢各位看官！

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值