KinectFusion算法解读

R. A. Newcombe et al., “KinectFusion: Real-Time Dense Surface Mapping and Tracking∗,” p. 10.
KinectFusion就不需要多废话介绍了吧

算法结构

• 曲面测量：预处理过程，由原始的深度测量生成稠密的顶点和法线金字塔
• 曲面重建更新：全局场景融合过程，通过跟踪新的一帧深度数据得到的位姿，将曲面测累加到场景model中，由TSDF表示
• 曲面预期：并不是使用帧到帧的位姿估计，是使用帧到model的位姿估计。通过将符号距离函数raycasting到估计帧中来提供一个稠密曲面预期
• 传感器位姿估计：通过在当前帧和期望的曲面之间进行多尺度的ICP进行

曲面测量

在时刻$k$时获得一帧原始深度图$\mathbf{R}_k$，在图像域$\mathbf{u} \in \mathscr{U} \subset \mathbb{R}^2$中的每个像素$\mathbf{u}=(u,v)^\top$处都提供对应的已标定深度测量$\mathbf{R}_k(\mathbf{u}) \in \mathbb{R}$。则有$\mathbf{p}_k = \mathbf{R}_k(\mathbf{u}) \mathbf{K}^{-1} \dot{\mathbf{u}}$为帧$k$的一个测量点。在原始深度图上进行双边滤波，以此得到降低噪声后的深度图$\mathbf{D}_k$，

$\mathbf{D}_k (\mathbf{u}) = \frac{1}{W_\mathbf{p}} \sum_{\mathbf{q} \in \mathscr{U}} \mathscr{N}_{\sigma_s} (\lVert \mathbf{u} - \mathbf{q} \rVert _2) \mathscr{N}_{\sigma_r} (\lVert \mathbf{R}_k(\mathbf{u}) - \mathbf{R}_k(\mathbf{q}) \rVert_2) \mathbf{R}_k(\mathbf{q})$

其中$\mathscr{N}_{\sigma}(t) = \exp(-t^2\sigma^{-2})$，$W_{\mathbf{p}}$是归一化常量。$\dot{\mathbf{u}}$表示齐次坐标，$\mathbf{q} = \pi (\mathbf{p})$表示透视投影$\mathbf{p} \in \mathbb{R}^3 = (x,y,z)^\top, \mathbf{q} \in \mathbb{R}^2 = (x/z, y/z)^\top$。

顶点和法线map

将滤波后的深度图反投影到传感器所在参考帧可以获得顶点map $\mathbf{V}_k$，公式(1)

$\mathbf{V}_k (\mathbf{u}) = \mathbf{D}_k(\mathbf{u}) \mathbf{K}^{-1}\dot{\mathbf{u}}$

在顶点map的领域中使用叉乘得到对应的法向量，公式(2)

$\mathbf{N}_k (\mathbf{u}) = \mathbf{v} \lbrack (\mathbf{V}_k(u+1, v)-\mathbf{V}_k(u,v)) \times (\mathbf{V}_k(u,v+1) - \mathbf{V}_k(u,v)) \rbrack$

其中$\mathbf{v} \lbrack \mathbf{x} \rbrack = \mathbf{x} / \lVert \mathbf{x} \rVert _2$。同时定义了顶点有效性mask：对于每一个深度测量能转换为有效顶点的像素$\mathbf{M}_k(\mathbf{u}) \mapsto 1$，反之如果深度测量值丢失的话$\mathbf{M}_k(\mathbf{u}) \mapsto 0$。

曲面金字塔

