ICP点云配准初探

1 简介

在逆向工程，计算机视觉，文物数字化等领域中，由于点云的不完整，旋转错位，平移错位等，使得要得到的完整的点云就需要对局部点云进行配准；

为了得到被测物体的完整数据模型，需要确定一个合适的坐标系，将从各个视角得到的点集合并到统一的坐标系下形成一个完整的点云，
然后就可以方便进行可视化的操作，这就是点云数据的配准。

点云配准的应用还有很多。
比如：
形状还原
运动估计
外观分析
纹理映射
追踪
等等

那么点云配准的本质是什么呢？

本质就是把不同的坐标系中测得到的数据点云进行坐标系的变换，以得到整体的数据模型，
问题的关键是如何让得到坐标变换的参数R（旋转矩阵）和T（平移向量），使得两视角下测得的三维数据经坐标变换后的距离最小。

目前配准算法按照过程可以分为全局配准和局部配准。

PCL中有单独的配准模块，实现了配准相关的基础数据结构，和经典的配准算法如ICP。

即：给定两个来自不同坐标系的三维数据点集，找到两个点集空间的变换关系，使得两个点集能统一到同一坐标系统中，即配准过程
求得旋转和平移矩阵。

P2 = R*P1 + T　　　　[R t]

点云配准的概念也可以类比于二维图像中的配准，只不过二维图像配准获取得到的是x，y，alpha，beta等放射变化参数

三维点云配准可以模拟三维点云的移动和对齐，也就是会获得一个旋转矩阵和一个平移向量，
通常表达为一个4×3的矩阵，其中3×3是旋转矩阵，1x3是平移向量。
严格说来是6个参数，因为旋转矩阵也可以通过罗格里德斯变换转变成1*3的旋转向量。

2 常用的点云配准算法

下面介绍下几种常用的点云配准算法。

1. 正态分布变换方法： NDT 正态分布变换进行配准（normal Distributions Transform）
2. 著名的迭代最近点 Iterative Closest Point （ICP） 点云配准

这里主要讲述ICP点云配准算法 。

3 ICP（Iterative Closest Point，最近点迭代法）

3.1 ICP要解决的问题

ICP要解决的问题就是将两片点云进行对齐，通过刚性配准的方法，即旋转和平移。

现在有两片点云，我们分别叫做model shape和scene shape，我们一般把model shape往scene shape上配准。

具体的，对于model shape来讲：

• 给定一个model shape，可以有多种表示形式，例如：
  – 点集、线段集、 隐式曲线、参数曲线、mesh、隐式曲面、参数化曲面（如下图所示）
  

对于scene shape来讲：

• 给定一个scene shape，其表示为点集，scene shape(下图绿线)可能对应于model shape(下图黄线)

• 需要估计最佳旋转和平移，将scene shape与model shape对齐或配准

3.2 ICP的核心思想

ICP算法本质上是基于最小二乘法的最优配准方法。

该算法重复进行选择对应关系点对，计算最优刚体变换这一过程，直到满足正确配准的收敛精度要求。

算法的输入：参考点云（model shape）和目标点云（即scene shape），停止迭代的标准。
算法的输出：旋转和平移矩阵，即转换矩阵。

那么问题来了，

我们怎么去找到对应的关系点对？

我们找到怎么去判断是否达到正确配准的收敛精度要求？

这里我简单的说一下，后面还会细讲

对于“我们怎么去找到对应的关系点对？”，

方法有很多，比如穷举法，这种就比较耗时；所以为了提升效率，我们会做一个粗对齐，比如先特征检测，基于特征做一个粗对齐，再去找最近邻的点去精对齐。这里大概的罗列了下：

使用点匹配时，使用点的XYZ的坐标作为特征值，针对有序点云和无序点云数据的不同的处理策略：
1. 穷举配准（brute force matching）;
2. kd树最近邻查询（FLANN）;
3. 在有序点云数据的图像空间中查找;
4. 在无序点云数据的索引空间中查找.

特征描述符匹配：
1. 穷举配准（brute force matching）;
2. kd树最近邻查询（FLANN）。

为了得到更高的收敛精度，我们需要保证我们的点尽可能多的比例是有效点，这里注意的是，有噪点很正常，但是噪点的比例不要太高 ，太高会影响到我们的收敛精度，噪点越多，错误对应关系的比例就越多。

由于噪声的影响，通常并不是所有估计的对应关系都是正确的，
由于错误的对应关系对于最终的刚体变换矩阵的估算会产生负面的影响，
所以必须去除它们，可以采用随机采样一致性估计，或者其他方法剔除错误的对应关系，
最终只使用一定比例的对应关系，这样既能提高变换矩阵的估计同时也可以提高配准点的速度。

3.3 算法流程

ICP计算流程如下：

总结为

1. 对原始点云数据进行采样(关键点 —— NARF、SIFT、FAST、均匀采样 UniformSampling，特征描述符 —— descriptions，NARF、 FPFH、BRIEF 、SIFT、ORB )
2. 确定初始对应点集(匹配 matching )
3. 去除错误对应点对(随机采样一致性估计 RANSAC )
4. 坐标变换的求解
5. 计算误差，反复迭代，直到误差小于阈值。

如果正确的对应关系点对是已知的，那我们就很快的找到变换关系(即旋转和平移)，如下图所示

如何找到对应关系点对呢？
用户输入？ 特征检测？

或者是换一种思路，
假设最近邻点是对应点，如下图

如果起始位置“足够接近”则会很容易收敛，如下图

那么如何找到最近邻点呢？

我们遍历scene shape的每一个点，用欧式距离找到在model shape最近的一个点，具体的，如下图所示

找到对应关系点对后，用四元数的方法，当然也不止这种方法，去计算点对之间的变换关系。

具体的

1. 先计算两片点云的质心
2. 再将两片点云的坐标分别减去质心坐标，对每一个点进行归一化
3. 计算对称矩阵，并得到特征值和对应的特征向量
4. 再计算变换矩阵

这里就大概知道怎么计算的就行啦，重要的是这些可以去辅助理解ICP核心思想，

至于如何计算，其实有很多API可以去调用的。

3.4 总结

以上讲了这么多，现在到了归纳总结时间

我们输入的是两片点云的坐标

输出的是变换关系、误差和施加变换后的点

ICP算法具体怎么做

1. 初始化
   

2. 找到一一对应点
   

3. 寻找变换关系

4. 施加变换
   
   • 零均值点集
   • 四元数计算
   • 旋转矩阵计算
   • 平移偏移计算
   • 残余误差计算

5. 计算误差

4 ICP优缺点

优点：

1）不需要对点云集进行分割和特征提取。

2）在初值较好的情况下，可以得到很好的算法收敛性。

缺点：

1）在搜索对应点的过程中，计算量较大，计算速度较慢。

2）对配准点云的初始位置有一定要求，不合理的初始位置会导致算法陷入局部最优。

3）ICP 算法在寻找对应点时，会将任何两个点云之间的欧氏距离最近的点作为对应点，这种假设会产生一定数量的错误对应点。