Paper——Flashfusion

对于论文的阅读是我的弱项,这是导致我学习过程总是恍恍惚惚一脸懵比的一个重要的原因。出来混总是要还的。大学时候的浪荡带来的后果现在就得到了体现。

今天记录一篇对于FlashFusion的阅读。这篇文章是我们实验室的学长发在RSS的论文,是关于实时三维重建的。

FlashFusion是一个实时三维重建系统,能够在CPU上实现全局一致的稠密实时三维重建。对于文章的分析主要是对于系统构架以及具体实现的技术进行解读,因此我们跳过abstract,related work以及实验部分。

FlashFusion的系统构架图如下:

我们可以看到的是FlashFusion采取的是关键帧策略。它分为三个线程:tracking,optimization,以及meshing,分别对应了追踪,优化以及网格提取。FlashFusion之所以能实现上述效果,是因为它在回环检测,以及SLAM上都有不少的改进。

定位

FlashFusion中的SLAM部分被称为FastGO,或者GCSLAM。采取的是帧注册的策略。当捕获到新的帧时,使用MILD闭环检测与上一个关键帧进行相似度的度量,如果相似度过大,则当前帧被识别为一个新的关键帧。否则,被当作前一个关键帧对应的局部帧,并且在判断的过程中,根据ORB等特征点匹配,计算出从关键帧到当前帧的位姿变换,从而得到当前帧的位姿。

如果当前帧被识别为关键帧,则就会进行一次全局注册,对所有的关键帧的位姿进行一次优化,也就是进入了optimization线程,用于维持global consistency。为了说明这个优化过程,我们先来看看一对帧之间的代价函数:

上式中,$T_{i,j}$是第i帧与第j帧之间的相对转换,$C_{i,j} = \{(p_i^k,p_j^k)\vert k = 0,1,…,\vert C_{i,j} \vert-1\}$是第i帧与第j帧之间的所有对应的特征点对的集合,由于有深度图,所以我们也就直接得到了特征点的相机坐标,通过转换后,属于一对的特征点坐标应当一致。因此上面的误差也算是重投影误差的一种吧。

而全局误差就是对所有的关键帧对应的局部帧上述误差的总和:

上式中,$\Phi(i)$指的是由mild选出来的最接近关键帧$F_i$的5个帧。如果对所有的帧都进行上述误差会使得计算量过大,使得CPU上的实现不够现实。

这5个帧是如何选出来的?实际上,当新的关键帧插入时候,会与之前所有的关键帧进行match,进而通过mild选出来5个相似度最高的关键帧,这也就意味着上述全局误差的贡献者全部为关键帧。

上述优化问题是比较好解决的,文章中使用的是高斯牛顿优化算法,根据之前的SLAM中非线性优化的章节,我们知道高斯牛顿优化需要的是:$J(x)^TJ(x)$与$J(x)^Tf(x)$。

我们将误差$E(\xi)$换一种写法:$E(\xi) = r(\xi)^Tr(\xi)$,其中$r(\xi) = [r_0^T ,r_1^T,…,r_{M-1}^T]$,$r_l = p_i^k - T_{i,j}p_j^k$,$M$就是特征点对的总和了。

则在本例中,我们要求的$\delta$,也就是每次迭代的增量为:

这里和高斯牛顿中推导的方法是一致的。而雅科比矩阵的求解:

我们不会直接求$J(\xi)$,因为需要的是上面两个矩阵,而他们的求法甚至比直接求单独的雅科比矩阵更加容易。从FastGO中,我们可以更加详细地了解解决上面优化话题的快速算法。同时,为了提高鲁棒性,Flashfusion对于cost funstion选择了Huber norm,为了实现实时减少计算量,也不会对所有的frame pair进行计算,而是选取变化最大的10个frame pair。

这里有一个细节,在观看demo的时候,我们看到作者捂住了镜头,换到了之前扫描过的场景,而重建的视野也直接回到了之前扫描的场景。对于这个帧,插入的一定是新的关键帧,甚至有可能会tracking failed。而该帧的位姿,就是通过global optimization得到的。可以看到FlashFusion是非常强大的。

TSDF Fusion

关于TSDF的介绍,之前TSDF也提了,虽然没有涉及到具体的公式,但是还是能够有个大概的了解。下面我们用一些数学表达,介绍一下原本的TSDF的具体实现。

TSDF将空间分成若干个voxel,voxel的个数依赖于重建的分辨率,而TSDF值就是每个voxel中心距离最近的表面的距离。这个TSDF值如何得到?我们将体素根据当前的相机位姿,转换到相机坐标系下,然后投影到成像平面上。然后对于该投影坐标(二维),我们有一个对应的深度值,使用该深度值减去体素在相机坐标下的z值,我们就可以得到它到这个平面的距离。

上式中,$K$为相机内参,$T_t$为此刻的相机位姿,而p为体素,$\pi$表示从齐次到非齐次的转换,也就是取了前两维,我们通过$D_t$来表示取深度值,$[p]_z$表示z轴坐标。

为了截断,我们还需要用下面这样的截断函数:

其中,$d_{\max}$为截断值。

