DynamicFusion简要解析

DynamicFusion所做的工作：基于体素流场，将每帧场景状态变换到固定的（fixed, canonical）frame。
主要贡献：由刚体场景（KinectFusion）发展得到的，保留体素帧融合（volumetric scan fusion）最优特性的一个非刚体变换和融合的方法。
次要贡献：有效表达了体素warp，并实时计算。

Overview

算法有3个核心组成部分：

1. 估算体素model-to-frame的warp场参数
2. 利用计算得到的warp场，将实时深度图融合到canonical空间
3. 调整warp场的结构去获取新的几何

稠密非刚体warp场

使用体素warp场表示动态场景运动，每一个点都对应一个6D的变换$\mathcal{W}:\mathbf{S} \mapsto \mathbf{SE}(3)$，每一个canonical点$v_c \in \mathbf{S}$，$\mathbf{T}_{lc} = \mathcal{W}(v_c)$将点从canonical空间变换到实时非刚体形变参考帧。如：

$\begin{aligned} \left( v_t^\top, 1 \right)^\top &= \mathcal{W}(v_c) \left( v_c^\top, 1 \right)^\top \\ \left( n_t^\top, 0 \right)^\top &= \mathcal{W}(v_c) \left( n_c^\top, 0 \right)^\top \end{aligned}$

对偶四元数内插

由于直接进行稠密计算不现实，计算量太大，warp场计算使用对偶四元数内插（DQB）方式进行，warp函数定义为：

$\mathcal{W}(x_c) \equiv SE3( \mathbf{DQB} (x_c) )$

其中$SE3(\cdot)$将对偶四元数转换为$\mathbf{SE}(3)$变换矩阵，

$\mathbf{DQB} (x_c) \equiv \frac{\sum_{k \in N(x_c)} \mathbf{w}_k(x_c)\mathbf{\hat{q}}_{kc}} {\lVert \sum_{k \in N(x_c)} \mathbf{w}_k(x_c)\mathbf{\hat{q}}_{kc} \rVert}$

$\mathbf{\hat{q}}_{kc} \in \mathbb{R}^8$为单位对偶四元数，$N(x)$为点$x$的$k$最近邻变换node，$\mathbf{w}_k:\mathbb{R}^3 \mapsto \mathbb{R}$定义了决定每个node影响半径的权重。也就是点$x$的变换通过$k$最近邻node对应变换的单位对偶四元数加权和再归一化，得到对应的单位对偶四元数之后专为变换矩阵。

warp场状态表示

$t$时刻的warp场$\mathcal{W}_t$状态由$n$个形变node $\mathcal{N}^t_\mathbf{warp} = \{ \mathbf{dg}_v, \mathbf{dg}_w, \mathbf{dg}_{se3} \}_t$表示。对于每个node $i = 1 \dots n$而言，$\mathbf{dg}_v^i \in \mathbb{R}^3$为canonical帧中坐标，$\mathbf{dg}_w$为半径基权重（radial basis weight），控制warp场内插权重，

$\mathbf{w}_i (x_c) = \exp \left(- \lVert \mathbf{dg}^i_v - x_c \rVert ^2 / \left( 2 \left( \mathbf{dg}^i_w \right)^2 \right) \right)$

$\mathbf{T}_{ic} = \mathbf{dg}^i_{se3}$为node对应的变换。
对于整个warp场而言，可以分离出由于相机运动等原因造成的公共刚体变换。最后完整的warp场函数为

$\mathcal{W}_t(x_c) = \mathbf{T}_{lw} SE3( \mathbf{DQB} (x_c) )$

稠密的非刚体曲面融合

在得到model-to-frame的warp场$\mathcal{W}_t$之后，就要更新canonical模型几何了。

TSDF表示

在canonical空间$\mathbf{S}$中重建由采样$\mathbf{TSDF} \space \mathcal{V} : \mathsf{S} \mapsto \mathbb{R}^2$表示。$\mathsf{S}$在体素域中$\mathsf{S} \subset \mathbb{N}^3$，每个体素$\mathbf{x} \in \mathsf{S}$对应一个采样点$x_c$。元组$\mathcal{V}(x) \mapsto \lbrack \mathsf{v}(\mathbf{x}) \in \mathbb{R}, \mathsf{w}(\mathbf{x}) \in \mathbb{R} \rbrack ^\top$，$\mathsf{v}(\mathbf{x})$为该店所有投影$\mathbf{TSDF}$值的加权平均，$\mathsf{w}(\mathbf{x})$为相关权重和。