计算由顶点和法线map金字塔表示的$L=3$多尺度曲面测量。首先计算深度图金字塔$\mathbf{D}^{l \in [1 \dots L]}$。设底层的深度图为原始的双边滤波后深度图，下采样版本$\mathbf{D}^{l+1}$从$\mathbf{D}^l$通过块平均下采样到一半分辨率。深度值使用平均值，仅当和中心像素的值在$3\sigma_r$之内时，保证平滑不会跨越深度边界。
顶点和法线map金字塔的每一层$\mathbf{V}^{l \in [1 \dots L]}, \mathbf{N}^{l \in [1 \dots L]}$使用该层的深度图和公式(1)(2)进行计算。将给定的相机到全局坐标系的仿射变换矩阵$\mathrm{T}_{g,k}$使用在曲面测量上，可以得到全局顶点为$\mathbf{V}^g_k(\mathbf{u}) = \mathrm{T}_{g,k} \dot{\mathbf{V}}_k(\mathbf{u})$，映射到全局中的法线为$\mathbf{N}^g_k(\mathbf{u}) = \mathrm{R}_{g,k} \mathbf{N}_k(\mathbf{u})$，其中

$\mathrm{T}_{g,k} = \begin{bmatrix} \mathrm{R}_{g,k} & \mathbf{t}_{g,k} \\ \mathbf{0}^\top & 1 \end{bmatrix} \in \mathbb{SE}_3$

TSDF

每一个连续的深度帧，使用其对应的相机位姿，增量式融合到一个用TSDF表示的单一3D重建中。将融合了配准后帧$1 \dots k$深度测量值的TSDF表示为$\mathbf{S}_k(\mathbf{p})$，其中$\mathbf{p} \in \mathbb{R}^3$是将被重建的3D volume中全局帧的点。TSDF点每个位置都要保存两个元素：当前的截断符号距离值$\mathbf{F}_k(\mathbf{p})$和权重$\mathbf{W}_k(\mathbf{p})$，

$\mathbf{S}_k(\mathbf{p}) \mapsto \lbrack \mathbf{F}_k(\mathbf{p}) , \mathbf{W}_k(\mathbf{p}) \rbrack$

截断

稠密的曲面测量在曲面重建中提供了两个重要约束。首先，假设可以截断深度测量的不确定性，比如真值在测量值的$\pm \mu$之间。那么对于从相机中心沿着深度图射线的距离$r$来说，$r < (\lambda \mathbf{R}_k(\mathbf{u}) - \mu)$是自由空间(free space)，其中$\lambda = \lVert \mathbf{K}^{-1} \dot{\mathbf{u}} \rVert _2$。其次，假设在$r > (\lambda \mathbf{R}_k(\mathbf{u}) + \mu)$的范围内无法获取曲面信息。因此仅仅需要表达曲面测量在$| r - \lambda \mathbf{R}_k (\mathbf{u}) | \leq \mu$范围内的不确定性的区域。可见空间的点到曲面上最近点的距离大于$\mu$时被截断为最大距离$\mu$，不可见的点距离超过$\mu$时不被测量。符号距离函数表达了到曲面最近点的距离。

投影截断符号距离函数

对于一个已知位姿$\mathrm{T}_{g,k}$的原始深度图$\mathbf{R}_k$，在全局帧中点$\mathbf{p}$处的投影TSDF$[\mathbf{F}_{\mathbf{R}_k}, \mathbf{W}_{\mathbf{R}_k}]$为

$\begin{aligned} \mathbf{F}_{\mathbf{R}_k}(\mathbf{p}) &= \Psi \left( \lambda^{-1} \lVert \mathbf{t}_{g,k} - \mathbf{p} \rVert_2 - \mathbf{R}_k(\mathbf{x}) \right) \\ \lambda &= \lVert \mathbf{K}^{-1} \dot{\mathbf{x}} \rVert_2 \\ \mathbf{x} &= \lfloor \pi \left( \mathbf{K} \mathrm{T}_{g,k}^{-1} \mathbf{p} \right) \rfloor \\ \Psi (\eta) &= \begin{cases} \min (1, \frac{\eta}{\mu}) \text{sgn}(\eta) &\text{iff } \eta \geq -\mu \\ null & otherwise \end{cases} \end{aligned}$

