PCL点云配准库4种ICP的使用方法

PCL库与ICP

众所周知，PCL（Point Cloud Library） 是目前最通用的点云处理的库，我所知的支持C++和Python两种版本。虽然效率、稳定性和平台通用性还存在疑问，实际工程中可能只会使用其中较为简单直接的函数，但是其中提供的广大的算法模块给研发提供了方便，避免了在探索期内自己造轮子和四处搜寻代码的麻烦。

其中的点云配准算法ICP（Iterative Closest Point） 就属于这样一类自己写较为复杂、网上找其他代码接口参差不齐、参数并不通用的一种算法。由于在类似点云建图的应用中，通常不会对实时性作过高要求，因此此时使用PCL自带库快速搭建应用、查看效果是一种非常理智的选择。

ICP对比点云SLAM中的配准（题外话）

如果对各大点云SLAM榜单有所了解应该知道，ICP虽然是一种非常重要和通用的点云配准算法，但是在点云配准的重要应用场景SLAM中的使用却非常少见，原因在于ICP这种算法在基于单核的运算中耗时较大，几乎无法做到实时（市面上大多数激光雷达的默认帧率为10Hz）。但是这并不意味着那些方法比ICP更优秀，以LOAM为核心的一系列方法（包括较新的LIO-SAM）为例，其中的配准方法利用了线扫式激光雷达的点云分布特性，因此无法在无结构的点云中很快拓展。

仅就配准原理来说，仍然逃不开与ICP一样的思想，即点与点、线与线、面与面匹配。只是在利用了点云结构特性后使得特征提取快了很多，而且在不使用单个点作为一类特征之后特征个数少了很多。总而言之，ICP在通用性上仍有其固有价值。

PCL中的ICP

ICP这种方法早就已经很透明，因此网上各种实现也很多，各类变种的区别要想完全了解可能不太有必要，可以从PCL的ICP入手，再针对问题去寻找替代方法。这里仅列举四种ICP：

• ICP：最原始的基础版本，使用点与点匹配
• NDT：Normal Distributions Transform，解决ICP直接用点进行计算的速度问题
• GICP：通用化的ICP，均衡使用点线面的匹配
• 基于点到面匹配的ICP

除开第四种PCL中没有专门实现，其余三种都有差不多的接口和使用方法，从效果而言并无必要特意使用第四种，只是在此作为记录。前三种方法在PCL官方文档中可找到各种配置的方法，但是我自己使用下来，有的配置影响不大，有的配置需要针对性调整，因此此处只列举通用配置，不作展开。

为了描述方便，这里统一一下命名：点云配准可以描述为求取一个点云到另一个点云的刚体变换矩阵，我们将不变的那个点云，也就是目标点云用Target来表示，而需要变换向Target靠拢的点云用Source表示。

1. ICP

原始的ICP匹配精度谈不上最高，但是算较为精确的一档，并且很通用，配置少，PCL官方文档中也有例程介绍。缺点是点多时在前三种方法中最慢，点稀疏时匹配效果不好，并且几乎所有ICP都对初始位置敏感，可以尝试用RANSAC等方法优化。

#include 

#include 
#include 
#include 
...
// 输入如下：
// 目标点云(Target)
pcl::PointCloud<pcl::PointXYZ>::Ptr tgt_cloud
// 需要变换的点云(Source)
pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud
// 最大迭代次数
int max_iter
...
// 1. 初始化ICP的实例
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// 2. 设置输入点云
icp.setInputSource(src_cloud);
icp.setInputTarget(tgt_cloud);
// 3. 配置ICP，可设置停止迭代的条件，算法参数等等，此处之设置迭代次数，其他默认
//    迭代次数并不是设置多少就真的要迭代多少，程序检测到收敛会自动停止，因此此设置足够大即可
icp.setMaximumIte