ICP算法改进--基于曲率特征

算法步骤：利用二次曲面逼近方法求每点的方向矢量以及曲率；根据曲率确定特征点集；根据方向矢量调整对应关系，从而减少ICP算法的搜索量，提高效率。

算法的创新点：针对目标函数，引入Niloy坐标框架，可根据点云距离调整收敛速度和精确配准精度。

简介：

通常获取被测物体的三维点云时，由于光的线性传播特性，光学设备每次只能测量到物体局部坐标系下的部分表面，并且出现平移错位和旋转错位。因此，配准迫在眉睫。点云配准技术有：手动配准、依赖仪器的配准、自动配准。通常意义上的点云配准是自动配准技术。在进行精确配准之前，初始配准的意义在于缩小点云平移以及旋转误差，提供给精配准良好的初值，以提高配准效率和趋势。精确配准使得点云配准误差达到更小，不能说最小。

程序中，首先利用PCA进行初始匹配。对于精确配准，采用基于曲率的特征点的改进ICP算法，结果表明降低了搜索复杂度，提高了算法效率，可使用于海量点云数据的配准。

在改进的ICP核心步骤中，采用Niloy坐标框架，把曲率引入目标函数的计算，根据点云距离有效的把目标函数从点到点的计算，过渡到点到面的计算，比传统方法具有更快的速度。

初始配准：

点云

是N维数据，均值和协方差矩阵分别为：

协方差矩阵cov的特征向量，即为点集P的主轴。由于初始配准，会出现坐标轴的两个方向相差180°的情况，可计算包围盒的重合体积，如果大于某一个设定容差，则两片点云大致重合，反之，则发转参考坐标系再次测试。

精确配准：

ICP是最常用的精配准算法，在每次迭代的过程下，对数据中每一点，在模型点云中寻找欧氏距离最近点作为对应点，通过对应点对，使得目标函数最小化。

从而得到最优的t和R。

ICP算法的缺陷：要求数据点云里的每一点在模型点云上都要有对应点，为寻找对应点，算法需要遍历模型点云的每一点，配准速度慢，并且易陷于局部最优解。

ICP算法改进原理：

① 计算方向矢量

对一点Pi，方向矢量等价于该点与其邻域Nb（Pi）的最小二乘拟合平面的法向量n(Pi)。则点Pi与拟合平面的误差可以由点Pi和平面内点的连线与点Pi的点积求得。误差矩阵定义如下：

其中o是Nb（Pi）的质心：

当Err最小时，n（Pi）的值为拟合平面的法向量，此问题可转化为求取协方差矩阵的最小特征值对应的特征向量问题：

上式 ，其最小特征值对应的特征向量就是所求点的方向矢量n(Pi)。

② 曲率计算

利用MLS算法计算点云每一点的高斯曲率和平均值曲率。MLS是沿向量场   n(x)方向，能量函数e(y,a)的局部最小值。

其中：y-位置向量；a-方向向量；Vi-法向量；qi-点集Q中的点。

则有：

③   坐标变换计算

对n组对应点集合P^’和Q^’，坐标变换的计算实质是使得目标函数最小化：

最小化得到最优的t和R。Niloy定义的距离函数把曲率引入目标函数，    能有效的把点到点过渡到点到面：

此目标函数重新定义了坐标框架，

表示沿框架坐标轴的坐标分    量。在该框架坐标系下，以模型点qi为原点的框架距离定义为：

其中，

表示

方向的曲率，d表示两点的欧氏距离。

参考文献：ICP算法在3D点云配准中的应用研究—杨现辉、王惠南

  程序编写成功，运行验证成功，写下重点，分享给大家，若有错误点，请各位及时指出，方便一起学习并进一步改进。之后我会把改进的程序共享。实验所用数据来于：http://geometryhub.net/scandata