三维重建入门第一篇：KinectFusion论文精读，原理、流程详解，帮你看懂这篇文章

一、简介

本文主要讲述KinectFusion论文里本身的原理与做法，至于它做的不太好或者可能有问题的地方，这篇文章就暂时不展开讨论了了。
本文对于公式也不作推导，想了解详细推导的，看我这篇：KinectFusion阅读笔记 公式详解
我也是刚入门的菜鸟，啃了一周才把这篇论文啃下来，写的东西不一定会很深，希望对大家有帮助，大家有啥不懂的也可以在评论区与我讨论，同时也希望大佬们能指出我文章的问题，谢谢~！

KinectFusion论文的地址是：https://www.microsoft.com/en-us/research/wp-content/uploads/2016/02/ismar2011.pdf
它主要讲述了使用 移动的深度相机 实时(real-time)重建 复杂的室内的静态场景 算法。

---

二、前置知识

1. 矩阵基本运算，特征分解、SVD分解、QR分解等。
2. 欧拉角，四元数(非必须，但对之后进阶版的dynamicFusion等比较有用)
3. 三维旋转与平移矩阵，相机模型等等。 (不太了解的可以看我这篇里的1.1~2.1：VSLAM学习(一) 三维运动、相机模型、SLAM模型)

---

三、算法详解

3.1 思想概述

用通俗一点的话来讲，这篇文章旨在解决以下俩问题

• 如何表示 或者说 使用什么数据结构来表示 整个空间里哪些地有物体哪些地方是空气。
• 相机在某个时刻只能看到物体某一个方向的表面，如何看到其他方向的表面，看到了以后又如何将他们融合在一起？

对于这两个问题，我们跟着论文思路，慢慢来解决。

3.2 流程说明

首先看下论文里的这张整体框架流程图（Figure 3）：

整个实现步骤分为4步：

1. Measurement - 通过深度相机获取的深度图来计算所看到的表面的空间坐标以及法线向量。
2. Pose Estimation - 通过ICP算法来计算当前相机所处位姿。
3. Update Reconstruction - 通过相机当前位姿与其他信息结合 计算当前的全局TSDF值
4. Surface Prediction - 通过ray-cast方法来预测当前平面。

当然，第一次直接看这个图基本就是蒙的，因为里面好多名词：ICP、TSDF、Ray-cast 等等根本不认识。
下一节中，我一个个的来说明一下

3.3 涉及到的方法名词及算法解释

如果已经了解这些算法可以直接跳过这一节。

3.3.1 ICP

ICP是一种点云匹配算法，在KinectFusion中主要用来确定当前相机的三维空间位姿。
关于ICP我已经写过一篇：ICP算法思想与推导详解。不了解ICP的直接看这一篇即可。

KinectFusion这一篇使用的ICP方案是 投影(Projective)对应点 + 点到面(Point-to-Plane) 的匹配
Point-to-Plane的目标函数为
$$\arg \underset{\mathbf{R},\mathbf{t}}{\min }\frac{1}{2}\sum _{i=1}^n\Vert [{\mathbf{q}}_i-(\mathbf{R}{\mathbf{p}}_i+\mathbf{t})]\cdot {\mathbf{n}}_i{\Vert }^2$$
其实无非就是将目标点${\mathbf{q}}_i$和估计点$\mathbf{R}{\mathbf{p}}_i+\mathbf{t}$的差投影到了法向量${\mathbf{n}}_i$方向上，再去求解最优$\mathbf{R},\mathbf{t}$值

对应论文中的公式(16)
∑ u ∈ U Ω k ( u ) ≠ n u l l ∥ ( T g , k V ˙ k ( u ) − V ^ k − 1 g ( u ^ ) ) T N ^ k − 1 g ( u ^ ) ∥ 2 (16) \sum_{

$$\begin{subarray}{c} \boldsymbol u\in\mathscr U \\ \Omega_k(\boldsymbol u)\neq null \end{subarray}$$ } \Bigg\|\Big(\boldsymbol T_{g,k}\dot{\boldsymbol V}_k(\boldsymbol u)-\hat{\boldsymbol V}_{k-1}^g(\hat{\boldsymbol u})\Big)^\mathsf T\hat{\boldsymbol N}_{k-1}^g(\hat{\boldsymbol u})\Bigg\|_2 \tag{16} u∈UΩk​(u)=null​∑​ ​(Tg,k​V˙k​(u)−V^k−1g​(u^))TN^k−1g​(u^) ​2​(16)

这里他是将 第$k-1$帧的融合点云 ${\hat{\mathbf{V}}}_{k-1}^g(\hat{\mathbf{u}})$ 与 第$k$帧的深度图点云 ${\dot{\mathbf{V}}}_k(\mathbf{u})$ 进行点到面的ICP配准，法向量使用的是融合点云的法向量 ${\hat{\mathbf{N}}}_{k-1}^g(\hat{\mathbf{u}})$

我们从上面的Figure 3中可以看到（第4个方块下面的箭头）， 第$k-1$帧的融合点云 ${\hat{\mathbf{V}}}_{k-1}$ 及其法向量 ${\hat{\mathbf{N}}}_{k-1}$ 都是由 Surface Prediction 的计算得来了。具体算法在 3.3.3 中说明。

3.3.2 TSDF

TSDF全名是Truncated Signed Distance Function，即“截断的带符号的距离函数”。
它通俗的来说是一个表示空间表面信息的方法，使用一个N×N×N的三维数组，来表示整个空间里面哪里有表面哪里是空气。
网上讲TSDF的文章一搜也有一大把，讲的都还不错，看懂问题不大，比如：
https://zhuanlan.zhihu.com/p/390276710
https://blog.csdn.net/weixin_37835423/article/details/113500937
等等都还行，也可以自己搜搜别的。

