ICP综述

原理+python–code
原理+python–code
原理+C++、PCL–code

算法思想

百度文库

ICP算法配准元素的选择、配准策略的确定、误差函数的求解

配准元素的选择

就是找到需要的点集

• 配准元素的选择，即对匹配点集进行采样
  • 采样方法有很多，目的是减少配准点的数目，用最少的点来表征原始点集的全部特征信息

配准策略的确定

• 配准策略的选择，包括特征度量的选择和搜索策略的选择

特征度量的选择

• 利用特征度量获取对应点对
• 利用特征度量建立迭代优化目标的目标函数
• 特征度量一方面是用来获取对应点对，另一方面，也是用特征度量建立迭代优化目标函数。比如，我们用欧式距离获取对应点对，同时也有欧式距离作为目标函数。我们的目标就是求使目标函数最小的R、T

1. 点到点的欧式距离 ---- 标准ICP算法，计算量大
2. 点到面的距离 ---- 场景集P中的点p的法线与模型点集X的交点q,作出q的切平面y，p到y的距离d

• 减少了迭代次数
  

3. 豪斯多夫距离 — 目前没实际应用
4. 几何特征

• 指除了距离以外的几何特征，比如法向量、曲率。

搜索策略

在对应点的确定，也就是构造各对应点的过程中，需要进行大量的搜索，提高搜索速度很重要。

• K-D tree

误差函数的求解

误差函数的求解，也就是目标函数的最小化
目前有基于奇异值的方法、四元数法、正交矩阵法、双四元数法，这四种方法的精确性和稳定性差异不大。基于SVD的方法和基于四元数的方法应用的更加广泛。

• 基于SVD的
  参考1
  参考2
• 基于四元数的

ICP寻找点集对应关系

标准ICP算法中，选用点集中所有的点来计算对应点，通常用于配准的点集元素数量都是非常巨大的，通过这些点集来计算，所消耗的时间很长。在后来的研究中，提出了各种方法来选择配准元素，这些方法的主要目的都是为了减小点集元素的数目，即如何选用最少的点来表征原始点集的全部特征信息。在点集选取时可以：1.选取所有点；2.均匀采样（Uniform sub-sampling ）；3.随机采样（Random sampling）；4.按特征采样（Feature based Sampling ）；5.法向空间均匀采样（Normal-space sampling），如下图所示，法向采样保证了法向上的连续性（Ensure that samples have normals distributed as uniformly as possible）

举个例子，将两组大小为20的点集进行配准，ICP算法在求最近邻点的过程中需要计算20×20次距离并比较大小。如果点的数目巨大，那算法的效率将非常低。

算法流程

1. 按照最近点的准则，计算两个点集中两两点对之间的距离。对于P点集中的点A，认为另一个点集Q中与自己欧式距离最近的点B与自己配对。
2. 确定对应点对之后，用SVD的方法，求出变换矩阵。$R=U{V}^T$ 和T
3. 用求出的变换矩阵作用于P，得到P1。计算P1和Q之间的误差函数，比如平均距离，距离平方和除以点数等等。如果误差满足要求，则停止。若误差不满足要求，则返回第一步。

影响因素

两个点集的初始相对位置对算法的收敛性有一定影响，最好在“足够近”的条件下进行ICP配准。