另外,TSDF fusion中一点是权重。因为对于不同的位姿得到的同一个voxel下的TSDF值是不同的,因此在integrate的时候需要有权重。在本篇文章中,对于sdf和权重的更新:

其中$w_0$为原来的,$w_i$为新来的,$w_n$为融合之后的。

我们做的重建主要是表面重建,因此关注的就是靠近表面的体素。传统的TSDF fusion会对视锥内的每一个voxel都进行上述过程,从而获取它的截断距离。而为了节省空间,研究人员提出来了Hashing和Octree两种方法,本篇文章采用的是Hash的方法。对于Hashing,每$8\times 8\times 8$个体素被组建成一个chunk,每个chunk都通过哈希函数映射到一个位置,大大提高了检索速度。我们可以观察一般的tsdf fusion中有效块的占比:

可以看到,视锥内只有一小部分的chunk包含了表面,也就是有效的TSDF值,而传统方法中对所有的voxel进行遍历得到TSDF,会浪费很多的时间和计算量。因此,FlashFusion采取了一种稀疏体素采样方法,选取有效块。对于每个chunk,计算它的8个定点的TSDF值,如果有一个定点的TSDF值(绝对值)小于某个阈值,那么将它作为有效的chunk。这背后的道理是,如果一个chunk中包含表面,它有很大的可能性至少某个面会经过这个面,有很大的可能性至少有一个顶点会投影到这个表面上,如下图:

假如分辨率为$r$,这个chunk的大小为$N^3$。如果一个voxel在表面的截断距离内,则:$\vert d_v \vert \le d_{\max}$。那么它的顶点应该会满足:

因此,如果所有的顶点都大于$d_{\max}+\sqrt 2N \times r$,那么这个chunk里几乎不可能包含表面了(这部分的内容和论文有区别,不清楚是不是论文的错误)。在实现中,论文将$8 \times 8 \times 8$个chunk再次归为一个Cube,现在Cube上进行稀疏采样,再在有效的Cube里进行chunk的采样。下面是采用稀疏采样的准确度和时间:

可以看到利用稀疏采样,我们可以选到98%的真正有效chunk,接着对有效的chunk进行遍历求得TSDF值,花费时间仅仅为全部体素遍历的2%。

对于有效chunk的选择仅仅应用在关键帧上,然后局部帧再被融合到对应的关键帧中。也就是,chunk的ID会被存储到对应的关键帧中,用于后面的de-integrated。对于颜色也是类似的,新的帧的颜色会给更多的权重。

需要注意的是,由于全局优化,可能各个关键帧的位姿会随时改变。这时候我们会需要deintegrate,用来在TSDF模型上减去之前的位姿的影响,而使用reintegrate,将具有新位姿的keyframe融合到TSDF模型上。

网格提取

对于网格提取可以分成多边形生成与法向提取两个部分。如何从TSDF场到一个三维网格模型,别的paper中很多又叫做raycasting,这篇文章中提到了一个方法,叫做ncrementalmarching cubes algorithm(这是一本书)。这些算法的道理都是表面两侧体素的TSDF值符号不同,因此会使用线性插值来得到表面。插值过程如下:

其中$s$为sdf值,而$v$代表了各个体素的坐标。你可能会奇怪,为什么sdf值和体素没有对应相乘,如果画一张图你就会理解:

可以看到,实际上表面的点是更靠近sdf值小的,因此计算位置时候应该给sdf值小的较大的权重。

在本篇paper中,采取了动态阈值的方法,因为仅仅根据sdf值来判断是不合适的,因为如果出现比较bad的情况,比如观测的方向和表面接近平行,那么即使表面附近的voxel也有比较大的sdf值。所以基于chunk的策略,它为不同的chunk设定不同的阈值,当生成网格时,包含表面的voxel中最大的sdf被当作这个chunk的阈值。这样,当引入新的frame时,如果某个体素的sdf比这个阈值更大,就会被忽略。在这种策略下,每次meshing阶段,平均只需要增加10%的新的mesh,其他的mesh只是被更新了。通过这种方法,多边形生成的速度被加速了两倍。

法向提取的过程如下:

上面的法向计算过程其实不难理解,是对该点的法向的粗略估计,需要6个临近的voxel(有效的)。在FlashFusion中,每个chunk中还维护了一个查找表,用来记录它相邻的chunk的地址,这样就避免了在多边形生成与法向提取中对总的查找表(chunkList)的查询。对于本文章中mesh的生成我了解得还不是太清楚,因为介绍的比较简单。不过这也不是我想了解的重点,因此暂时就先不深究了。

Re-integrate

reintegrate是只将关键帧对应的局部帧融合进去。对于每个关键帧,我们只融合它对应的前10个局部帧,因为如果局部帧过多,只能说明相机没有移动或者移动过慢,而这些局部帧大多提供了相似的信息,并不会有多大的贡献。因此选取前10个既保证了速度,又保证了效果。

上述就是FlashFusion的关键部分介绍。下面是一个FlashFusion重建的demo:

关于mild闭环检测和GCSlam可以看下面两篇文章: