RGB-D 实时三维重建/SLAM 中的 ICP 算法解析

提要

KinectFusion，ElasticFusion，Kintinuous 在跟踪的时候，都用到了这篇博客里的 ICP 算法，这里给出 ICP 介绍和详细推到。

关于ICP算法

ICP 算法主要用于两帧点云配准，在两帧点云位姿差别较大时，通常先用特征匹配计算粗略位姿，然后再使用 ICP 算法对位姿精炼。在实时的三维重建中，由于相邻两帧之间位姿变化比较小，可以直接使用上一帧求解到的相机位姿作为初始值，省去了特征点匹配的步骤。

ICP 算法按照目标函数分类，可以分成通过极小化点到点距离平方和计算位姿，和通过极小化点到面距离平方和计算位姿。KinectFusion 算法中使用的 ICP 算法通过极小化点到面的距离平方和计算位姿。

ICP 算法步骤

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

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

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

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

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

投影算法计算匹配点

对于两帧图像间位姿变换比较小时，可以使用投影算法计算匹配点，不需要像建立 k-d tree 等算法搜索，投影算法直观上公式理解：
k(ud, vd,1)=KMPs

其中 Ps ：待求匹配点的三维空间坐标
M ：两帧之间的位姿变换，在 ICP 算法中通过不断迭代求得
K ：相机内参数
(ud, vd) ： Ps 在图像 Dd 上的匹配点像素坐标

ICP 通过 R 矩阵线性化极小化目标函数

点到平面距离示意图如下：

点到平面的距离平方和目标函数如下：

E=∑i=1k((Rpi+t−qi)⋅ni)2

上式表达式中 R, t 是待求解的位姿变换
pi 和 qi 分别是原始点和目标点
ni 指目标点的法向量

上式关于 R 只有三个自由度，用 r=(rx, ry, rz) 表示，平移向量 t 表示为 t=(tx, ty, tz) 。

上式因为旋转矩阵 R 是非线性的，造成目标函数也是非线性，非线性问题求解通常不存在闭式解，这里将 R 线性化，使得目标函数存在闭式解。

当相邻两帧之间位姿变化较小时，可以有以下近似：
sin(θ)=0, cos(θ)=1

用三个旋转角 r=(rx, ry, rz) 展开 R 矩阵，
Rpi+t=pi+r×pi+t
并且： r×pi⋅qi=r⋅(pi×qi)

上述近似，对于目标函数可以写成：

E=∑i=1k((pi−qi)⋅ni+r⋅(pi×qi)+t⋅ni)2

以上目标函数对于未知 6 维位姿参数求导并且令导数为 0，可以得到 Cx=b
C 的表达式为：

C=(p1×n1n1……pk×nknk)⎛⎝⎜⎜(p1×n1)⊤…(pk×nk)⊤n⊤1…n⊤k⎞⎠⎟⎟.

写成单个点累加形式：

C=∑i=1k(pi×nin1)((pi×ni)⊤n⊤i).

b 的表达式为：

b=(p1×n1n1……pk×nknk)⎛⎝⎜⎜(p1−q1)⊤⋅n1…(pk−qk)⊤⋅n1⎞⎠⎟⎟.

写成单个点累加形式：

b=∑i=1k((pi×ni)τ, ni)τ⋅((pi−qi)τ⋅ni)

未知参量 x 表达式为 x=(rx, ry, rz, tx, ty tz)

上式对于单点求和的形式，可以用 GPU 做并行计算。