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

最新推荐文章于 2026-04-03 09:34:58 发布
原创 最新推荐文章于 2026-04-03 09:34:58 发布 · 1k 阅读 · 27 ·
本内容遵循CC 4.0 BY-SA版权协议
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
GEO检测
标签
#人工智能 #自动驾驶

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

PCL - 从源码构建开发环境: 编程实践
2301_79326930的博客
08-13 313
在计算机视觉和三维点云处理领域，点云库（Point Cloud Library，简称PCL）是一个强大且广泛使用的开源框架。通过以上步骤，你已经成功地从源码搭建了PCL的开发环境，并编写并运行了一个简单的示例程序。PCL提供了丰富的功能和算法，用于处理点云数据，包括特征提取、配准、分割等。你可以进一步探索PCL的文档和示例代码，以应用其强大的功能于你自己的项目中。访问PCL的官方GitHub仓库（https://github.com/PointCloudLibrary/pcl），选择并下载最新版本的源码。
【PCL自学：Registration 1】PCL点云配准一般流程介绍
斯坦福的兔子的博客
03-13 4831
PCL点云配准一般流程
VTK与PCL源码编译（Ubuntu 20.04.6）
最新发布
m0_74224591的博客
04-03 396
让make使用你CPU的所有核心并行编译，可以大大加快速度。你需要告诉你的 CMake 项目去哪里找 cuPCL 的库和头文件。：配置工具，它会检查你的系统环境，并生成Makefile。是 cmake 的交互式文本界面，你可以方便地开关选项。：更新系统的共享库缓存，让新安装的VTK库能被系统找到。输入命令后，终端会变成蓝色背景的界面，顶部显示各种选项。：开启Qt支持（PCL的可视化需要VTK的Qt模块）。文件中，添加以下内容，找到 cuPCL 并链接它。：编译为“发布”版本（优化后，运行更快）。
PCL: Registration模块之IterativeClosestPoint点云配准
宫保鸡丁
07-30 2万+
参考文献：         关于此类的详细信息和学术上的理论推导大家可以查看PaulJ. Besl的文章“A Method forRegistration of 3-D Shapes”。 小结： ICP匹配，中文应该叫临近点迭代吧，是计算机图形学中的一个非常有用的算法，尤其是在三维重建点云配准中有着极其重要的地位，此外在SLAM等移动机器人导航等领域也有着很大的用武之地
PCL函数库摘要——点云配准
qq_36829039的博客
11-07 4387
1.Class pcl::registration::CorrespondenceEstimation< PointSource, PointTarget, Scalar > 类CorrespondenceEstimation是确定目标和查询点集(或特征)之间的对应关系的基类，输入为目标和源点云，输出为点对，即输出两组点云之间对应点集合。 #include <pcl/registration/correspondence_estimation.h> CorrespondenceEst
PCL学习九：Registration-配准
qq_42994487的博客
05-10 1245
PCL学习九：Registration-配准
PCL库源代码阅读—配准模块(Registration)—GICP算法
不知名的代码小白
05-27 4299
pcl库中GICP算法是通过gicp.h、gicp.hpp两个文件实现的，gicp.h为声明文件，gicp.hpp文件是函数的具体实现文件，pcl库中实现的GICP算法是基于BFGS(拟牛顿法)优化方法。本文章将解析源代码的逻辑实现，GICP算法本身的原理请自行阅读相关文献。由于算法实现的源代码继承于ICP算法，所以阅读本部分前请先阅读ICP部分的源代码解读。
PCL库源代码阅读—配准模块(Registration)—收敛标准(ConvergenceCriteria)
不知名的代码小白
05-24 1402
pcl库中收敛条件的判断是通过ConvergenceCriteria模块的相关文件实现的，具体实现文件为convergence_criteria.h，default_convergence_criteria.h，default_convergence_criteria.hpp。 收敛标准模块为点云配准提供不同的收敛标准，该模块作为具体配准算法的一部分来使用，收敛标准主要为： 1、迭代次数达到阈值；2、变换矩阵的变化值小于阈值；3、均方误差(MSE)的变化值小于阈值；
PCL库源代码阅读—配准模块(Registration)—Registration类
不知名的代码小白
05-22 2275
Registration类是PCL库中用来配准的基类，它建立了配准的框架。两点云配准所需的源点云、目标点云源的设置，用来判断是否收敛的各种阈值参数的设置都是在该类中实现。而变换矩阵的迭代求解则通过computeTransformation()纯虚函数让子类进行重载，以实现具体的配准算法。
PCL库源代码阅读—配准模块(Registration)—变换估计(transformation_estimation)
不知名的代码小白
05-26 1161
pcl库中变换估计模块(transformation_estimation)用来计算两点云配准的变换矩阵，不同的原理对应不同的文件，本文主要围绕经典ICP算法介绍相关的变换估计文件，具体为：transformation_estimation.h，transformation_estimation_svd.h，transformation_estimation_svd.hpp。其中，h文件是声明文件，hpp文件是代码的具体实现文件。
PCL - 3D点云配准(registration)介绍
jiugeshao的专栏
11-05 2330
代码会加载5张点云数据，A,B,C,D,E分别代表这5个点云，然后AB,BC,CD,DE分别两两配准，这样就可以把B,C,D,E都变换到A空间中去， 向A对齐，完毕后将这5个点云数据做融合（相加），就这样这，可以实现一个拼接或者融合的功能，后续任务就可以基于这份包含更细致信息的点云做文章。运行效果如下（左边显示的是对齐后的点云和目标点云，右边是源点云和目标点云），左边两组点云的一致性很好，而右边两组由于未对齐，可看到是错乱分布的。这里需要清楚的一点，是将源点云向目标点云对齐，让其变换到目标点云的样子。
PCL-- pcl_registration详解
weixin_45590420的博客
06-21 1228
PCL点云配准模块深度解析 PCL注册模块(pcl_registration)提供了一系列点云配准算法，包括ICP及其变种(如Point-to-Plane ICP、G-ICP)、NDT和基于特征的配准方法。该模块支持完整的配准流程：从粗配准(基于特征匹配+RANSAC)到精配准(ICP优化)，并提供多帧点云配准方案(Pairwise串联和全局优化LUM)。工程实践中建议采用降采样预处理、OpenMP并行化等加速策略，并通过Fitness Score评估配准质量。关键算法如ICP可通过设置最大对应点距离、迭代
PCL源码剖析 -- 欧式聚类
Darchan的博客
04-12 2474
PCL源码剖析 -- 欧式聚类
PCL --KD-ICP实现点云精配准
weixin_45590420的博客
12-12 952
摘要： PCL的KD树加速版ICP配准通过构建三维空间索引（KD树）优化最近点搜索，将时间复杂度从O(N²)降至O(NlogN)，显著提升大规模点云对齐效率。该方法采用“KD树索引→快速近邻匹配→最小二乘优化→迭代收敛”流程，支持调节对应点距离阈值（如1.0m）、转换精度（1e-10）等参数。实验表明，10万级点云下KD树搜索耗时仅毫秒级（暴力搜索需秒级），适用于激光雷达等场景。核心API包括pcl::search::KdTree和setSearchMethod绑定KD树，配合变换矩阵求解与可视化验证，实现
PCL库源代码阅读—配准模块(Registration)—对应关系模块(Correspondence)
不知名的代码小白
05-25 1037
对应关系模块可以用来确定两点云中满足最近邻原则的对应点对，为后续的两点云配准提供基础。该模块中对应关系的数据形式是结构体，核心函数是determineCorrespondences()和determineReciprocalCorrespondences()，该函数可以单独使用于读者所编写的程序中，实现二次开发的目的。
PCL中的点云配准（Registration）ICP算法
热门推荐
wokaowokaowokao12345的专栏
06-26 7万+
引言给定两个来自不同坐标系的三维数据点集，找到两个点集空间的变换关系，使得两个点集能统一到同一坐标系统中，即配准过程。配准的目标是在全局坐标框架中找到单独获取的视图的相对位置和方向，使得它们之间的相交区域完全重叠。对于从不同视图（views）获取的每一组点云数据，需要一个能够将它们对齐在一起的单一点云模型，从而可以应用后续处理步骤，如分割和对象重构。 这篇博文就基于PCL中的ICP算法，实现不同视
PCL：实现随机采样（附完整源码）
希望我的博客，能帮上你解决学习中工作中所遇到的问题
10-06 340
PCL：实现随机采样（附完整源码）
PCL系列-1.源码编译
周陽讀書
09-24 1512
归纳整理PCL使用及点云算法。
PCL学习笔记二：Registration (ICP算法)
TODAY WAS A GOOD DAY
05-17 7万+
PCL Registration API Registration：不断调整，把不同角度的3D点数据整合到一个完整的模型中。 它的目的在于在一个全局坐标系下找到不同视角的定位与定向（两个视角交叉部分重叠完好为最优）。这就是KinectFusion论文中所提到的ICP（ Iterative Closest Point ）算法。给定输入数据集，首先做一个估计，然后通过旋转和平移变换一个数据集，找到
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

打赏作者

想躺平的点云工程师

感谢各位看官！

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

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

打赏作者

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

抵扣说明：

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

余额充值