LR: Voxel Map for Visual SLAM

Abstract

在现代视觉SLAM系统中, 很常规的做法是从重叠的关键帧获得潜在的候选地图点来做后续的特征匹配或是直接跟踪. 我们认为关键帧不是最优解, 因为一些内在的限制, 比如弱几何理由和破可扩展性.

我们提出了一种基于voxel的方法.

从相机位姿可见的点会被查询, 通过sampling相机frustum(函数体, 截头集)在一个raycasting(光线投射)的方式, 这个方法可以用高效的体素哈希的方法以常量时间做到.

跟关键帧比较的话, 被检索的点可以在几何上保证在相机的视角内, 被遮挡的点可以被定义然后被移除到certain extend. 实验表明我们的体素地图跟关键帧地图(滑窗=5)一样高效, 并且提供了更高的定位精度(46% in RMSE).

1. Introduction

理想的话, 地图表示应该:

1. 可以意识到场景的几何信息
2. 就计算时间和内存使用的高效.

Geometry-awareness

在稀疏SLAM中, 使用关键帧和可视点(共视图)作为地图只是允许了有限的几何信息. 共视图对于遮挡没有信息量, 这也很难决定和滤出遮挡点.

Efficiency

对于一个有\(N\)个关键帧的局部地图, 提升\(N\)会提升鲁邦性, 尽管事实上我们只对一个固定大小的时空感兴趣.

对于多相机系统来说, 关键帧系统的设计就更复杂了. 例如, 所有相机的图像作为关键帧保持了最多的信息, 但是有很多冗余.

所以我们认为关键帧策略不是tracking处理中的最优解. 我们提出的voxel-map是有可扩展性且geometry-aware的.

对于SLAM中的数据关联, 我们提出了投影的方法. 的我们将图像中的grid里的像素投影到地图, 然后从在射线上的voxel收集点. 景观很简单, 这个方法有两个优势.

1. 所有返回的点保证会落在相机的FoV里. 而基于关键帧的方法只能依靠很弱的共视假设.
2. 一旦遇到了足够的3D点, 我们就可以停止检查爸爸voxel.

这个方法使得我们的方案可以有reason about occlusion to a certain extent.

2. Voxel Hashing for SLAM

A. Voxel Hashing Data Structure

基于[14].

每个被分配的体素保存它的世界坐标系, 也保存一系列的体素点(就是3D POI, 比如SLAM里的3D landmark). 每个体素点有世界坐标和描述子. 描述子是用于2D-3D数据关联的(帧-地图alignment).

• 对于特征法来说, 描述子就是特征描述子.
• 对于直接法来说, 描述子就是特征被提取的图像patch.

我们用高效的C++应用of哈希表来处理体素的分配和检索. 每个哈希表的入口包含一个指针到对应的体素. 哈希表用hashing function (体素的世界坐标)来获得被分配的体素. 哈希值:

\[H(x, y, z)=\left(\left(\lfloor x\rceil \cdot p_{1}\right) \otimes\left(\lfloor y\rceil \cdot p_{2}\right) \otimes\left(\lfloor z\rceil \cdot p_{3}\right)\right) \bmod n, \]

这里\(p_1, p_2, p_3\) 是large prime number. []是rounding操作. \(\otimes\) 是以 bit为单位的 XOR操作, \(\bmod\) 是modulo 操作符. \(n\)是哈希表的大小.

别的方法, 比如raw点云或者是关键帧, 随着地图大小的增长都不会处理的很好. 对于没有结构的点云, 每个点都需要各自被检查. 使用重叠着的关键帧通常有不错的启发, 单还是需要情景的检查所有的关键帧的点.

Voxel size

需要选择合适的体素大小

Resolving collisions

因为哈希函数的特性, 有可能会发生collisions如果很多个体素映射到相同的hash值. 为了解决这个问题, 我们把哈希表分为buckets. 每个bucket对应一个唯一的哈希值. 这些buckets把整个哈希实体保存为一个列表.

B. SLAM Map Management with Voxels

