KinectFusion公式推导、理解

  “KinectFusion Real-Time Dense Surface Mapping and Tracking”一文于2012年发表，该文章首次实现了实时稠密重建（Real time dense restruction），我认为微软的Kinect深度相机是其成功的根本，该文章也是首次成功应用了深度相机，是做实时稠密重建（Real time dense restruction）方向的必读论文。

  系统的主体结构如图所示：（摘自原文）

1. 深度相机采集数据，对数据做预处理。（Surface measurement）
2. 使用Prj ICP算法估计相机位姿。（Sensor pose estimation）
3. 融合当前帧点云到模型。（Surface reconstruction update）
4. 使用光线投射法（投影算法）将点云模型投影到下一帧相机位置的图像上供下一帧计算。（Surface prediction）

深度图像预处理（Surface measurement）

  记从传感器获得的第 k 帧深度图像为 Rk ，记 u=[uv]T 为深度图某个像素的像素坐标。那么 Rk(u) 为第 k 帧深度图的第 (u,v) 像素的值，即深度。对 Rk 进行双边滤波得到 Dk 。

  记 K 为相机的内参矩阵（Intrinsic matrix）， u˙=[uT1]T 为 u 的齐次化向量。通过反投影法由 Dk 计算得到该帧的三维点云集合 Vk （其中每个点 Vk(u)∈R3 ）：

Vk(u)=RkK−1u˙

*注1：双边滤波器可以参考双边滤波器的原理及实现一文。其作用是保留边界梯度的情况下，滤除高频噪声。

  然后通过 Vk(u) 计算对应像素点 u 的法向量 Nk(u) :

Nk(u)=ν[(Vk(u,v+1)−Vk(u,v))×(Vk(u+1,v)−Vk(u,v))]

  其中 ν[x] 为向量单位化函数：

ν(x)=x||x||2

  由于深度图像并不是每个像素点都是有值的，如下图，深度图像中纯黑色的部分就是传感器无法采集深度的区域，原因可能是多种的：探测区域超出了传感器的探测范围、过于光滑的材质、能够吸收红外光的材质等。

  因此，需要一个掩码矩阵 Mk 来表示该帧深度图像对应的位置是否有值。若对应像素 u 有值，则 Mk(u)=1 ，否则 Mk(u)=0 。

  至此，得到了 Rk 与 Nk 以供后面的投影法 ICP使用。

相机位姿估计（Sensor pose estimation）

  定义相机坐标系，取相机光心为原点，取相机光轴为 Z 轴，像素坐标轴 u 与相机坐标轴 X 同向。坐标系符合右手系法则。

  TSDF(truncated signed distance function)，一般译作“截断符号距离描述函数”，可以把这个词拆开看：

• 一张深度图像反投影变换到三维空间中的每个三维点都对应着原深度图像中的每一个像素点，当点云比较稠密时，我们可以将该点云看成一个三维曲面，也就是function，记作 F(x,y,z)=0 。
• 所谓signed distance就是带正负符号的距离，以相机的光轴为Z轴，对于一个三维点 (x0,y0,z0) ，记 d=F(x0,y0,z0) ，如果它在平面前面，则 d 的值为正，否则为负，（这是《高等数学》空间几何代数部分的基础内容）。
• 而“truncated”就是截断，当 |d|>t 时，则 |d|=0 ，其中 t 是截断距离。

  ICP算法（Iterate Closest Points）可以用于计算两组点云之间的刚体变换，对于 ICP算法有多种变种，如点到点 ICP（Point-to-Point）、点到面 ICP（Point-to-Plane）、面到面 ICP（Plane-to-Plane）等。

  传统的 ICP（Point-to-Point）是需要知道两组点云之间，一组点云的各个点在另一组点云中的最近点，才能进行计算。显然，这需要遍历另一组点云以寻找最近点，最差会需要 O(N2) 的时间复杂度，即使使用一些数据结构来降低复杂度，（如KD-Tree、Octave-Tree等）也至少需要 O(Nlog(N)) 的复杂度来搜索最近点，这直接导致我们的算法无法实时运行。

  为了抛掉“各个点的对应关系”这一搜索步骤，变种的 ICP都假设两组点之间的旋转、平移量很小，所以一个点附近的点就是它的当前最近点。
  左图中是点到面 ICP，它并不需要去寻找最近的点是哪一个，这个算法所需要的只有两组点云的坐标和当前帧的法向量，也就是 Rk 、 Nk ，通过 Rk 可以计算得到当前帧的点云 Vk ，已知在此之前融合好的模型点云 V^gk 。