TSDF在论文中对应的是公式(6)~(9)。公式看起来挺复杂，原理却比较简单。
这里我再大致说明一下TSDF的原理流程：

1. 先在所观察的空间（或者说要重建的空间）建立一个N×N的网格（三维的话就是N×N×N）
2. 取一个网格，将它映射到相机成像平面，找到对应像素；并记这个网格到相机光心的距离为$d_1$
3. 读取所找到像素的深度值（即该像素所看到的表面的深度），并记光心到表面的距离为$d_2$
4. 这个网格的TSDF值即为 $(d_2-d_1)/\mu$，当 $d_2-d_1>\mu$（或$<-\mu$） 时，TSDF的值置为1（或-1）（truncated截断的含义）
5. 重复2-4过程直到遍历所有网格

我以2d截面为例画了个动图展示了一下他的过程：

遍历完整个空间以后就是下面这张图的样子：

相机这一侧TSDF为正值，相机看不到的那一侧TSDF为负值，物体表面处的TSDF值为0。

3.3.3 Ray-cast

从Figure 3流程图中的第四个方格中可以看到，它是用来“Compute Surface Prediction”的。计算物体表面的预测值，也就是计算它的表面点云以及他的表面的方向(表面法向量)
流程大致如下：

1. 在成像平面选择某一像素，从相机出发发射过该像素点的射线（Ray casting/ Ray tracing）
2. 记录射线每个途径网格的TSDF值，直到TSDF值由正变为负的时候停止
3. 当前网格坐标即为$\hat{V}$点云中的一点，该点对应的$\hat{N}$由TSDF的梯度得到（见论文公式(14)）

也画了个动图大致流程：

图中，
绿色$V,N$是由深度图得到的点与法向量 （论文中称为 frame）
蓝色$\hat{V},\hat{N}$是由TSDF网格得到的点与法向量 （论文中称为 model）

3.3.4 Marching cubes

Marching Cubes在Figure 3流程图中并没有提到，但我要提一下
它是一个将TSDF空间可视化的算法。
它可以将TSDF的值转化为空间三角面片（mesh），从而在可视化编程中（如Open3d、Opencv viz等）将整个TSDF空间展示出来。

具体算法流程网上也挺多的，搜搜看就可以。

3.4 融合（Fusion）

知道了这些名词与算法之后，Figure 3中的第1、2、4个框基本都没问题了。
现在就剩下第3个框中的“Update Reconstruction”是如何将第2个框中算出的新TSDF 融合 到全局TSDF中了

融合方法在论文中对应公式(11)和(12)
$${\mathrm{F}}_k(\mathbf{p})=\frac{{\mathrm{W}}_{k-1}(\mathbf{p}){\mathrm{F}}_{k-1}(\mathbf{p})+{\mathrm{W}}_{{\mathrm{R}}_k}(\mathbf{p}){\mathrm{F}}_{{\mathrm{R}}_k}(\mathbf{p})}{{\mathrm{W}}_{k-1}(\mathbf{p})+{\mathrm{W}}_{{\mathrm{R}}_k}(\mathbf{p})}\text{\hspace{1em}(11)}$$
$${\mathrm{W}}_k(\mathbf{p})={\mathrm{W}}_{k-1}(\mathbf{p})+{\mathrm{W}}_{{\mathrm{R}}_k}(\mathbf{p})\text{\hspace{1em}(12)}$$

其中的符号：
${\mathrm{F}}_{{\mathrm{R}}_k}(\mathbf{p})$ 与 ${\mathrm{W}}_{{\mathrm{R}}_k}(\mathbf{p})$ 分别表示第k帧使用深度图计算得出的TSDF值与权重，
${\mathrm{F}}_k(\mathbf{p})$ 与 ${\mathrm{W}}_k(\mathbf{p})$ 分别表示第k帧融合以后的全局TSDF的值与权重。

给 ${\mathrm{W}}_{{\mathrm{R}}_k}(\mathbf{p})$ 赋值 正比于 $\cos (\theta )/{\mathrm{R}}_k(\mathbf{x})$ ，$\theta$是像素射线与表面法向量的夹角。

所以，所谓“融合”
就是将新帧的TSDF以一定的权重比例融入到之前帧的全局TSDF中去，使得全局TSDF越来越接近真实表面，也使记录的表面越来越完整。

3.5 流程总结

至此，KinectFusion整个流程已经比较清楚了

1. 在k时刻读取深度相机的深度图计算$V,N$
2. 与k-1时刻的${\hat{V}}_{k-1},{\hat{N}}_{k-1}$进行ICP配准，计算两帧之间的位姿转换矩阵，从而得到第k帧的相机位姿
3. 计算第k帧的TSDF，并将它融合至之前的全局TSDF中去
4. 对融合后的TSDF做Ray-cast计算${\hat{V}}_k,{\hat{N}}_k$
5. 来到新的k+1帧，重复上面所有步骤

---

四、实验

论文中做了很多对照实验，比如：

分别做了5组实验

1. 使用 $V_{k-1},N_{k-1}$ 做ICP匹配而不用 ${\hat{V}}_{k-1},{\hat{N}}_{k-1}$
2. 使用前 $L$ 帧做 ICP+TSDF 融合
3. 使用全部 $N$ 帧做 ICP+TSDF 融合
4. 将全部 $N$ 帧在 ICP+TSDF 融合中重复使用 $M$ 遍
5. 将真实录制帧数扩大到$MN$次

发现第1种由于累计误差，模型直接飘没了
其他几种的效果大家直接看论文Figure 8即可