$\lfloor \cdot \rfloor$为最近邻查找，而不使用深度值内插。$\lambda$为到归一化平面上的距离。$\Psi$进行截断。$\mathbf{t}_{g,k}$为当前相机坐标，$\lVert \mathbf{t}_{g,k}-\mathbf{p} \rVert_2$为点在当前坐标系中的坐标。相关推导可以结合公式(1)进行。

相关权重$\mathbf{W}_{\mathbf{R}_k} (\mathbf{p})$与$\cos (\theta) / \mathbf{R}_k (\mathbf{x})$成比例，$\theta$是局部帧中像素射线方向和曲面法线方向的夹角。

投影TSDF仅仅在$\mathbf{F}_{\mathbf{R}_k}(\mathbf{p}) = 0$或者仅仅存在孤立的点(之前在此处没有测量值)时恰好有效。

融合

所有深度图的全局融合是通过将每个深度图分别计算得到的所有独立TSDF进行加权平均，可以被看作由多个噪声的TSDF测量进行去噪得到全局的TSDF。在$\mathscr{L}_2$范数下的去噪(融合)曲面结果为作为$0$交界的逐点符号距离函数F最小化：

$\min_{\mathrm{F} \in \mathscr{F}} \sum_k \lVert \mathbf{W}_{\mathbf{R}_k} \mathbf{F}_{\mathbf{R}_k} - \mathrm{F} \rVert _2$

这个解可以由更多数据项使用简单的加权平均增量获得，在每个点$\{ \mathbf{p} | \mathbf{F}_{\mathbf{R}_k}(\mathbf{p}) \neq null \}$处定义：

$\begin{aligned} \mathbf{F}_k(\mathbf{p}) &= \frac{\mathbf{W}_{k-1}(\mathbf{p}) \mathbf{F}_{k-1}(\mathbf{p}) + \mathbf{W}_{\mathbf{R}_k}(\mathbf{p}) \mathbf{F}_{\mathbf{R}_k}(\mathbf{p})}{\mathbf{W}_{k-1}(\mathbf{p}) + \mathbf{W}_{\mathbf{R}_k}(\mathbf{p})} \\ \mathbf{W}_{k}(\mathbf{p}) &= \mathbf{W}_{k-1}(\mathbf{p}) + \mathbf{W}_{\mathbf{R}_k}(\mathbf{p}) \end{aligned}$

实践中发现，权重$\mathbf{W}_{\mathbf{R}_k}(\mathbf{p}) = 1$时也能提供很好的结果。并且，将更新后超过某个值$\mathbf{W}_\eta$的权重截断

$\mathbf{W}_{k}(\mathbf{p}) \leftarrow \min (\mathbf{W}_{k-1}(\mathbf{p}) + \mathbf{W}_{\mathbf{R}_k}(\mathbf{p}) , \mathbf{W}_\eta)$

在有动态目标运动的场景中，可以获得运动的平均曲面重建结果。 原始的深度测量值被用来进行TSDF融合，双边滤波的版本被用在跟踪中。 滤波后移除了高频部分，类似噪声的部分丢失会导致重建更好结果的能力下降，因为细节很多都在高频的部分。

对TSDF进行raycasting得到期望曲面

计算一个稠密点期望曲面，通过将零水平集$\mathbf{F}_k = 0$渲染到当前位姿$\mathrm{T}_{g,k}$的虚拟相机中。期望曲面储存为(这是重要的，估计位姿的数据关联就是将这些顶点投影到下一帧中)参考帧$k$中顶点和法线map $\hat{\mathbf{V}}_k$和$\hat{\mathbf{N}}_k$，并用于相机位姿估计。