（图片摘自“KinectFusion 解析”）

  GPU的三维数组中的每一个元素代表一个体素（Voxel），其存储在体素空间内的顶点的位置。
  例如：用 1000×1000×1000 大小的三维数组表示 1m3 空间内的三维结构。每个元素就是一个体素（Voxel），这个元素对应的数组下标就表示体素在这 1m3 空间内的对应位置的 1cm3 空间，这个元素所存储的值，就是处于这 1cm3 空间中的顶点在这 1m3 内的精确位置。

  通过使用TSDF的描述方法，以上图形式将 V^gk 作为距离描述函数曲面直接放在GPU的三维数组中，即GPU的各个线程对这个三维数组赋值，把对应位置的元素赋值为“0”，离相机近的一侧为正，另一侧为负。然后每个线程负责读取一个 Vk(u) 在这个三维数组中对应位置的值，就得到了当前 Vk(u) 距离 V^gk(u) 的距离。

  记第 k 帧的相机外参矩阵（Extrinsic matrix）为 Tg,k ，其下标“ g ”表示“global”即世界坐标系，“ k ”表示相机第 k 帧， Tg,k 表示世界坐标系变换到第 k 帧相机坐标系的变换矩阵。 Rg,k 为旋转矩阵， tg,k 为平移矩阵。

Tg,k=[Rg,k0T3tg,k1]∈SE(3)

  ICP的目标函数为：

min∑u∈Ωdu,du=||V˙k(ui)−T−1g,kV^gk−1(uj)||2

  其中 ui 与 uj 是一对最近点的像素坐标，通过不断循环迭代，以求得最佳的 Tg,k 。

  KinectFusion使用了更快的投影法ICP（Projective ICP），投影法 ICP直接把 V^gk(u) 投影到当前相机位姿的图像上（新图像），得到 Dgk−1 。

V^gk(u)=[xguyguzgu1]T

  记函数 q=π(p) 为降维函数。其中 p∈R3=[xyz]T,q∈R2=[x/zy/z]T 。

Dgk−1(u)=[001]π(T−1g,kV^gk(u))

π(V^gk(u))=[xgu/1ygu/1zgu/1]T

  计算 Dgk−1(u) 的像素坐标 u ：

u=π(Kπ(T−1g,kV^gk(u)))

  通过 D^gk−1(u) 可以计算得到对应的法向量 N^gk−1(u) 。

  那么两点之间的最近距离 d 为：

du=(Tg,kVk(u)−V^gk(u))TN^gk−1(u)

  投影法ICP的目标函数为：

min∑Ω(u)≠0du,du=∥∥(Tg,kV˙k(u)−V^gk−1(u))⋅N^gk−1(u)∥∥2

  其中 Ω(u) 为掩码矩阵， Ω(u)≠0 表示 u 有效，否则无效。

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪Mk(u)=1,and||π(Tg,kV˙k(u)−V^gk−1(u))||2<εd,andarccos(||N˙k⋅N^gk−1||2||N˙k||2⋅||N^gk−1||2)<εθ

  这三个条件分别表示：

• 当前帧的深度图像对应像素值必须存在。
• 一对“最近点”的距离必须小于截断距离 εd 。
• 当前帧的法向量与已建立好的三维模型对应点的法向量必须几乎相同，小于夹角阈值 εθ 。

  其效果为：（亮灰色是被过滤掉的部分）

  记目标函数为 E(Tg,k) ：

E(Tg,k)=∑Ω(u)≠0∥∥(Tg,kV˙k(u)−V^gk−1(u))⋅N^gk−1(u)∥∥2

  现在，需要把投影法 ICP的目标函数转化成线性最小二乘求解。最小二乘的标准形式是：

min∥Ax−b∥

  其中 A、b 为已知参数， x 为带求变量。对于上式有以下结论：

(ATA)x=ATb⇔x=(ATA)−1ATb

  根据分块矩阵二范数的性质，将 E(Tg,k) 中求二范数的部分写成三维矩阵的形式：

