Paper——Large-Scale and Drift-Free Surface Reconstruction Using Online Subvolume Registration

Posted on 2019-03-09 Edited on 2023-10-22 In 论文

这次周末再读一篇文章，希望能从中得到一些启发，来做好现在的相关的工作。这篇文章为Large-Scale and Drift-Free Surface ReconstructionUsing Online Subvolume Registration，是2015年CVPR的一篇文章。

关于TSDF，我们已经知道的很多了。在这篇文章中也是同样的。文章中，\(\mathbf{u}\)代表空间中的三维点，而\(F(\mathbf{u})\)代表该点的SDF值，\(W(\mathbf{u})\)表示的是SDF值的信心度量（可以理解为权重）。不同的地方是，文章中还定义了一个归一化梯度： \[ \hat{\nabla}F(\mathbf{u}) \triangleq \frac{\nabla F(\mathbf{u})}{\Vert \nabla F(\mathbf{u}) \Vert} \] 当\(F\)是常数时，返回一个未定义值。

问题描述

为了进行depth fusion，每个voxel都被投影到二维平面中，并且和传感器获得的深度值对比： \[ \Delta _z(\mathbf{u},t) = D_t(\pi (T^{-1}_t \cdot \mathbf{u})) - \zeta(T^{-1}_t\cdot \mathbf{u}) \] 上式中，\(\pi\)是一个\(\mathbb R^3 \rightarrow \mathbb R^2\)的投影过程，也就是从三维到二维的转换，而\(\zeta\)提取了\(z\)坐标，也就是\(\zeta(x,y,z) = z\)。当\(\Delta_z > -\sigma\)时候更新TSDF值，而\(\sigma\)指的是截断距离。新的TSDF\((F^{new},W^{new})\)为： \[ F^{new}(\mathbf{u}) = \frac{F(\mathbf{u})W(\mathbf{u})+ \min\left(1,\frac{\Delta_z(u,t)}{\sigma}\right)}{W(\mathbf{u})+1},\\ W^{new}(\mathbf{u}) = W(\mathbf{u})+1 \] 值得注意的是，这里依然保留的是截断距离，只不过用了另一种方法，选取和1比较的最小值。另外，对于权重的分配，有很多种方法，论文中采取的是最简单的，也就是平均分配，每次\(W = W+1\)。

这篇文章的模型是在KinectFusion的基础上进行的，而KinectFusion之所以不能很好的在large scale上应用，主要有两个原因，一是drift累积，二是global TSDF会需要较大的内存。

Low-Drift局部模型

这篇文章在解决Drift问题上，采取了submap的形式。在本文中，称为subvolume。每K帧会建立一个submap。这篇文章提出了一个比较有趣的方法，来做integrate，为了避免每一步都进行K次integrate，也就是对F，W的更新，每次得到新的帧时，它将该帧以及对应的位姿push进一个队列中（FIFO），同时将\(t-K\)帧pop出去，对TSDF volume进行腐蚀（erosion）： \[ F^{new}(\mathbf{u}) = \frac{F(\mathbf{u})W(\mathbf{u})-\min\left(1,\frac{\Delta_z(u,t-K)}{\sigma}\right)}{W(\mathbf{u})-1},\\ W^{new}(\mathbf{u}) = W(\mathbf{u})-1 \] 当处理的帧数量少于K帧时，不会发生腐蚀。当到达K帧时，队列也满了，将volumen从GPU拷贝到主机中，作为一个新的subvolume，然后腐蚀操作开始。其实我是真的不知道这样做有什么吊用。。。或者我理解有误？总之，这不是关注的重点。

这一部分还包含了对有效Block的筛选，也是比较常规的做法，就不多说了。

本文中\(K=50\)。

Subvolume注册

在本文中，每K帧形成一个新的subvolume\((F_j,W_j)\)，被存储在主机上，下面会将各个subvolume的位姿表示为\(V_j\)。尽管每个subvolume的局部位姿都很小，但是将它们直接简单收集起来会产生较大的drift和misalignment。本篇文章中通过全局优化各个subvolume的位姿来解决这个问题，当得到一个新的subvolume时，会通过一个创新的volume混合方案来进行非刚性变换。一个比较显著的点是，这个优化过程不需要相机tracking模块，因此它可以使得操作实时进行，并且在把新的subvolume放入一个共享缓存区，允许位姿优化作为一个并行运行的过程。

提出的优化过程是受到了ICP算法的启发。优化需要的cost function是这样得到的：

对于每个subvolume，提取出zero-level的点的集合，并且得到它们的法向量
对于每个subvolume，考虑它的边界框，并且找到与其他subvolume之间的overlap
对于每个subvolume得到的点集，通过距离函数的梯度来找到与它有overlap的subvolume中的对应
每个有效的对应都会带来一个点到平面的距离约束，用来优化
如果有一个位姿超出约束，至少有一个位姿错误项被添加，来保证全局一致

