ICP点云配准原理及优化

ICP算法简介

根据点云数据所包含的空间信息，可以直接利用点云数据进行配准。主流算法为最近迭代算法（ICP，Iterative Closest Point）,该算法是根据点云数据首先构造局部几何特征，然后再根据局部几何特征进行点云数据重定位。

一、 ICP原理

假设两个点云数据集合P和G，要通过P转换到G（假设两组点云存在局部几何特征相似的部分），可以通过P叉乘四元矩阵进行旋转平移变换到G，或者SVD法将P转换到G位置，总体思想都是需要一个4x4的旋转平移矩阵。对于每次旋转平移变换后计算P的所有（采样）点到G对应（最近）点的距离，用最小二乘法（求方差）求出最小二乘误差，看是否在要求的范围内，如果最小二乘误差小于设定的值，（或迭代次数达到上限，或每次重新迭代后最小二乘误差总在一个很小的范围内不再发生变化），则计算结束，否则继续进行迭代。

粗配准优化方法：主成分分析法

精配准优化方法：基于正交投影的ICp算法改进

二、粗配准优化

PCA是一种有效的检测数据集简化分析方法，用于减少数据集的维数，同时保持数据集对方差贡献最大特征，对于点集P(x₁,x₂,…,x_n)，其中，x_i是n维数据，均值和协方差矩阵分别为：

$\bar x = \frac{{\mathop \sum \nolimits_{i = 1}^n {x_i}}}{n}$

${\rm{cov}} = \frac{{\mathop \sum \nolimits_{i = 1}^n \left( {{x_i} - \bar x} \right){{\left( {{x_i} - \bar x} \right)}^T}}}{n}$

协方差矩阵cov的特征向量，即为点集P的主轴。对于三维点云数据，以均值为坐标系的原点，PCA求得的三个特征向量对应XYZ轴，建立点云的参考坐标系。

由于PCA反应了数据集对方差贡献的最大特征，相似度大的两片点云，只要把其参考坐标系调整到一致，即可达到初始配准的目的。由于可能出现坐标轴的两个方向相差180^o的情况，需要建立最小包围盒来测试两片点云是否调整重合，通过坐标变换可以将数据点云包围盒变换到模型点云参考坐标系中，使两包围盒的空间位置大体一致。计算包围盒的重合体积，如果大于某一设定的容差，则两片点云大致重合，如果小于设定容差，则反转数据点云参考坐标系坐标轴再次尝试。通过坐标系调整后，即可达到点云的初始配准目的，为下一步的精确配准提供较好的初值。

三、精配准优化

通过初始配准，两片点云大致重合但是误差精度还远远达不到实际应用中的精度要求。为了使用点云间的误差达到最小，还需要对其进行精确配准。ICP算法是最常用的数据精确配准方法，算法在每次迭代的过程中，对数据点云的每一点，在模型点云中寻找欧氏距离最近点作为对应点，通过这组对应点使目标函数s²最小化：

${{\rm{s}}^2} = {\rm{min}}\mathop \sum \limits_{i = 1}^N {\left\| {{Q_i} - \left( {R{P_i} + t} \right)} \right\|^2}$

来得到最优的四维转换矩阵（包含平移和旋转），将四维变换矩阵作用到点云数据上，得到新的数据点云带入下次迭代过程。但是ICP算法具有比较明显的缺陷，要求数据点云上的每一点在模型点云上都要找到对应点，为了寻找对应点，算法需要遍历模型点云上的每一点，配准速度很慢，且容易陷入局部最优解。

由于ICP算法的时间消耗主要花费在计算对应点集上，如果能降低此步骤的时间消耗，即可以提高ICP算法效率，具体计算请参考博客点云对齐。