E(Tg,k)=∑Ω(u)≠0∥∥(Rg,kVk(u)+tg,k−π(V^gk−1(u)))⋅N^gk−1(u)∥∥2

  由于旋转矩阵 Rg,k 有9个自变量元素但是只有3个自由度，为了减少最小二乘矩阵的计算规模，需要用罗德里格斯公式（Rodriguez formula）减少变量。

Rg,k=Rg,k−1Rk−1,k

Rk−1,k=I+sin(θ)Fk−1,k+(1−cos(θ))F2k−1,k

Fk−1,k=⎡⎣⎢0rz−ry−rz0rxry−rx0⎤⎦⎥

  由于 θ 的值很小，对 Rk−1,k 作Tylor展开：

Rk−1,k=I+θFk−1,k+o(θ2)≈⎡⎣⎢1rzθ−ryθ−rzθ1rxθryθ−rxθ1⎤⎦⎥=⎡⎣⎢1α−γ−α1βγ−β1⎤⎦⎥

  记 r=[αβγ]T ， [r]× 为 r 的叉积矩阵（详见罗德里格斯公式推导过程）。记“ [⋅]× ”为向量的叉积矩阵化运算符。

[r]×=⎡⎣⎢0γ−β−γ0αβ−α0⎤⎦⎥,[r]×Vk(u)=r×Vk(u)=−Vk(u)×r

  代入上式得：

E(Tg,k)=∑Ω(u)≠0∥∥{Rg,k−1([Vk(u)]×r+Vk(u))+tg,k−π(V^gk−1(u))}⋅N^gk−1(u)∥∥2

  现在取 x=[αβγtxtytz]T∈R6 。

E(Tg,k)=∑Ω(u)≠0∥∥{Rg,k−1[[Vk(u)]×∣I3]x+Rg,k−1Vk(u)−π(V^gk−1(u))}⋅N^gk−1(u)∥∥2

  写成目标函数形式：

min∑Ω(u)≠0∥∥N^gk−1(u)TRg,k−1[[Vk(u)]×∣I3]x+N^gk−1(u)T{Rg,k−1Vk(u)−π(V^gk−1(u)))}∥∥2

  其中：

A=N^gk−1(u)TRg,k−1[[Vk(u)]×∣I3]

b=N^gk−1(u)T{Rg,k−1Vk(u)−π(V^gk−1(u)))}

  使用最小二乘的结论即可求得 x 。

点云融合、表面重建更新（Surface reconstruction update）

  完成对相机位姿的估计之后，就将把新的点云融合到已有的点云模型中。记直到第 k−1 帧点云模型中的点的坐标为 Fk−1(p) ，新的点云中的点的坐标为 FRk(p) 。当 FRk(p) 与 Fk−1(p) 位于同一个体素（Voxel）中时，就需要加权融合两个点的坐标。当然，融合次数越多的点权重越。

Fk(p)=Fk−1(p)Wk−1(p)+FRk(p)WRk(p)Wk−1(p)+WRk(p)

  其中 WRk(p) 为第 k 帧新加入点的权重， Wk−1(p) 为直到 k−1 帧模型中该点的权重。然后更新权重， Wη 为最大权重阈值。

Wk(p)={Wk−1(p)+WRk(p),Wk(p)<WηWη,Wk(p)≥Wη

  以上就是“KinectFusion”整个系统工作的详细步骤。

总结：
  “KinectFusion Real-Time Dense Surface Mapping and Tracking”一文首次实现了稠密的实时三维重建。就起效果而言是非常优秀且稳定的。是所有稠密重建的典范，之后所有带“Fusion”方法的论文基本都跑不出这个框架。十分值得我们深入学习探讨这一方法的框架和细节。
  明显的缺点：该方法只能重建范围有限的空间，空间的大小取决于显存的大小。（InfiniTAM为其改进了这一缺陷）
  不太好的地方：对于细节纹理的重建不好，因为定大小的体素（Voxel）本身就是一个低通滤波器，虽然这也是其滤除噪声的重要方法（优点）。

参考：

1.原文：KinectFusion Real-Time Dense Surface Mapping and Tracking

2.泡泡机器人团队的Xingyin-Fu老师的三篇博客：

• KinectFusion 解析
• RGB-D 实时三维重建/SLAM 中的 ICP 算法解析
• KinectFusion 中的 ICP 算法 GPU 代码解读

3.《视觉SLAM十四讲》

4.双边滤波器的原理及实现