接着，通过最小化一个合适的cost function来优化subvolume的位姿。在添加subvolume时候，上一个创建的subvolume的位姿保持固定，来得到tracked相机的最终估计。相对应的，下一个subvolume也能很快根据之前的subvolume得到一个合适的位姿，在相机track成功的情况下。cost funtion会最小化直到收敛，当得到新的对应时，继续最小化。下面会详细介绍一下point matching和优化问题。

The Correspondence Set

给定subvolume\((F_j, W_j)\)，以及zero-level点集：\(\left\{ \mathbf p_i^{(j)} \right\}\)以及计算的法向量\(\mathbf{n_i^{(j)} } = \hat{\nabla}F_j\left(p_i^{(j)}\right)\)。我们定义最小的边界框（bounding box）包围了它所有的有效voxel。接着，找到subvolume的子集\(S^{(j)}\)，它们之间有重叠的边界框。接着，对于每个点\(\mathbf p_i^{(j)}\)，我们遍历候选集，对于每个\(k \in S^{(j)}\)，计算插值距离函数以及它在\(V_k^{-1} \cdot V_j \cdot\mathbf p_i^{(j)}\)的梯度。如下图：

一个点的match为下： \[ \mathbf q_k^{ij} = V_k^{-1} \cdot V_j \cdot \mathbf p_i^{(j)} - F_k\left(V_k^{-1} \cdot V_j \cdot \mathbf p_i^{(j)}\right)\hat \nabla F_k\left(V_k^{-1}\cdot V_j \cdot \mathbf p_i^{(j)} \right). \] 对于所有的subvolume中所有的采样点进行上面的过程，有点类似于ICP算法匹配的过程。我还不是很明白这里梯度那一项的作用，也许可以增加鲁棒。

The Opitimization Problem

假设通过上面的过程，我们已经成功找到了对应。我们可以根据这些对应建立cost funstion。对于每个点对\(\left( \mathbf p_i^{(j)},\mathbf q_k^{ji} \right)\)，我们对\(F_k\)在\(\mathbf q_k^{ji}\)处的形状是没有保证的。我们已经估计了\(F_j\)中\(p_i^{(j)}\)处的法向量，因此我们可以得到一个点与平面之间的constraint： \[ e_k^{ji} = \left(\mathbf p_i^{(j)} - V_j^{-1}\cdot V_k \cdot \mathbf q_k^{ji}\right)\cdot \mathbf n_i^{(j)}. \] cost function的形式为： \[ \arg \min_{V_1,...,V_N} \sum_j \sum_i \sum_k\Vert e_k^{ji} \Vert^2 \] 在本文中，通过Ceres中的列文伯格-马夸尔特方法来优化。

在这个过程中，有可能会出现一些别的情况，比如subvolume \(F_h\)是空的，或者有很少的基数，使得约束不成立，或者得到很差的位姿估计，在这种情况下可以引入位姿之间的约束来增强相机追踪得到的估计。当这个位姿引用的两帧分别在两个subvolume上，如\(Z_{h-1,h}\)，则估计的错误项可以描述为下： \[ e^{h-1,h} = \Phi\log (Z_{h-1,h} \cdot V_h^{-1} \cdot V_{h-1}) \] 上式中\(\log\)将\(SE(3)\)转换到\(\mathcal{se}(3)\)，而\(\Phi\)是一个\(6\cdot 6\)的刚性矩阵。按照经验将\(\Phi\)设置为单位矩阵。

融合Subvolume

对于融合Subvolume，文章中没有过多介绍。如何融到一个global volume中？ \[ F_G(\mathbf u)=\frac{\sum_j F_j(V_j^{-1}\cdot \mathbf u)W_j(V_j^{-1}\cdot \mathbf u)}{\sum_{j}W_j(V_j^{-1}\cdot \mathbf u)}. \] 这个过程并没有比较新奇的地方。文章中，如何融合global volume，是通过对一个全局的位置依次扫描，对每个位置\(\mathbf u\)进行上述的操作。这样的采样，如果subvolume分布比较广，会导致大量的无用计算。如下图：

因此，它会根据有overlap的subvolume来进行： \[ F_j(\mathbf u) = \frac{\sum_k F_k(V_k^{-1} \cdot V_j \cdot \mathbf u) W_k(V_k^{-1} \cdot V_j \cdot \mathbf u)}{\sum_kW_k(V_k^{-1}\cdot V_j \cdot \mathbf u)}. \] 上式中\(k \in S^{(j)}\)，也就是与subvolume j有重叠的subvolume的集合。

上述就是这个文章的主要内容。其实读完了我觉得没什么特别新颖的点。