# BAMF-SLAM：基于循环场变换的捆绑调整多鱼眼视觉-惯性 SLAM 系统
张伟 $^{1,2}$、王森 $^{2,3}$、董星亮 $^{4}$、郭荣伟 $^{4}$ 和诺伯特·哈拉 $^{1}$
摘要—本文提出了BAMF-SLAM，这是一种新型的多鱼眼视觉-惯性 SLAM 系统，它利用捆绑调整（Bundle Adjustment, BA）和循环场变换（Recurrent Field Transform, RFT）在复杂场景下实现精确且稳健的状态估计。首先，该系统直接处理原始鱼眼图像，从而能够充分利用鱼眼相机的宽视场角（Field-of-View, FoV）。其次，为克服低纹理环境的挑战，我们探索了通过统一的因子图将多相机输入与互补的惯性测量数据进行紧耦合整合，进而联合优化位姿与稠密深度图。第三，为实现全局一致性，鱼眼相机的宽视场角使系统能够发现更多潜在的回环闭合点；借助RFT广阔的收敛域，我们的系统即便在重叠区域极少的情况下也能完成极大基线距离的回环闭合。此外，我们引入了半位姿图捆绑调整方法，以避免耗时的全全局捆绑调整过程。通过将相对位姿因子与回环闭合因子相结合，既能以较小的内存占用高效调整全局状态，又能保持高精度。在TUM-VI、Hilt-Oxford和Newer College数据集上的评估结果表明，所提系统的性能优于现有方法。在2022年Hilt-SLAM挑战赛中，我们的视觉惯性导航（Visual-Inertial Navigation, VIO）版本荣获第二名；后续提交的包含全局捆绑调整后端的完整系统，其性能甚至超过了夺冠方案。
## I. 引言
同时定位与地图构建（Simultaneous Localization and Mapping, SLAM）技术正广泛应用于移动机器人和增强现实等领域的实际应用。在SLAM研究领域，视觉-惯性 SLAM（Visual-Inertial SLAM, VISLAM）已成为一种鲁棒性高且精度优异的解决方案。尽管VI-SLAM研究已取得显著进展 [^1]、[^2]、[^3]、[^4]，但新兴的具有挑战性的数据集和新的工业应用场景 [^5]、[^6] 给现有最先进的VI-SLAM系统带来了新的严峻挑战，要求它们具备更高的精度和鲁棒性。例如，建筑工地监测需要在低纹理环境、光照快速变化且物体运动剧烈的条件下达到厘米级精度。为应对这些挑战，我们选择采用多个鱼眼相机与互补的惯性测量单元（Inertial Measurement Unit, IMU）相结合的传感器配置。这种配置能为系统提供充足的环境感知信息与自身运动数据，从而应对复杂场景，实现更鲁棒、更高效的环境建模。
Fig. 1. Estimated point cloud and trajectory by the proposed system on exp09 sequence of Hilti-Oxford dataset. The start and end points of the trajectory are marked with red rectangles.
捆绑调整[^7]已成为现代SLAM系统中的黄金标准。在基于特征的方法中[^3]、[^4]，其目标是最小化所检测到的稀疏特征的重投影误差。相比之下，直接法[^8]、[^9]则是最小化密集像素的光度误差。随着计算能力的提升和深度学习的进步，近期的研究[^10]、[^11]、[^12]倾向于结合两者的优势，即利用RFT的预测结果作为目标，来最小化密集深度图的重投影误差。基于这一方案，我们构建了相应的系统，并将其扩展为多摄像头VI-SLAM架构；该架构采用了统一的联合因子图表述方式，能够以紧耦合的形式融合多摄像头的输入数据以及IMU测量结果。
大多数视觉SLAM系统的一个主要局限在于其对摄像头视野内视觉特征的依赖。当摄像头视野被遮挡、纹理信息过少，或因快速运动导致视野重叠度较低时，这些系统便难以准确跟踪摄像头位姿。多鱼眼相机VI-SLAM系统的巨大优势在于：一方面，鱼眼相机的大视场角降低了低纹理情况的出现概率，同时增加了视野重叠度；即便某一视角完全被遮挡，其他摄像头或IMU测量结果仍能提供冗余信息，从而避免系统出现跟踪失败或灾难性漂移现象。
为了实现全局一致的地图构建，我们提出了一种名为半位姿图捆绑调整的算法，用于联合优化位姿以及由闭环检测所涉及的深度图。与完整的捆绑调整相比，我们的方法并不依赖重投影因子来连接所有关键帧，而是将前端计算得到的重投影因子转换为相对位姿边，以此构建位姿图。
---
与位姿图优化（PGO）不同，我们的方法不仅优化位姿，还会优化闭环检测所涉及的关键帧的深度图——这些深度图正是通过重投影因子来表示的。因此，我们的方法比PGO更精确，同时比完整的捆绑调整更高效。
综上所述，本研究的主要贡献如下：
- 一款先进的名为BAMF-SLAM的多鱼眼相机VI-SLAM系统，该系统充分利用了多鱼眼相机的完整图像信息以及互补的IMU测量结果。
- 一种统一的因子图表述方式，能够将多摄像头输入数据与IMU测量结果紧密耦合，从而联合优化位姿和深度图。
- 一种半位姿图捆绑调整方案，该方案利用相对位姿边和闭环作为重投影因子，在实现高效闭环闭合的同时最大化地图的全局一致性。
### II. 相关工作
视觉惯性SLAM和多摄像头SLAM领域已有大量研究成果。为了融合视觉与惯性测量数据，包括本工作在内的大多数研究都采用[^13]提出的预积分方法来减少变量数量，进而高效求解捆绑调整问题。在固定前端计算成本的前提下，对旧状态进行边缘化[^1]的方法因能有效建模其不确定性而广受欢迎；另一类研究[^3]、[^14]则将旧状态作为固定先验纳入局部捆绑调整中。由于后者更具简洁性和实效性，本研究采用了这种方法。
已有多项研究探讨了在SLAM系统中使用多摄像头的技术。[^15]提出了一种多摄像头SLAM系统的通用设计方案，包括初始化策略和关键帧选择机制。MultiCol [^16]对ORB-SLAM2 [^17]进行了扩展，使其能够通过束调整算法处理多个鱼眼摄像头的输入数据。[^18]提出了一种用于整合多摄像头图像的全景相机模型，但该模型仅适用于摄像头中心存在微小偏移的情况。我们的传感器配置无法满足这一要求，因此无法采用该全景模型。
尽管此前已有少量尝试将多摄像头输入与惯性测量数据相结合，但大多数方法仍依赖于跨摄像头的特征点跟踪技术。例如，VILENS-MC [^19]利用跨摄像头特征跟踪来利用摄像头重叠区域的信息。相比之下，我们的系统并不跟踪特征点，而是基于深度图表示建立密集的像素级对应关系。此外，我们的系统还集成了全局优化后端，从而在精度和鲁棒性方面相比以往的研究取得了进一步提升。
### III. 循环场变换的相关基础
循环场变换的概念可追溯至RAFT [^20]，这是一种用于光流估计的深度网络架构。受此成功的启发，RAFT-3D [^21]和RAFT-Stereo [^22]分别将该概念应用于场景光流估计和立体匹配任务。
Fig. 2. Network structure for the update operation of recurrent field transforms borrowed from [^10].
随后，RFT概念被应用于密集SLAM系统DROID-SLAM [^10]中，该系统将RFT与束调整算法相结合，以从光流数据中求解相机位姿和深度信息。RFT利用残差式网络，根据输入图像提取的特征图来预测共视视图间的密集像素级对应关系。特征相关性可通过最新估算的位姿和深度信息推导出的光流来查找。如图2所示，GRU模块的输入包括当前的相关性数据、上下文特征、上一次的光流更新结果以及上一个隐藏状态。这些数据会被拼接起来，用于计算下一次光流更新的结果及相应的置信度值。
为了提升RFT与束调整算法的协同效果，DROID-SLAM通过将束调整算法作为可微层纳入训练过程，从而使损失函数能够直接针对位姿和深度图估计值进行优化，而非光流数据，最终在多个基准测试中达到了最先进的精度水平。本研究以DROID-SLAM为基础，将其扩展为多摄像头视觉惯性SLAM系统，以应对现实应用中更为复杂的场景挑战。
尽管[^23]训练的预训练模型仅在针孔图像数据集[^10]上进行过训练，但其在鱼眼图像上仍表现出优异的泛化能力。因此，我们直接使用该预训练模型，无需进一步微调。
IV. SYSTEM OVERVIEW
Fig. 3. Overview of the proposed system.
我们提出的系统的任务是通过搭载的多鱼眼摄像头和惯性测量单元，尽可能准确且鲁棒地估算传感器平台的状态。图3展示了系统的总体架构。遵循现代SLAM框架的设计理念，我们
---
将我们的系统划分为前端和后端。前端负责处理输入图像和IMU测量数据，确定哪些属于关键帧，在滑动窗口中跟踪最新的关键帧，并执行替代的RFT和局部BA优化。后端则负责管理由前端生成的所有关键帧的数据缓冲区，将重投影因子的中间结果转换为相对位姿因子，从而构建全局位姿图。最后，为了提高整体精度和全局一致性，后端采用了回环检测与全局半位姿图BA技术。
### 五、方法
本节将详细介绍所提出系统的方法论。我们首先在第V-A节讨论鱼眼相机模型的应用，以充分利用其宽广的视场角。第V-B节阐述了关键的IMU初始化过程，该过程使我们的系统能够融合视觉与惯性信息。接着，我们在第V-C节介绍因子图的构建方式，并解释用于融合多种输入的各类因子。最后，我们提出半位姿图BA方法（第V-D节），该方法通过识别长期回环并最大化相对位姿与回环约束之间的一致性，从而实现全局一致性和更高的精度。
### A. 鱼眼相机模型
传统的SLAM方法通常假设理想的针孔相机模型，这就要求对输入图像进行预处理以消除畸变。然而，对于鱼眼图像而言，这种做法可能会因裁剪掉边缘区域或插值导致的采样伪影而丢失有用信息，进而无法发挥鱼眼图像广视场角的优势。为解决这一问题，我们提出的系统在整个框架中均采用Kannala-Brandt鱼眼相机模型 [^24]，正如 [^3] 中所描述的那样。该模型用于图像像素的投影与反投影，以及(单目)投影函数中雅可比矩阵的分析计算。与 [^3] 不同，我们使用深度图而非稀疏特征，并实现了定制的CUDA内核来对所有像素的操作进行并行化处理。
### B. IMU初始化
为了实现IMU观测数据的无缝集成，需要初始化一组在纯视觉系统中未被估计的新状态，包括重力方向、尺度、机体速度以及陀螺仪和加速度计的偏差。遵循 [^3] 中的最佳实践，最好在初始化IMU之前，先利用具有足够运动量的短序列数据进行纯视觉BA。简而言之，我们的IMU初始化流程包含以下四个步骤：
1) 执行纯视觉BA，以确定前K个关键帧的初始位姿。
2) （静态起始情况下可选）利用约束条件 $R_{wg}$ 对初始重力方向 $b_a$ 和加速度计偏差 $\bar{g} - b_a = R_{wg}(0,0,G)^T$ 进行最小二乘优化，其中G表示重力加速度大小，$\bar{g}$ 是通过静止状态下的加速度计测量值取平均值得到的。
3) 执行纯惯性BA，联合优化速度、尺度、偏差 $b_g$，同时细化前两步得到的估计值，即 $R_{wg}$ 和 $b_a$，这一过程借助预积分的IMU因子完成（详见第V-C节）。
4) 执行视觉-惯性BA，最终利用视觉重投影因子和IMU预积分因子对所有变量进行优化。
在第二步中，我们通过检查陀螺仪测量的幅度来判断初始阶段是否处于静止状态。如果初始阶段确实处于静止状态，那么该阶段的加速度计数据就能提供关于重力方向的可靠先验信息。
通常来说，通过已知的立体基线进行离线校准后，就可以直接观测到尺度参数，无需将其作为变量来优化。然而，我们的实证研究表明，多摄像头校准往往不够精确，引入尺度变量能够降低校准误差带来的影响。
### C. 因子图建模
我们基于光束法平差的系统核心在于构建包含各类因子的因子图，从而联合优化系统状态和三维结构。三维结构由关键帧的深度图来表示。系统状态定义在以B表示的IMU坐标系中，时间$t_{i}$ 时的状态可定义如下：
$$ \mathbf{x}_{\mathbf{i}}=[\mathbf{T}_{\mathbf{i}},\mathbf{v}_{\mathbf{i}},\mathbf{b}_{\mathbf{i}}^{\mathbf{a}},\mathbf{b}_{\mathbf{i}}^{\mathbf{g}}]^{T} $$
其中$\mathbf{T}_i = [\mathbf{R}_i, \mathbf{p}_i] \in \mathrm{SE}(3)$表示机体位姿，$\mathbf{v}_i^\mathrm{w}$表示速度，而$\mathbf{b}_i^\mathrm{a}$和$\mathbf{b}_i^\mathrm{g}$则分别对应IMU加速度计和陀螺仪的偏差。
图4分别展示了前端和后端对应的因子图建模方式。在前端，我们维护一个滑动图，仅对动态窗口内的变量进行优化。当新关键帧出现时，我们首先优化该关键帧及其深度图的状态，此时窗口大小设为1，这一过程我们称之为跟踪过程。接下来，采用更大的窗口大小（n个关键帧）执行局部光束法平差。通过实证观察我们发现，直接对新关键帧执行局部光束法平差需要更多迭代次数才能收敛；但如果在跟踪过程之后再进行局部光束法平差（其速度远快于直接执行局部光束法平差），仅需两次迭代即可收敛。正如第VI-E节所讨论的，这种方法能显著提升运行效率。此外，窗口外那些通过重投影因子关联的几个顶点也会作为固定先验纳入优化过程。
为了融入IMU测量数据，我们利用IMU预积分方法[^13]来计算连接连续关键帧的IMU预积分因子。对于重投影因子，我们区分单目和立体两种类型。单目因子用于连接具有共同视域的一对关键帧，重投影时会利用顶点位姿估计值来变换坐标；在立体情况下，则利用已知的相机外参来计算跨相机重投影。设$\Pi$表示鱼眼投影函数，a
---
Fig. 4. Illustration of the proposed joint factor graph formulation. a) A sliding graph with a window size of three keyframes and one fixed keyframe served as prior. b) Semi-pose-graph with a long-term loop closure between two distant keyframes shown as an example.
点$p_{i}$可以利用估计的深度$\hat{d}_{i}$通过以下公式进行重投影：
$$ \hat{\mathbf{p}}_{i j}=\Pi(\hat{\mathbf{T}}_{i j}\Pi^{-1}(\mathbf{p}_{i},\hat{d}_{i})) $$
其中，在单目情况下$\hat{\mathbf{T}}_{ij} = T_{CB} \hat{\mathbf{T}}_j^{-1} \hat{\mathbf{T}}_i T_{CB}^{-1}$表示从关键帧i到关键帧j的相对变换；在立体情况下，它则表示相机i和相机j之间的外参$T_{C_i C_j}$。
对于重投影目标，RFT会预测流修正量$\delta_{ij}$并将其叠加到上一时刻的光流$\hat{p}_{ij}$上。因此，更新后的光流（即密集对应关系）可按如下方式获得：
$$ \mathbf{\check{p}}_{ij}=\mathbf{\hat{p}}_{ij}+\boldsymbol{\delta}_{ij} $$
为了减轻错误对应关系和遮挡现象的影响，RFT会为每个像素预测置信度$w_i$。那些在目标视图中没有对应点或对应关系模糊的像素会被赋予较低的置信度。此时重投影误差可按如下方式表述：
$$ r_{i j}=\|\mathbf{\check{p}}_{i j}-\Pi(\mathbf{\hat{T}}_{i j}\Pi^{-1}(\mathbf{p}_{i},\hat{d_{i}}))\|_{w_{i}}^{2} $$
在我们多摄像头配置中，有一种特殊情况：一个关键帧会对应多个指向不同视角的深度图。在这种情况下，我们会针对所有深度图计算重投影误差，我们将其称为多重投影因子。对于Newer College和Hilti-Oxford数据集而言，除了正面视图的深度图外，我们还会纳入左侧和右侧视图的深度图。
### D. 半位姿图BA
Fig. 5. Example of a wide baseline loop closure with color-coded confidence values predicted by the RFT network. Note that values lower than 0.01 are omitted for clarity.
执行全局完整BA的计算成本很高，且需要占用大量内存，因为它要利用前端生成的所有重投影因子来优化所有关键帧的系统状态和深度图。此外，由于前端提供的状态已经具备较高的相对精度和局部一致性，或许没有必要再去优化本就十分精确的相对位姿。基于此，我们确定可以将前端的重投影因子通过局部BA建立的密集对应关系转化为相对位姿因子。随后便可利用这些相对位姿因子构建位姿图。重投影因子所蕴含的几何约束主要由相对位姿因子及其对应的协方差来表示。我们采用高斯-牛顿法来计算这些相对位姿因子和协方差，具体过程如下：[^25]
$$ \mathbf{H}\Delta\mathbf{x}=\mathbf{J}^{T}\mathbf{W}\mathbf{J}\Delta\mathbf{x}=\mathbf{b} $$
$$ \mathbf{C o v}=(\mathbf{J}\Delta\mathbf{x}-\mathbf{r})^{T}\mathbf{W}(\mathbf{J}\Delta\mathbf{x}-\mathbf{r})\mathbf{H}^{-1} $$
其中，J是重投影函数相对于相对位姿的雅可比矩阵，r是最后的重投影误差向量，W是由RFT预测的置信度构成的权重矩阵。相对位姿的更新量表示为$\Delta x$。
前端估计的轨迹容易出现里程计漂移现象，这种漂移会随着行驶距离的增加而累积。为了减轻漂移误差，后端会搜寻潜在的回环闭合点并将其作为额外约束加入。后端首先会计算所有关键帧之间的帧距离，该距离是根据最后的位姿和深度图估计值计算出的平均光流来确定的。通常会采用较为宽松的帧距离阈值，以便能纳入更多基线较宽的潜在回环闭合点。虽然这可能会导致错误的回环闭合点被纳入，但其影响微乎其微，因为RFT网络的可靠性很高，会给错误匹配分配较低的置信度。图5展示了一个基线较宽的回环闭合示例：在长途行驶后重新回到了同一个房间。我们系统中使用的鱼眼相机具有宽广的视场角，即便图像边缘处的重叠区域很小，也能检测到此类回环闭合点。
利用相对位姿因子即可构建位姿图。此外，检测到的回环闭合点可以以重投影因子的形式呈现，从而将位姿图扩展为包含回环闭合中所涉及深度图的半位姿图，如图4.b)所示。半位姿图BA比完整BA更高效，因为需要优化的变量数量要少得多。而且，由于前端滑动窗口内的关键帧重叠度很高，由前端生成的重投影因子所衍生的相对位姿因子具备很高的精度和鲁棒性。因此，半位姿图BA既高效又精确。
### VI. 实验
我们使用多种数据集对我们的系统进行了广泛评估。为了验证该系统在立体视觉+IMU配置下的有效性，我们在TUM-VI数据集中的多个室内场景下进行了评估[^5]。我们还进一步在极具挑战性的Newer College数据集上评估了多摄像头配置，该数据集包含室内和大规模室外场景[^26]。
---
Fig. 6. Qualitative results on TUM-VI dataset. As groundtruth only covers a single room, we transform all results to the groundtruth frame for consistency check. Estimated trajectories are depicted in colors and point clouds are accumulated. Red-box marked region is zoomed in on the right from the side look to provide a better illustration. Notably our results demonstrate high consistency in cleanly separating floors and clear-cut edges of stairs.
场景以及Hilt-Oxford数据集[^6]，该数据集包含了在建筑工地和历史剧院采集的具有挑战性的序列。在所有评估中，我们同时提供VIO和VI-SLAM的结果，以便区分前端里程计漂移方面的性能以及经过全局优化后的整体性能。最后，我们对处理时间进行了分析，并展示了其实时处理的潜力。
### A. 实现细节
该框架采用Python/CUDA编写，使用了PyTorch和Eigen库，并在配备Intel i9-12900K CPU和Nvidia RTX 3090Ti GPU的台式机上进行了测试。对于TUM-VI及其他两个数据集，输入图像分别被下采样至$440 \times 440$和$384 \times 512$分辨率。为了减少内存占用，我们采用了环形缓冲区数据结构来存储图像特征和关键帧的中间结果。这样一来，无论数据长度如何，只需分配固定数量的GPU内存，每个新传入的关键帧数据会被写入缓冲区的起始位置。我们的系统能够在拥有11GB内存的GPU上运行前端和后端，即便面对大规模数据集也能保证高性能。
### B. 在TUM-VI数据集上的性能表现
TUM-VI[^5]数据集是通过搭载两台鱼眼相机（采用立体拍摄模式）和惯性测量单元的手持设备采集的。我们选取了四个具有挑战性的场景来进行方法评估，包括低纹理场景（slides）、快速旋转场景（corridor）以及大规模多层建筑场景（magistrale）。表I总结了定量评估结果以及与以往工作的对比情况。我们将自己的VIO方案与BASALT[^27]以及OKVIS2的VIO版本[^4]进行了比较，这两种都是基于特征的VIO方法且不具备回环检测功能。在四个场景的平均ATE指标上，我们的方法均优于其他所有方法。
在VI-SLAM系统的评估中，我们考察了经过回环闭合全局优化的轨迹表现。我们的完整SLAM系统性能超过了OKVIS2[^4]和ORB-SLAM3[^3]，取得了最低的ATE值。值得注意的是，我们的系统能够在重叠区域很小的情况下成功检测并闭合回环，从而使得几乎所有序列的ATE都达到了1厘米级别。图5展示了来自corridor 1序列的宽基线回环闭合情况。而在magistrale 3序列中，由于里程计漂移较大导致帧间距离增加，因此无法成功找到回环。
TABLE I
ABSOLUTE TRAJECTORY ERROR (ATE) IN METERS ON THE TUM-VI DATASET [^5]. BEST RESULTS ARE IN BOLD.
| Seq | Stereo-VIO | Stereo-VI-SLAM | Length [m] |
| BASALT | OKVIS2 | Ours | ORB-SLAM3 | OKVIS2 | Ours |
| corridor | 1 | 0.34 | 0.54 | 0.31 | 0.03 | 0.02 | 0.01 | 305 |
| 2 | 0.42 | 0.44 | 0.15 | 0.02 | 0.06 | 0.01 | 322 |
| 3 | 0.35 | 0.55 | 0.21 | 0.02 | 0.03 | 0.01 | 300 |
| 4 | 0.21 | 0.11 | 0.04 | 0.21 | 0.10 | 0.01 | 114 |
| 5 | 0.37 | 0.51 | 0.17 | 0.01 | 0.09 | 0.01 | 270 |
| avg | 0.34 | 0.43 | 0.18 | 0.06 | 0.06 | 0.01 | 262 |
| magistrale | 1 | 1.20 | 2.03 | 1.74 | 0.24 | 0.07 | 0.01 | 918 |
| 2 | 1.11 | 3.22 | 1.00 | 0.52 | 1.22 | 0.01 | 561 |
| 3 | 0.74 | 2.11 | 1.53 | 1.86 | 0.09 | 1.39 | 566 |
| 4 | 1.58 | 1.94 | 1.26 | 0.16 | 0.25 | 0.01 | 688 |
| 5 | 0.60 | 1.01 | 0.21 | 1.13 | 0.02 | 0.01 | 458 |
| 6 | 3.23 | 2.32 | 1.66 | 0.97 | 0.76 | 0.01 | 771 |
| avg | 1.41 | 2.10 | 1.23 | 0.81 | 0.40 | 0.24 | 660 |
| room | 1 | 0.09 | 0.06 | 0.02 | 0.01 | 0.01 | 0.01 | 146 |
| 2 | 0.07 | 0.07 | 0.07 | 0.01 | 0.01 | 0.01 | 142 |
| 3 | 0.13 | 0.06 | 0.04 | 0.01 | 0.01 | 0.01 | 135 |
| 4 | 0.05 | 0.02 | 0.02 | 0.01 | 0.01 | 0.01 | 68 |
| 5 | 0.13 | 0.02 | 0.02 | 0.01 | 0.01 | 0.01 | 131 |
| 6 | 0.02 | 0.02 | 0.02 | 0.01 | 0.01 | 0.01 | 67 |
| avg | 0.08 | 0.04 | 0.03 | 0.01 | 0.01 | 0.01 | 115 |
| slides | 1 | 0.32 | 0.96 | 0.43 | 0.41 | 0.37 | 0.01 | 289 |
| 2 | 0.32 | 0.74 | 0.51 | 0.49 | 0.16 | 0.01 | 299 |
| 3 | 0.89 | 2.51 | 0.41 | 0.47 | 0.13 | 0.01 | 383 |
| avg | 0.51 | 1.40 | 0.45 | 0.45 | 0.22 | 0.01 | 324 |
需要注意的是，真实轨迹仅在序列的开头和结尾处可用，这使得真实轨迹的覆盖范围仅限于单个房间内的运动。因此，表I中报告的ATE并不能完全反映房间外估计轨迹的精确度。为了定性评估整体精确度，我们使用SE3对齐将结果转换到真实轨迹坐标系中，并检查累积点云的一致性。图6展示了各自行进轨迹对应的累积点云，它们以不同颜色标示。
### C. 在Newer College数据集上的性能表现 表II
EVALUATIONS ON NEWER COLLEGE DATASET BASED ON RELATIVE POSE ERROR (RPE) AND ATE (UNIT: METER).
| Seq | 10m RPE | ATE | Length\n[m] |
| Open VINS | ORB-SLAM3 | VILENS MC | Ours MC-VIO | Our Stereo | VIO MC | VI-SLAM Stereo | MC |
| MATH | 0.65 | Fail | 0.26 | 0.12 | 0.47 | 0.40 | 0.21 | 0.20 | 329 |
| MINE | 0.98 | Fail | 0.20 | 0.07 | 0.28 | 0.45 | 0.08 | 0.11 | 236 |
| QUAD | 1.01 | 0.23 | 0.31 | 0.18 | 0.73 | 0.64 | 0.24 | 0.37 | 244 |
| STAIRS | 0.33 | 0.20 | 0.16 | 0.09 | 0.09 | 0.07 | 0.05 | 0.04 | 59 |
| PARK | 0.18 | 0.13 | N/A | 0.10 | 4.27 | 5.08 | 0.53 | 1.04 | 2396 |
Newer College数据集[^26]包含四台鱼眼相机和一个惯性测量单元传感器。该数据集的难度和多样性……
---
这些场景涵盖了狭窄的楼梯、黑暗的地下矿井，以及超过2公里的户外步行路径。通过将激光雷达扫描数据与详细的先验地图对齐，为整个序列提供了地面真实数据。表二展示了在Newer College数据集上的评估结果。根据[^19]，我们基于10米范围内的相对位姿误差（RPE）指标，将我们的VIO系统分别与OpenVINS [^28]、ORB-SLAM3 [^3]以及VILENS-MC [^19]进行比较。结果显示，我们的方法在所有场景下均优于其他算法。值得注意的是，同样采用多摄像头配置的VILENS-MC与我们的方法最为接近，这凸显了利用多摄像头输入的优势。
Fig. 7. Before and after global semi-pose-graph BA on sequence Park.
图7展示了我们后端算法的有效性：在长步行序列“Park”中，经过半位姿图优化后，定位精度有了显著提升。在超过2公里的步行过程中，无论是穿过公园还是走廊，所估计的轨迹都保持平滑且高度一致，这一点从点云地图中清晰的建筑边缘和公园围墙即可看出。
除了使用相对指标外，我们还通过ATE指标来评估绝对精度。令人惊讶的是，实验结果表明立体视觉环境下的误差要小于多摄像头（MC）环境。我们推测这种差异可能源于外部校准误差，未来将进一步展开研究。在比较VIO与VI-SLAM的结果时，由于闭环检测与半位姿图优化的作用，VI-SLAM的结果明显优于VIO。可以说，VIO前端为后端提供了一个相当不错的起点，其里程计漂移较小；基于此，后端便能通过测量关键帧之间的距离来识别潜在的闭环。
### D. Hilti-Oxford数据集上的性能表现
TABLE III
ATE IN METER AND OBTAINED SCORES ON HILTI-OXFORD DATASET.
| exp01 | exp02 | exp03 | exp07 | exp09 | exp11 | exp15 | exp21 | score |
| OKVIS2 | - | - | - | - | - | - | - | - | 32.5 |
| Our VIO | 0.16 | 0.28 | 0.30 | 0.15 | 0.38 | 0.17 | 0.18 | 0.30 | 22.2 |
| Our SLAM | 0.10 | 0.15 | 0.15 | 0.14 | 0.28 | 0.21 | 0.17 | 0.30 | 40.9 |
Hilti-Oxford数据集[^6]属于2022年Hilti SLAM挑战赛的一部分，其传感器配置与Newer College数据集类似，包含五台鱼眼摄像头和一个惯性测量单元（IMU）。为了降低计算资源消耗，我们选取了五台摄像头中的四路画面进行处理，并排除了特征较少的顶部视角。该数据集在稀疏位置提供了毫米级的地面真实数据，还提供了用于评估的网页界面及评分排行榜以便进行基准测试。评估指标采用基于稀疏地面真实数据的ATE均方根误差，结果汇总于表三。我们的VIO系统所得到的ATE值在10厘米到30厘米之间。Exp01-exp03是在同一地点起止的建筑工地中采集的数据，具备闭环条件；因此我们的完整SLAM系统在这些序列上表现出色，最终得分超过了2022年Hilti SLAM挑战赛的获胜方案OKVIS2。
### E. 处理时间
PROCESSING TIME OF DIFFERENT MODULES (UNIT: MILLISECOND).
| IO | Extract\neature | Check keyframe | Preintegrate\nIMU | Tracking | Local-BA | Compute rel. pose | Total |
| Non-keyframe | 7 | 4 | 2 | - | - | - | - | 13 |
| Keyframe | 7 | 4 | 2 | 3 | 48 | 106 | 8 | 178 |
我们在TUM-VI数据集上评估了该系统在非关键帧和关键帧两种情况下的运行性能。表四列出了各阶段的时间分解结果。对于非关键帧，传入的图像会被处理至关键帧检测步骤，其平均处理时间为13毫秒。相比之下，每个关键帧还需经过四次单帧位姿图优化迭代以及两次局部位姿图优化迭代；随后还要计算关键帧的相对位姿因子及其在当前滑动窗口外的重投影因子。平均而言，处理一个关键帧需要约175毫秒。
该系统在处理速度与精度之间取得了良好的平衡：在TUM-VI数据集上，平均有23.5%的帧被选为关键帧，同时处理速度仍能达到每秒25帧，比录制时间快25%。不过在多摄像头模式下，由于计算量增大，系统在Newer College和Hilt-Oxford数据集上的处理速度会降至每秒22帧左右。在后端，程序结束时会执行一次闭环检测与全局半位姿图优化，对于Newer College Park这类长序列数据，这一过程可能需要数秒到几分钟时间。
### 七、结论
本文提出了一种先进的视觉惯性SLAM系统，能够将多台鱼眼摄像头的视觉重投影因子与IMU预积分因子融合到统一的因子图中。为了实现全局一致的建图，我们提出了一种新颖的半位姿图束调整方案，用以替代计算开销较大的完整光束法平差。通过交替进行光束法平差优化与RFT预测，该系统能够收敛到最大似然解。在多个数据集上的大量实验验证了所提系统的精度与鲁棒性。在未来的工作中，我们将致力于提升该系统在大规模室外场景下的鲁棒性，更好地应对天空、阴影等噪声像素的影响；此外，我们还计划引入词袋模型等全局闭环检测方法，以增强系统在因长距离移动产生较大里程计漂移时的闭环能力。
---
## 参考文献
[^1]: S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, "Keyframe-based visual-inertial odometry using nonlinear optimization," The International Journal of Robotics Research, vol. 34, no. 3, pp. 314–334, 2015.
[^2]: T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
[^3]: C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D. Tardós, “ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap slam,” IEEE Transactions on Robotics, vol. 37, no. 6, pp. 1874–1890, 2021.
[^4]: S. Leutenegger, “OKVIS2: Realtime scalable visual-inertial slam with loop closure,” arXiv preprint arXiv:2202.09199, 2022.
[^5]: D. Schubert, T. Goll, N. Demmel, V. Usenko, J. Stückler, and D. Cremers, “The TUM VI benchmark for evaluating visual-inertial odometry,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 1680–1687.
[^6]: L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scaramuzza, and M. Fallon, “Hilti-oxford dataset: A millimeter-accurate benchmark for simultaneous localization and mapping,” IEEE Robotics and Automation Letters, vol. 8, no. 1, pp. 408–415, 2022.
[^7]: B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzgibbon, “Bundle adjustment—a modern synthesis,” in Vision Algorithms: Theory and Practice: International Workshop on Vision Algorithms Corfu, Greece, September 21–22, 1999 Proceedings. Springer, 2000.
[^8]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular SLAM," in European conference on computer vision. Springer, 2014, pp. 834–849.
[^9]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE transactions on pattern analysis and machine intelligence, vol. 40, no. 3, pp. 611–625, 2017.
[^10]: Z. Teed and J. Deng, “DROID-SLAM: Deep visual SLAM for monocular, stereo, and RGB-D cameras,” Advances in Neural Information Processing Systems, vol. 34, pp. 16558–16569, 2021.
[^11]: W. Zhang, S. Wang, and N. Haala, “Towards robust indoor visual SLAM and dense reconstruction for mobile robots,” ISPRS Annals of the Photogrammetry, Remote Sensing and Spatial Information Sciences, vol. 1, pp. 211–219, 2022.
[^12]: P. Ji, Q. Yan, Y. Ma, and Y. Xu, “Georefine: Self-supervised online depth refinement for accurate dense mapping,” in Computer Vision–ECCV 2022: 17th European Conference, Tel Aviv, Israel, October 23–27, 2022, Proceedings, Part I. Springer, 2022, pp. 360–377.
[^13]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “IMU preintegration on manifold for efficient visual-inertial maximum-a-posterior estimation.” Georgia Institute of Technology, 2015.
[^14]: R. Mur-Artal and J. D. Tardós, “Visual-inertial monocular SLAM with map reuse,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 796–803, 2017.
[^15]: J. Kuo, M. Muglikar, Z. Zhang, and D. Scaramuzza, “Redesigning SLAM for arbitrary multi-camera systems,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 2116–2122.
[^16]: S. Urban, S. Wursthorn, J. Leitloff, and S. Hinz, “MultiCol bundle adjustment: a generic method for pose estimation, simultaneous self-calibration and reconstruction for arbitrary multi-camera systems,” International Journal of Computer Vision, vol. 121, no. 2, pp. 234–252, 2017.
[^17]: R. Mur-Artal and J. D. Tardós, “ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras,” IEEE transactions on robotics, vol. 33, no. 5, pp. 1255–1262, 2017.
[^18]: S. Ji, Z. Qin, J. Shan, and M. Lu, “Panoramic SLAM from a multiple fisheye camera rig,” ISPRS Journal of Photogrammetry and Remote Sensing, vol. 159, pp. 169–183, 2020.
[^19]: L. Zhang, D. Wisth, M. Camurri, and M. Fallon, “Balancing the budget: Feature selection and tracking for multi-camera visual-inertial odometry,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 1182–1189, 2021.
[^20]: Z. Teed and J. Deng, “RAFT: Recurrent all-pairs field transforms for optical flow,” in European conference on computer vision. Springer, 2020, pp. 402–419.
[^21]: Z. Teed and J. Deng, “RAFT-3D: Scene flow using rigid-motion embeddings,” in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2021, pp. 8375–8384.
[^22]: L. Lipson, Z. Teed, and J. Deng, “RAFT-Stereo: Multilevel recurrent field transforms for stereo matching,” in 2021 International Conference on 3D Vision (3DV). IEEE, 2021, pp. 218–227.
[^23]: W. Wang, D. Zhu, X. Wang, Y. Hu, Y. Qiu, C. Wang, Y. Hu, A. Kapoor, and S. Scherer, “TartanAir: A dataset to push the limits of visual SLAM,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 4909–4916.
[^24]: J. Kannala and S. S. Brandt, “A generic camera model and calibration method for conventional, wide-angle, and fish-eye lenses,” IEEE transactions on pattern analysis and machine intelligence, vol. 28, no. 8, pp. 1335–1340, 2006.
[^25]: W. Niemeier, “Ausgleichungsrechnung,” in Ausgleichungsrechnung. de Gruyter, 2008.
[^26]: L. Zhang, M. Camurri, and M. Fallon, “Multi-camera LiDAR inertial extension to the newer college dataset,” arXiv preprint arXiv:2112.08854, 2021.
[^27]: V. Usenko, N. Demmel, D. Schubert, J. Stückler, and D. Cremers, “Visual-inertial mapping with non-linear factor recovery,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 422–429, 2019.
[^28]: P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A research platform for visual-inertial estimation,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 4666–4672.