ICP综述

一、ICP 求解过程

       ICP 有好多变种，ICP求解步骤可以分为以下部分：

       1、选点

       在模型或者场景中选取匹配点

       2、匹配

       将选取的一组点匹配另一组点

       3、分配权重

       给匹配的点对分配权重

       4、去除错误匹配

       根据单个匹配点对或者根据总体匹配情况去除错误匹配点

       5、给定误差标准

       可以是匹配点对的误差二模或者一模等形式

       6、最小化匹配误差

       通过最小化匹配误差求解位姿转换矩阵

       下面分别从六个方面介绍ICP的求解过程。

二、选点

选点有以下方式

1、利用全部点进行迭代匹配

2、对所有点进行均匀采样

3、对所有点进行随机采样

4、利用可见光图像采样梯度值大的点

5、对所有点按照法向量分布采样，使得采样点法向量分布均匀

6、对于所有上述采样算法，可以只从模型采样，或者只从场景采样，或者双向采样

7、按照对点云配准的贡献进行采样，算法见论文《Geometrically StableSampling for the ICP Algorithm》，见附录 C。

利用方法 1 – 4 采样缺点，对于下图所示的配准会产生误差，下图配准靠十字形凹槽进行配准。

三、匹配

1、closest point 在一组点云中找另一组点云的最近点，可以用 k-d tree等算法加速最近点搜索

2、normal shooting 一组点云中的一个点，沿着点的法向量的光线和另一组点云的交点最为匹配点

3、project将场景点，从模型点的视角下，投影到模型点云

4、project and walk 如 3 方法，在投影到目标点云后，再在目标点云处搜索，搜索可以根据两点之间的距离，点到光线的距离，或者灰度值的相近程度等

5、可以上述的所有算法，前提是在两个点能在给定的衡量标准下可以匹配上，衡量标准比如颜色、两个点之间法向量的角度等，利用最速下降法搜素

优缺点：

       closestpoint对于 (b) 场景不收敛，对于噪声比较敏感

  对比closest point 和 project

       对于场景 (c) 上述算法只有最近点算法是收敛的

     利用 project 或者 project walk 算法收敛速度块好多（甚至一个数量级）

四、分配权重

   1、权重是常量

  2、对于距离远的点降低其权重

   3、根据点法向量的相近程度分配权重

   4、根据曲率分配权重

   5、根据点颜色的相近程度分配权重

  6、根据测量点的不确定性分配权重（由点云测量传感器决定）

       优缺点：

       实验表明上述算法的效果相当。

五、去除错误匹配

       1、将匹配点距离超过一定值的去除

       2、将匹配点中，以一定衡量标准衡量的（通常点到点距离） n% 最坏的匹配点对去掉

       3、将匹配点中，点到点距离大于一定乘机值的去掉（例如 2.5 倍的标准差）

       4、去掉和周围点对不一样的匹配

       5、去掉在mesh边界的点

       作者推荐去除mesh边界上的点，使用起来计算代价低，对于，当两组点云不是完全重合匹配时，可以避免好多无匹配。

       实验表明去掉 10% 最坏的匹配、2.5倍的标准差去除，对于算法的收敛性没有改善，反而两组匹配点相隔较远时，收敛速度较慢。

       6、Picky ICP 对于一个模型点被几个场景中点作为匹配点

       如果，一个模型中的点，被几个场景中的点，选择作为匹配点，则按照模型点和场景点匹配的距离，选择距离最小的场景点作为模型点的匹配点。算法在两组点云只是部分重合时很有效，会使得收敛速度变慢，同时对于原始ICP算法收敛性的证明也不再适用于 Picky ICP。

       7、对于一个模型中的点在场景中有几个匹配点的问题，还可以参考论文《Robust ICP Registrationusing Biunique Correspondence》的解决方法。

六、给定误差标准

       1、匹配点距离的平方和

       对于匹配点距离的平方和有闭式解，求解时，不需要迭代，算法有 SVD 等算法，求解步骤见附录 A。匹配点的误差平方和最为目标函数，求解的方法还有Quaternions、Orthonoraml matrices和Dual quaternions。

       2、上面点到点距离平方和再加上点的颜色的差别

       3、点到包含另一个点平面的距离的平方和

       如图：

       点到平面距离表达式：

       没有闭式解，可以用非线性最小化算法例如 LM 算法求解，或者将问题线性化求解，令 sin a = a，cosa = 1，将求解问题线性化适合两帧之间变化小的时候求解，利用线性化求解，求解步骤见附录 B。

       4、其它衡量标准，例如 L1 模

七、加速算法

       如下图：

       表示位姿求解的结果

       令

       当两次变化的角度满足以下关系时，对于最后三次位姿的移动可以有很好的近似。

       令为位姿求解后带入能量函数的残差， 为近似的弧线的长度，则利用和可以对上述的曲线做近似为：

       对于直线近似在和 X 轴交点处，可以给出线性的更新，对于二次曲线在最小值处，给出二次的更新，如下；

       

附录A

       参考《Evaluation of surface registrationalgorithms for PET motion correction》2010

    利用 SVD分解求解点到点距离平方和最小

待求解的能量函数

       对于匹配点距离的平方和有闭式解，求解时，不需要迭代，算法有 SVD 等算法。

    利用SVD分解求解算法：

     p 和 q 最为匹配点对

    计算匹配点对的中心坐标

       点到中心点的偏差

      找变换 T 使得下面目标函数最小化

      将上式带入可得

     因为要最小化误差，需要将两组点云数据的中心点移到一起，满足下式

     目标函数简化为

     定义

    目标函数最小化转换为下式最小化

    由柯西不等式得

    由 R 是单位正交矩阵

    对于 N 奇异值分解

    当 R 为

    RN trace 变为

  即 R 取上式时目标函数达到最小。

 

 

 

 

附录B

参考文献《Linear Least-Squares Optimization for Point-to-Plane ICP SurfaceRegistration》

点到平面距离平方和表达式为：

其中 M 是欧式变换

       其中

       其中

       由上式可以看出，目标函数是三个旋转角  和三个位移量  的二次函数，只能用迭代求解的方法（例如LM）。

       线性近似

       当旋转角  近似为 0 度，可以让、，由近似可以得到以下表达式：

       变换矩阵 M 变为：

       目标函数：

       对于 N 对匹配点，将目标函数变为：

       变量

       其中

       目标函数变为

       位姿最优解

注意：

          1、虽然近似的时候，角度不能太大，实际应用的时候，近似的角度甚至可以得到30度（角度的大小和两组点云的几何形状，和两组点云重叠的范围有关）。

 

附录C

       按照点对配准的贡献进行采样，如图，采样如下

算法如下：

利用点到平面距离作为目标函数最小化，表达式如下：