KinectFusion: Real-Time Dense Surface Mapping and Tracking 论文解析

1. 概述

Kinect Fusion 是第一个系统利用RGBD相机在房间大小的范围实现具有 real-time, dense volumetric reconstruction特性的三维重建。并且因为具有稠密的深度信息，所以在黑暗的情况下，也可以实现三维重建。本文的创新点是用了tracking的方法来吧整个地图的surface model进行了融合，而不是进行帧与帧之间的融合。同时，他们也使用了并行GPU系统来加速。

2. 实现方法

现在我们详细看一下Kinect Fusion的流程和实现方案，如下图所示。

还系统主要分为4个部分，Measurement, Pose Estimation, Update Reconstruction 和 Surface Prediction。我们将在后面详细介绍每一部分的功能以及实现方法。首先我们先来了解一下本文的符号系统。

2.1 Preliminaries

$$T_{g,k}=\left[\begin{array}{cc}{R}_{g,k}& t_{g,k}\\ {\text{0}}^T& 1\end{array}\right]\in \mathbb{S}{\mathbb{E}}_3\text{\hspace{1em}(3)}$$
一个在第k帧相机坐标系下的点${\text{p}}_k\in {\mathbb{R}}^3$转到世界坐标系${\text{p}}_g=T_{g,k}{\text{p}}_k$

$\pi ()$函数是把齐次坐标系下的坐标转为非齐次坐标。例如：$p\in {\mathbb{R}}^3=[x,y,z{]}^T$则$q\in {\mathbb{R}}^2=[x/z,y/z{]}^T$。

同时，我们把坐标上面有“点”的坐标表示为齐次坐标，例如 $\dot{\text{u}}=[{\text{u}}^T|1{]}^T$。

2.2 Surface Measurement

在第k帧像素包含的深度信息用$R_k(\text{u})\in \mathbb{R}$表示，像素坐标则是$\text{u}=[u,v{]}^T\in \mathbb{U}\in {\mathbb{R}}^2$。$K$为相机calibration matrix，则第k帧像素$\text{u}$所对应的三维空间点在相机坐标系下的坐标为${\text{p}}_k=R_k{K}^{-1}\dot{\text{u}}$。

首先我们先对原始的深度图$R_k$进行bilateral filter操作 （bilateral filter可以去除噪音并且不会把图像中的边界变得模糊，具体工作原理我还没有研究，有时间的话补上）

$$D_k(\text{u})=1/{\text{W}}_p\sum _{\text{q}\in \mathbb{U}}{N}_{{\sigma }_s}(||\text{u}-\text{q}|{|}_2)N_{{\sigma }_s}(||R_k(\text{u})-R_k(\text{q})|{|}_2)R_k(\text{q})$$其中$N_{{\sigma }_s}(t)=exp(-t^2{\sigma }^{-2})$，${\text{W}}_p$是归一化参数。

那么我们利用这个新的深度图可以重新计算像素$\text{u}$所对应的相机坐标系下三维空间坐标是${\text{V}}_k(u)=D_k(\text{u})K^{-1}\dot{\text{u}}$。

因此，我们可以利用像素$\text{u}$附近的点来计算三维空间中surface在该点的法向量，公式为
$$N_k(\text{u})=v[({\text{V}}_k(\text{u}+1,v)-{\text{V}}_k(u,v))\times ({\text{V}}_k(\text{u}-1,v)-{\text{V}}_k(u,v))]$$其中$v[\text{x}]=\text{x}/||x|{|}_2$，对向量进行归一化处理。另外，如果一个像素点有深度信息，我们用$M_k(\text{u})\to 1$表示，否则为$M_k(\text{u})\to 0$。

我们还用一种$\text{L}=3$ 的multi-scale的表示方法来表示surface的测量结果。首先是深度图， D l ∈ { 1 … L } D^{l\in\{1\dots L\}} Dl∈{1…L}。$D_k^1$是$R_k$经过bilateral filter的结果。而$D^{l+1}$是通过对$D^l$进行二次采样，分辨率降为$D^l$一半的过程。另外，只有像素$D_k(\text{u})$附近的平均深度值在其值的$3{\sigma }_r$以内，才使用其平均值，否则会把原本不平滑的边界也变得平滑。已知 D l ∈ { 1 … L } D^{l\in\{1\dots L\}} Dl∈{1…L}后，我们可以利用之前的公式计算对应的 V l ∈ { 1 … L } \textbf{V}^{l\in\{1\dots L\}} Vl∈{1…L}和 N l ∈ { 1 … L } \textbf{N}^{l\in\{1\dots L\}} Nl∈{1…L}。同时已知k帧和世界坐标系的transformation matrix $T_{g,k}$后，则有${\text{V}}_k^g(\text{u})=T_{g,k}{\dot{\text{V}}}_k(\text{u})$，${\text{N}}_k^g(\text{u})=R_{g,k}{\text{N}}_k(\text{u})$。

至于为什么此处要设置multi-scale我还没有很好的理解，欢迎懂得朋友评论

2.3 Mapping as Surface Reconstruction

本文是通过把连续的深度图和相机位姿不断通过使用volumetric truncated signed distance function (TSDF)来融合成为一个单一的三维重建地图。（本文不会对TSDF过多的展开介绍，想要了解可参考论文 “A volumetric method for building complex models from range images”）

