ICP算法理解

点云数据配准采用最多的就是
Besl 等 [3]提出的迭代最近点(ICP)算法， 该算法思想简单、精度高， 但因其使用需要满足两个前提条件，
即两个点云间存在包含关系且两个点云初始位置不能相差太大， 所以当前最流行的匹配方案都采取先初始后精确的匹配模式。通常所采用的初始配准方法有: 1)
标志

点法[4-5],在对物体进行测量时， 人为的对其贴上一些标志点， 再通过这些标志点进行匹配；
2) 重心重合法[6]
,简单的将两个点云的重心重合在一起；
3) 特征提取法， 例如提取轮廓曲线
[7]、 平面几何特征
[8-11]等。

ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中，
4000
首先是找到一个可用的变换，配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。
ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对， 计算最优刚体变换，直到满足正确配准的收敛精度要求。
ICP 算法的目的是要找到待配准点云数据与参考云数据之间的旋转参数R和平移参数 T，使得两点数据之间满足某种度量准则下的最优匹配。

假设给两个三维点集 X1 和 X2，ICP方法的配准步骤如下：
第一步，计算X2中的每一个点在X1 点集中的对应近点；
第二步，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；
第三步，对X2使用上一步求得的平移和旋转参数，得到新的变换点集；
第四步， 如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的X2继续迭代，直到达到目标函数的要求。
 最近点对查找：对应点的计算是整个配准过程中耗费时间最长的步骤，查找最近点，利用 k－d tree提高查找速度 K－d tree 法建立点的拓扑关系是基于二叉树的坐标轴分割，构造 k－d tree 的过程就是按照二叉树法则生成，首先按 X 轴寻找分割线，即计算所有点的x值的平均值，以最接近这个平均值的点的x值将空间分成两部分，然后在分成的子空间中按
Y 轴寻找分割线，将其各分成两部分，分割好的子空间在按X轴分割……依此类推，最后直到分割的区域内只有一个点。这样的分割过程就对应于一个二叉树，二叉树的分节点就对应一条分割线，而二叉树的每个叶子节点就对应一个点。这样点的拓扑关系就建立了。