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