KinectFusion: Real-time dense surface mapping and tracking
论文链接：https://ieeexplore.ieee.org/document/6162880
参考视频：KinectFusion 和 ElasticFusion 三维重建方法-付兴银https://www.bilibili.com/video/av6060335/
参考博文：https://www.cnblogs.com/zonghaochen/p/8325905.html
https://zhuanlan.zhihu.com/p/35894630

这是RGBD实时三维重建的开山之作，去年课上分享了这篇论文，好久没看都忘差不多了，整理成博文，加深一下印象。

1. 预备知识

1.1 两代Kinect相机

Kinect V1是采用结构光的方式获取深度，读取投射的红外线pattern，通过pattern的变形来取得Depth的信息。为此，Depth传感器分为投射红外线pattern的IR Projector（左）和读取的这个的IR Camera（右）。还有Depth传感器中间还搭载了Color Camera。

Kinect V1

Kinect V2是采用TOF(time of flight)，通过从投射的红外线反射后返回的时间来取得Depth信息。Depth传感器看不到外观，不过Color Camera旁边是红外线Camera(左)和投射脉冲变调红外线的Porjector（右）。

Kinect V2

详细关于Kinect的细节可以参考https://www.cnblogs.com/TracePlus/p/4136297.html

1.2 对原始深度图进行双边滤波

双边滤波

作用是保留梯度的情况下，滤除高频噪声

1.3 ICP(Iterative Closest Point)

已知两组3D点云，用于计算两组点云的位姿变换（R,T）。
传统的ICP需要知道两组点云，一组点云的各个点在另一组点云中的最近点，才能计算。遍历另一组点云寻找最近点，最差会需要(^2)复杂度，即使使用一些数据结构来降低复杂度（如KD-Tree、Octave-Tree），也至少需要(())的时间复杂度，直接导致算法无法实时运行。

point-to-point ICP

KinectFusion采用了更快的投影法ICP，直接利用位姿变换关系得到的对应点作为最近点。

point-to-plane ICP

极小化点到平面距离计算pose

其中和是待求解的位姿，和分别是原始点和目标点，是目标点的法向量。
kinectFusion采用frame-to-model的方式，而不是frame-to-frame的方式计算两帧位姿，论文里也验验证了采用frame-to-model的形式重建更加精确。

1.4 TSDF(truncated signed distance function)截断符号距离函数

一张深度图反投影变换到三维空间中的每个三维点都对应着原深度图中的每一个像素点，当点云比较稠密时，可以将该点云看成一个三维曲面，也就是(, , )=0。
Signed distance就是带正负符号的距离，相机的光轴为z轴，对于一个三维点 $\left(_0, _0, _0\right)$ ，记 $= \left(_0, _0, _0\right)$ ，如果它在平面前面，则的值为正，否则为负。
Truncated就是截断，当|| > max ⁡ 时，则||=max ⁡ 。

TSDF

1.5 Volumetric Representation

TSDF模型将整个待重建的三维空间划分成网格，每个网格中存储了数值，并且隐含表示了重建好的表面。

网格模型

网格模型中值的大小代表网格离重建好的表面的距离，如上图表示的是重建的一个人的脸（网格模型中值为 0 的部分，红线表示重建的表面，示意图给出的二维信息，实际是三维的），重建好的表面到相机一侧都是正值，另一侧都是负值，网格点离重建好的表面距离越远绝对值越大，在网格模型中从正到负的穿越点表示重建好的场景的表面。

2. 算法框架

主要可以分为四个步骤：

Overall system workflow

1）读入的深度图像转换为三维点云并且计算每一点的法向量；
2）计算得到的带有法向量的点云，和通过光线投影算法根据上一帧位姿从模型投影出来的点云，利用ICP算法配准计算位姿；
3）根据计算得到的位姿，将当前帧的点云融合到网格模型中去；
4）根据当前帧相机位姿利用光线投影算法从模型投影得到当前帧视角下的点云，并且计算其法向量，用来对下一帧的输入图像配准。

3. Method

3.1 符号约定

3.2 Surface measurement 表面测量

3.3 Mapping as Surface Reconstrution

输入：给定当前k时刻相机的位姿_(,)，和k-1帧的global TSDF地图。
输出：将当前一帧的深度图融合到当前第k时刻的TSDF地图中。

