ICP

SLAM

ref 博客

ICP 算法主要用于两帧点云配准，在两帧点云位姿差别较大时，通常先用特征匹配计算粗略位姿，然后再使用 ICP 算法对位姿精炼。

ICP 算法按照目标函数分类，可以分成

• 通过极小化点到点距离平方和计算位姿，
• 和通过极小化点到面距离平方和计算位姿。

KinectFusion 算法中使用的第2种

流程如下

1、输入的两幅深度图像，分 3 层抽样，按照 coarse-to-fine 的方式配准，对于抽样后的点云做滤波。

2、通过原始深度图像计算点的三维坐标（用于点云的配准和融合），通过滤波后的图像计算三维点云坐标（用于计算法向量）。

3、对于两幅点云计算匹配点（根据投影算法计算匹配点，后续会介绍）

4、根据匹配点极小化目标函数计算位姿（通过线性化 R 计算位姿，有闭式解不需要迭代）

5、目标函数误差小于一定值时，停止迭代，否则进入第 3 步