我们不会完全丢弃关键帧, 因为对于BA还是需要关键帧的.

Insert Point

我们用哈希函数找到哈希表中的目标bucket. 然后在bucket中的哈希实体迭代.

• 如果在空间中存在体素, 我们将点加到体素中. 如果点的位置已经被占据了, 我们用新的点更新点的描述.
• 如果不存在体素, 我们创建体素然后加入到bucket中(作为hash entry), 然后点被加入到新建的voxel中.

Delete point

删点和插点类似. 首先计算哈希值, 然后在目标bucket里面迭代所有的哈希入口. 然后删点.

Query Map

首先将查询的位置转化为整形, 然后计算哈希值. 然后迭代在bucket里找.

C. Point query with raycasting

因为哈希函数是基于体素位置的, 我们直接在raycasting的frustum上采样.

• Image plane sampling and raycasting: 先从图像平面的grid上采样. 然后将这些像素反投影到3D空间中的方向向量上, 得到\(r\)个光线 \({R_i}^r_{i=1}\). 注意这些射线是以相机坐标系下表示的.
• Ray sampling 对于每个射线\(R_i\), 我们采样\(s\)个点, 深度距离从\(D_{min}\) 到 \(D_{max}\). 这些点是在相机坐标系下的, 离线先计算好.
• Point query 在SLAM里, 在query的死后, 我们有一个当前位姿的先验(比如IMU预积分或者是之前的相机位姿). 用这个先验, 我们可以将前面提到的sample点转换到世界系中.

前两步本质上是在给定的范围内离散化了相机的frustum. 因为它是离线计算的, 只需要做一次. 计算复杂度是\(\mathcal{O}(r\cdot s)\). 因为\(r\)和\(s\)和地图尺寸无关, 所以query的耗时是常量.

另外, 如上图右所示, 我们要强调采样的顺序是从近到远的. 我们只会返回第一个体素, 来避免遮挡点.

3. Case Study: SVO with Voxel Map

SVO是一个混合的pipeline, 它组合了直接法和特征点法的优势.

它首先通过最小化之前的图像和新图像的光度误差来align. 这个会给新的帧一个不错的pose先验.

有了给定先验之后, 这个pipeline会找到关键帧和新帧的重叠. 通过从当前帧投影选择的点到局部地图中的关键帧来找到重叠的关键帧, 直到一组\(M\)个重叠的关键帧被找到.

因为在通常的移动下, 最近的\(M\)个关键帧就是最新的\(M\)个关键帧, 所以平局的查询时间是基于\(M\)这个值而不是地图大小. 当匹配被建立之后, 会通过一个motion-only的BA来计算位姿. pipeline还有一个建图线程, 它使用鲁邦贝叶斯滤波来估计深度.

我们做了下述调整.

Map query for motion estimation: 我们没有检查重叠的关键帧, 而是直接在相机frustum(截头椎体)上采样. 我们的基于射线投影的方法会返回一组可见的voxel, 不包括遮挡的voxel. 我们假设一个voxel里的点不会相互遮挡.

Map Management: 如果在深度滤波器里一个点的深度的不确定性降低到了一个程度, 一个新的3D点会在这个深度上被初始化. 这个点会被插入体素中用于后续的相机tracking. 在tracking时, 一些点会被标记为外点(在motion-only的BA中). 这些点会从体素中移除.

4. Experimental Evaluation

Naive-Keyframe 在墙上均匀取关键帧.

Voxel-hasing 我们分配了足够多的体素. voxel的格子大小固定为2m.

地图查询时间如下:

Geometric Awareness

红点表示远, 绿点表示近. 天真-关键帧策略对于遮挡毫无sense, 而我们的方案很牛逼.

B. Real-world experiments

我们在EuRoC上跑了一下.

• 原来的pipeline关键的数量从5-30.
• 我们的方案.

对比而言, voxel map可以相对在少的计算时间里获得好的精度. 但是也不是一直好的, 因为voxel map的表现是依赖于场景和voxel的大小的.

5. Conclusion and Future Work

没啥.