TSDF融合

对于给定的实时深度图$D_t$，将每个体素中心warp到实时帧中，$\left( x_t^\top, 1 \right)^\top = \mathcal{W}_t(x_c) \left( x_c^\top, 1 \right)^\top$，然后通过直接将warp之后的（体素）中心投影到该深度帧中进行$\mathbf{TSDF}$曲面融合操作。

$\mathbf{psdf} (x_c) = \lbrack \mathbf{K}^{-1} D_t(u_c) \lbrack u_c^\top, 1 \rbrack ^\top \rbrack _z - \lbrack x_t \rbrack _z$

其中$u_c = \pi \left( \mathbf{K}x_t \right)$为体素中心投影对应的像素，通过这种投影将体素和深度图像素对应起来。在实时帧坐标系下，计算使用深度图得到的$z$轴坐标和warp后对应体素中心的$z$轴坐标的差值。
然后就该更新$\mathbf{TSDF}$了，

$\mathcal{V}(\mathbf{x})_t = \begin{cases} \lbrack \mathsf{v}'(\mathbf{x}), \mathsf{w}'(\mathbf{x}) \rbrack ^\top , &\text{if } \mathbf{psdf}(\mathbf{dc}(\mathbf{x})) > - \tau \\ \mathcal{V}(\mathbf{x})_{t-1} , &\text{otherwise} \end{cases}$

$\mathbf{dc}(\cdot)$将离散的体素转变到连续的$\mathbf{TSDF}$域中，截断距离$\tau > 0$，

$\begin{aligned} \mathsf{v}'(\mathbf{x}) &= \frac{\mathsf{v}(\mathbf{x})_{t-1} \mathsf{w}(\mathbf{x})_{t-1} + \min (\rho, \tau) w(\mathbf{x})}{\mathsf{w}(\mathbf{x})_{t-1} + w(\mathbf{x})} \\ \rho &= \mathbf{psdf}(\mathbf{dc}(\mathbf{x})) \\ \mathsf{w}'(\mathbf{x}) &= \min (\mathsf{w}(\mathbf{x})_{t-1} + w(\mathbf{x}), w_{max}) \\ w(\mathbf{x}) &\propto \frac{1}{k} \sum _{i \in N(x_c)} \lVert \mathbf{dg}^i_w - x_c \rVert _2 \end{aligned}$

最后这个这个似乎不应该是$\mathbf{dg}_w^i$，而应该是$\mathbf{dg}_v^i$，因为后者才是表示3维坐标的量。

估计warp场状态$\mathcal{W}_t$

前面说了$\mathcal{W}_t$的数据结构与如何在曲面融合中使用，但是还没有说如何计算得到，现在开始计算这个东西。
方法是使用给出的深度图$D_t$和对应的重建$\mathcal{V}$，通过能量函数最小化估计当前变换$\mathbf{dg}_{se3}$的值：

$E(\mathcal{W}_t, \mathcal{V}, D_t, \mathcal{E}) = \mathbf{Data}(\mathcal{W}_t, \mathcal{V}, D_t) + \lambda \mathbf{Reg}(\mathcal{W}_t, \mathcal{E})$

其中数据项为稠密的model-to-frame的ICP，正则项惩罚不平滑的运动场，保证由边集$\mathcal{E}$连接的变换node之间的ARAP（as-rigid-as-possible）形变。

数据项