当有了全局SDF形式表示的稠密曲面重建之后，便可以对每个像素进行raycast。从像素最小的深度开始，沿着像素对应的射线$\mathrm{T}_{g,k}\mathbf{K}^{-1}\dot{\mathbf{u}}$，直到零交界处(可视曲面的$+ve$到$-ve$范围)。当到达$-ve$到$+ve$范围的背面或者存在正在工作的volume时同样也停止，这两种情况将会导致像素$\mathbf{u}$处没有曲面测量值。

在曲面界面$\mathbf{F}_k(\mathbf{p}) = 0$上或者离其非常近的点，TSDF点梯度在该处被假设为正交于零水平集，所以与像素$\mathbf{u}$相关点$\mathbf{p}$处的曲面法线可以从$\mathbf{F}_k$直接用SDF的数值导数计算：

$\begin{aligned} \mathrm{R}_{g,k} \hat{\mathbf{N}}_k &= \hat{\mathbf{N}}^g_k (\mathbf{u}) = \mathbf{v} [\nabla \mathbf{F}(\mathbf{p})] \\ \nabla \mathbf{F} &= \lbrack \frac{\partial \mathbf{F}}{\partial x} , \frac{\partial \mathbf{F}}{\partial y}, \frac{\partial \mathbf{F}}{\partial z} \rbrack ^\top \end{aligned}$

相关原理可以参考原文的参考文献[4]，就是算当前帧的像素对应更新后曲面上的哪个点。

相机位姿估计

实时相机定位包含对每一帧新的深度图估计当前相机位姿$\mathrm{T}_{g,k} \in \mathbb{SE}_3$。在这个工作中，使用两个因素带来的优势，使用深度图中所有的数据进行一个基于稠密ICP的位姿估计。首先，由于很高的跟踪帧率，可认为相邻两帧之间的运动非常小。其次，现代GPU硬件可以实现高度并行化计算。该系统中通过align实时曲面测量$(\mathbf{V}_k, \mathbf{N}_k)$到由前一帧得到的$(\hat{\mathbf{V}}_{k-1}, \hat{\mathbf{N}}_{k-1})$模型预期。
利用期望曲面，估计相机位姿$\mathrm{T}_{g,k}$的全局点面能量函数为

$\mathbf{E}(\mathrm{T}_{g,k}) = \sum_{ \begin{aligned} \mathbf{u} &\in \mathscr{U} \\ {\Omega}_k(\mathbf{u}) &\neq null \end{aligned}} \lVert \left( \mathrm{T}_{g,k} \dot{\mathbf{V}}_k(\mathbf{u}) - \hat{\mathbf{V}}^g_{k-1}(\hat{\mathbf{u}}) \right)^\top \hat{\mathbf{N}}^g_{k-1}(\hat{\mathbf{u}}) \rVert_2$

通过计算透视投影点，使用投影数据关联算法(就是将储存为期望曲面的顶点投影到该帧上)得到一系列顶点对应$\{ \mathbf{V}_k(\mathbf{u}), \hat{\mathbf{V}}_{k-1}(\mathbf{\hat{u}}) | \Omega(\mathbf{u}) \neq null \}$。设定阈值处理特别不正确的对应

$\Omega(\mathbf{u}) \neq null \text{ iff} \begin{cases} \mathbf{M}_k(\mathbf{u}) &= &1 ,&\text{and}\\ \lVert \tilde{\mathrm{T}}^z_{g,k} \dot{\mathbf{V}}_k(\mathbf{u}) - \hat{\mathbf{V}}^g_{k-1}(\hat{\mathbf{u}}) \rVert &\leq &\varepsilon_d ,& \text{and} \\ \langle \tilde{R}^z_{g,k} \mathbf{N}_k(\mathbf{u}), \hat{\mathbf{N}}^g_{k-1}(\mathbf{\hat{u}}) \rangle &\leq &\varepsilon_\theta .& \end{cases}$

$\tilde{\mathrm{T}}^{z=0}_{g,k}$由前一帧的位姿$\mathrm{T}_{g,k}$初始化。