下图为计算每个体素的TSDF值。首先通过相机内外参将体素坐标p点变换到像素坐标系，得到像素坐标；然后根据像素坐标得到其深度 $R_k(x)$ ；然后将体素p与相机的距离与 $R_k(x)$ 比较，再通过截断函数，就得到每个体素坐标p的TSDF值。

通过加权的方式更新TSDF值，其中权重与入射的角度有关。

计算每个体素的TSDF值

TSDF融合方法

3.4 估计相机位姿

3.4.1 问题与解决方法

问题描述：已知k-1时刻的 global TSDF（即上一节中最后得到的update 后的TSDF ）和位姿估计 $\left(,−1\right)$ 以及k时刻的深度信息 $_$ ，估计k时刻的位姿 $\left(,\right)$ 。
解决方法：
- 根据k-1时刻的global TSDF得到surface prediction;
- 利用 k 时刻的surface measurement与上一步中surface prediction，使用ICP算法预测k时刻的位姿。（这就是frame to model）

3.4.2 Surface Prediction from Ray Casting the TSDF

输入：根据TSDF construction得到的 $_ \left(\right)$ 和 $_ \left(\right)$ ，以及k时刻的相机位姿 $\left(,\right)$ 。（从model中得到预测表面，而不是使用frame的表面）
输出：预测此时相机看到的 $_= 0$ 的位置，并且将predicted surface存储在vertex map和normal map。
方法：ray casting（光线透射）

光线投射法

在世界坐标系下进行：
光线从像素u出发,沿着方向 $_{,} ^{-1} \dot{}$
直到遇见zero-crossing（TSDF从正变到负），则认为找到了visible surface，添加到vertex map和normal map
这是原理上寻找intersection的方法，实践中还用到了interpolation以及增加march along the ray的步进距离（因为一次前进一个voxel的效率比较低，可以使用一个小于μ的step加速寻找过程。
法向量通过梯度计算得到：
$R_{g,k}\hat{N}_k = \hat{N}^g_k\left(u\right)=v\left[ \nabla F\left(p\right)\right] , \nabla F = \left[ \frac{\partial F}{\partial x},\frac{\partial F}{\partial y},\frac{\partial F}{\partial z}\right]$

3.4.3 相机位姿估计

输入：上一帧计算得到的predicted surface $\left[ \hat{} ^g_{−1} \left( \hat{} \right),\hat{N} ^g_{−1} \left( \hat{} \right) \right]$ , 当前k时刻深度图通过surface measurement计算得到的 $\left[ \hat{} ^g_{} \left( \hat{} \right),\hat{N} ^g_{} \left( \hat{} \right) \right]$
输出：当前k时刻的相机位姿估计 $_{,}$

上面代入的是point-to-plane ICP的公式。
这里的vertex并不是直接用k-1帧深度图的结果，而是用第k-1帧融合TSDF后的预测值。如果直接使用第k-1帧深度图的数据，那就是frame-to-frame，会带来累计误差，而实际采用的是frame-to-model的方式，误差会小很多。

4. 实验

作者在实验部分主要探究了KinectFusion在未引入显式的全局优化的条件下从包含回环的轨迹中构建尺度一致的地图的能力；在不同处理及存储资源大小情况下保持地图准确性的能力。
作者将Kinect置于一个固定的位置并观察一个旋转的桌面。观察过程共采集了560帧图像（用时约19秒），实验中选取256^{3个体素用于表示3m}3的空间。

移动Kinect并接收560*4帧图像所生成的模型。此次重建质量优于前述第四次实验，原因为更大范围的观察角度以及更为独立的图像数据。（第四次实验仅仅是将560帧图像运用四次。）

上述实验表明frame-to-model的跟踪方法远优于frame-to-frame，且该方法在没有引入全局联合优化的情况下也能建立尺度一致的模型。
接下来的实验作者将560帧图像以每6帧使用一次降采样，并将重建的分辨率减至64^3/3m3（将使用的显存减少了64倍）。得到的结果如上图所示。建立该模型的计算及存储资源大幅减少，然而重建质量却没有大幅下降。

在接下来的实验中作者展示了在室内光照条件下深度信息的鲁棒性，相机快速运动时运动跟踪的鲁棒性。当传感器的视野由一个巨大的平面场景覆盖时，KinectFusion将会失效。原因在于平面场景仅能提供相机姿态的三个自由度的信息。

KinectFusion论文品读