目标是估计非刚体变换参数，每个点对应的$\mathbf{T}_{ic}$和公共变换$\mathbf{T}_{lw}$，warp canonical体素到实时帧中。
mesh由点-法线对存储在canonical帧中：$\mathcal{\hat{V}}_c \equiv \{ V_c, N_c \}$，将该mesh用$\mathcal{W}_t$非刚性变换得到实时warp后的点-法线对$\mathcal{\hat{V}}_w$，warp后的与对应的实时深度数据之间构造数据项。
在model几何和实时帧之间初始的数据对应估计，通过将$\mathcal{\hat{V}}_w$渲染到使用rasterizing渲染流程的由canonical帧顶点坐标shade的实时帧中（关于rasterizing请参考该链接）。将得到在实时帧中可见的canonical帧的几何：$\mathcal{P}(\mathcal{\hat{V}}_c)$。
作为非刚体优化的先验，当给定一帧新的数据时，首先使用KinectFusion的稠密ICP方法得到公共变换因子$\mathbf{T}_{lw}$。然后在当前可见的canonical几何对应的当前帧像素区域$\Omega$中，构造每个点model-to-frame的point-plane误差为数据项

$\mathbf{Data}(\mathcal{W}, \mathcal{V}, D_t) \equiv \sum _{u \in \Omega} \psi _\mathbf{data} (\mathbf{\hat{n}}_u^\top (\mathbf{\hat{v}}_u - \mathbf{vl}_{\tilde{u}}))$

其中$\psi _{\mathbf{data}}$ 为Tukey惩罚函数，

$\begin{aligned} \lbrack \mathbf{vl}(u)^\top , 1 \rbrack ^\top &= \mathbf{K}^{-1} D_t(u) \lbrack u^\top , 1 \rbrack ^\top \\ \mathbf{\tilde{T}}^u &= \mathcal{W} (\mathbf{v} (u)) \\ \mathbf{\hat{v}}_u &= \mathbf{\tilde{T}}^u \mathbf{v}(u) \\ \mathbf{\hat{n}}_u &= \mathbf{\tilde{T}}^u \mathbf{n}(u) \\ \tilde{u} &= \pi (\mathbf{K} \mathbf{\hat{v}}_u) \end{aligned}$

第一个公式计算当前深度图像素对应点的坐标，第二到第四公式计算由canonical帧中warp后的点和法线，第五个公式寻找对应关系。

正则项

数据项计算当前可见的形变，但是还有一个关键问题是当前不可见部分的形变也要估计，因此加入了正则项。
基于形变图模型定义正则项，如果node $i$ 和 $j$ 之间存在边则增加一个ARAP正则项到全局能量中最小化

$\mathbf{Reg}(\mathcal{W}, \mathcal{E}) = \sum ^n _{i=0} \sum _{j \in \mathcal{E}(i)} \alpha_{ij} \psi_{\mathbf{reg}} (\mathbf{T}_{ic} \mathbf{dg}^j_v - \mathbf{T}_{jc} \mathbf{dg}^j_v)$

$\psi_{\mathbf{reg}}$为Huber惩罚函数，$\mathcal{E}$定义了正则化的图拓扑，$\alpha_{ij}$为边的权重$\alpha_{ij} = \max (\mathbf{dg}^i_w, \mathbf{dg}^j_w)$。

扩大warp场

原有的warp场node无法覆盖新增的数据时，需要扩大warp场。包括增量式更新形变图node $\mathcal{N}_{\mathbf{warp}}$并重新计算新的分层边拓扑$\mathcal{E}$。
当 $\min _{k \in N(x_c)} \left( \frac{\lVert \mathbf{dg}_v^k - v_c \rVert}{\mathbf{dg}^k_w} \right) \ge 1$ 时，被认为不支持的顶点（vertex）。对于不支持的顶点使用下采样寻找新的node（详细描述看论文）。
给到新增的node之后，需要重新计算边。构造一个层数$L \ge 1$的正则图node分层，$l = 0$层node简单设为$\mathcal{N} _{\mathbf{warp}}$。计算$l = 1$层的正则node，通过在warp场node $\mathbf{dg}_v$ 上，基于半径搜索的下采样到增加的decimation半径 $\epsilon \beta ^l,\beta > 1$。再次通过DQB使用当前更新的$\mathcal{W}_t$计算初始node变换。反复如此到指定层数。新的边集$\mathcal{E}$开始于$l = 0$的$\mathcal{N}_{\mathbf{warp}}$到位于$l = 1$的$\mathcal{N}_{\mathbf{reg}}$，finer层中每个node增加的边都连到coarser层中的$k$近邻。