下面两张图可以让我们对TSDF有个较为直观的初步了解。Figure3 表示如果对同一方向进行了2次测量，得到了2个signed distance function，那么我们可以使用其weighted funciton对这两次测量进行融合。

现在我们开始详细介绍一下Kinectic Fusion中如何得到Signed distance function以及对应的weighted function。世界坐标系下的TSDF是由$1\dots k$帧的深度测量融合起来的。我们用${\text{S}}_k(\text{p})$表示，$\text{p}\in {\mathbb{R}}^3$是世界坐标系下的三维点。每个点$\text{p}$在TSDF中有对应的 signed distance value 和 weight:
$${\text{S}}_k(\text{p})\to [F_k(\text{p}),W_k(\text{p})]$$
为了实现一个dense的surface测量，我们做出了以下2个假设：

1. 我们要把测量点的不确定性做一个截断（truncate）。如果一个三维空间点到相机中心所在surface的深度测量值为d，那么我们认为这个点一定在$[d-\mu ,d+\mu ]$的区间内（$d=||K^{-1}\dot{\text{u}}|{|}_2{R}_k(\text{u})$）。$\text{r}$是图像平面像素点到该三维点的射线，当$||\text{r}|{|}_2<(d-\mu )$时，是free space。当$||\text{r}|{|}_2>(d+\mu )$时，是我们不了解的空间范围。
2. 因此，SDF只存在于这个不确定范围的测量值 $|||\text{r}|{|}_2-d|<\mu$。

虽然可以具体的计算真正的离散SDF值，但是为了提高计算效率，本文用了一个别的方案来计算，公式如下：

概括来说，该公式(6)计算的是一个三维点$\text{p}$在投影到第k帧图像后的SDF value。其中公式(8)的那个最外层括号里面计算的是 $\text{p}$在k帧对应的真是像素点，但因为计算结果是连续的值，我们需要在k帧图像的离散像素点中找到与其最近的位置作为$\text{p}$的对应像素，最外层括号即表示这个查找操作。公式(9)是截断函数，保证了点$\text{p}$的深度与$R_k(\text{x})$的距离不超过$\mu$。$\text{p}$点在第k帧深度图对应的weight $W_{R_k}(\text{p})$和$cos(\theta )/R_k(\text{x})$呈正相关， $\theta$是前面$\text{r}$的方向与surface在$\text{p}$点的法向量夹角。

获得每帧的SDF的值后，我们把每帧融合在一起用最小二乘法表示的形式为：
$$\underset{F\in \mathbb{F}}{\min }\sum _k||W_{R_k}{F}_{R_k}-F|{|}_2$$
它的解可以用迭代的方式表示，定义每个在世界坐标系下的三维点 { p ∣ F R k ( p ) ≠ n u l l } \{\textbf{p}|F_{R_k}(\textbf{p})\neq null\} {p∣FRk​​(p)​=null}则

$$F_k(\text{p})=\frac{W_{k-1}(\text{p})F_{k-1}(\text{p})+W_{R_k}(\text{p})F_{R_k}(\text{p})}{W_{k-1}(\text{p})+W_{R_k}(\text{p})}$$
$$W_k(\text{p})=W_{k-1}(\text{p})+W_{R_k}(\text{p})$$

另外，使用$W_{\eta }$对weight进行一个截断操作，移动平均表面重建可以在具有动态物体运动的场景中进行重建。（我并不明白这句话什么意思，原文是:“While $W_k(\text{p})$ provides weighting of the TSDF proportional to the uncertainty of surface measurement, we have also found that in practice simply letting $W_{R_k}(\text{p})=1$, resulting in a simple average, provides good results. Moreover, by truncating the updated weight over some value $W_{\eta }$, a moving average surface reconstruction can be obtained enabling reconstruction in scenes with dynamic object motion.”）
$$W_k(\text{p})\leftarrow min(W_{k-1}(\text{p})+W_{R_k}(\text{p}),W_{\eta })$$

2.4 Surface Prediction form Ray Casting the TSDF

因为之前所获得的点$\text{p}$的SDF值$F_k(\text{p})$不一定为0，而表面上的点的SDF值应当为0。所以我们要利用已有的$\text{p}$和$F_k(\text{p})$，把SDF为0的点${\hat{\text{V}}}_k$以及对应的法向量${\hat{\text{N}}}_k$求出来。如何求解${\hat{\text{V}}}_k$可以查看论文“Interactive ray tracing for isosurface rendering”。如果已知一个在世界坐标系下的点$\text{p}$使得$F_k(\text{p})=0$，则$\text{p}$点的法向量为

详细证明可参考论文"Interactive ray tracing for isosurface rendering"。

2.5 Sensor Pose Estimation

利用了类似于ICP的算法来求解位姿，此处不再赘述。值得注意的是，此处会检查求解是是否有充足的约束条件以及设置阈值来防止求解失败，如果失败，则会启动重定位程序。重定位过程中，使用丢失前的位姿来对当前帧初始化，然后通过优化器求解位姿，再判断该解是否有效，如果有效则完成重定位。

总结

Kinect在$\le 7m^3$的范围表现不错，但是在大场景下，目前使用的dense volumetric representation会占用大量内存，同时不可避免地产生累积误差。所以要使用submaps和改进储存方式来优化内存占用。2020年的一篇论文刚好对这些方面做出了改进，实现了大场景应用：“Elastic and Efficient LiDAR Reconstruction for Large-Scale Exploration Tasks”。