本文已被接受在IEEE Transactions on Robotics (T-RO)的视觉SLAM特刊上发表。最终版本将通过IEEE Xplore提供。

##### DOI: 10.1109/TRO.2025.3619051

©2025 IEEE。允许个人使用本材料。若需用于其他用途，无论是当前还是未来的媒体形式，包括为广告或宣传目的重新印刷/发布本材料、创建新的合集、转售或分发至服务器或列表，或是将本作品中的任何受版权保护的内容用于其他作品，都必须获得IEEE的许可。

---

# OKVIS2-X：一种支持密集深度或激光雷达以及GNSS的可配置开源关键帧视觉-惯性SLAM系统

Simon Boche*，Jaehyung Jung*，Sebastián Barbas Laina*，Stefan Leutenegger

摘要—为了赋予移动机器人可用的地图以及最高的状态估计精度和鲁棒性，我们提出了OKVIS2-X：一种先进的多传感器同步定位与建图（SLAM）系统，能够构建密集的体积占据地图，同时可扩展至大型环境并实时运行。我们的统一SLAM框架无缝集成了多种传感器模式：视觉、惯性、实测或学习得到的深度数据、激光雷达以及全球卫星导航系统（GNSS）测量数据。与大多数先进的SLAM系统不同，我们在利用深度或测距传感能力时采用密集体积地图表示方式。我们采用了高效的子图策略，使系统能够扩展到大型环境，在长达9公里的序列中得到了验证。OKVIS2-X通过地图对齐因子将估计器与子图紧密耦合，从而提升了其精度和鲁棒性。我们的系统可提供全局一致的地图，可直接用于自主导航。为了进一步提高OKVIS2-X的精度，我们还加入了在线校准相机外参的功能。在EuRoC数据集上，我们的系统在轨迹精度方面超越了所有现有先进方法；在Hilt22仅视觉基准测试中，它也优于所有竞争对手，其激光雷达版本同样表现出色；在VBR数据集的多样化大规模序列中，它展现了业界领先的精度。代码获取地址：<https://github.com/ethz-mrl/OKVIS2-X>。

索引词—SLAM；地图构建；定位；传感器融合

## 一、引言

随着自主机器人和移动设备日益普及，它们感知环境并与之建立空间关系的能力依然至关重要。为了应对可能未知的环境，某些应用场景要求系统能够同时完成环境建模与定位。

此类同步定位与建图（SLAM）系统通常需要异构传感器来提高精度和鲁棒性，其中视觉-惯性配置因其成本与精度之间的理想平衡而成为首选传感器组合之一。该配置结合了摄像头提供的空间信息与惯性测量单元（IMU）提供的本体感知时间信息；当两者紧密耦合时，即可观测到重力方向，从而降低对外部依赖。

![Image](images/f086e691881434f5b99909ee300c6e33ebda5c7a51f1bfbfebb8edf4a4e2243f.jpg)

<div style="text-align: center;">Fig. 1. 3D reconstruction from a run of OKVIS2-X on the Spagna sequence of the VBR dataset [^1]. Reconstruction with a LiDAR sensor (top) or with a depth network (bottom) to showcase the versatility of the presented system to different sensor modalities. The estimated trajectory is visualized in black. Furthermore, different colors per submap are used.</div>

脆弱的视觉数据关联。对于需要更高地图绘制和定位精度的应用场景而言，激光雷达因其无与伦比的深度感知分辨率和精度而成为一种强大的选择，不过成本相对较高。为了提升SLAM系统的鲁棒性，近年来人们对能够结合这三种传感器模态的系统愈发感兴趣，但这也增加了对所有不同传感器之间外部参数进行精确校准的难度——而这是实现精准SLAM系统的基本要求。此外，对于需要全球定位的系统来说，如果能稳定接收到信号，GNSS显然是户外应用的首选，不过并非所有环境都能保证信号的稳定接收。

文献中提出的大多数视觉惯性SLAM（VI SLAM）系统[^2] [^3] [^4] [^5]仅利用一小部分相机信息来构建稀疏地图，因此缺乏下游任务所需的相关几何信息。为解决这一局限，研究界提出了

---

![Image](images/e34c687f8e0da09226357c69172a0dad09f6376c678edff2513d8ec46cb386c6.jpg)

<div style="text-align: center;">Fig. 2. System architecture of our proposed multi-sensor state-estimator OKVIS2-X. Components with a grey background correspond to original elements from OKVIS2 [^2] and components with a white background are the extensions for the multi-sensor setup.</div>

能够利用视觉深度信息[^6] [^7] [^8]或激光雷达信息[^9] [^10] [^11]来构建更密集的地图，以便后续复用——这自然会增加内存需求。底层地图表示不仅在内存占用方面至关重要，对支撑下游任务而言或许更为重要。我们认为，体积占据地图在支持安全路径规划与导航方面具有无可比拟的优势，它能明确表征自由空间，而常用的点云（或其他常见表示形式如网格）则无法做到这一点。

为应对上述挑战，我们提出了一种名为OKVIS2-X的多传感器SLAM系统，它是OKVIS2的重大扩展[^2]。其核心思路是利用基于子图的密集体积占据地图，通过子图对齐因子将其与状态估计器紧密耦合。图1展示了利用我们的框架得到的3D重建示例。此外，我们还提出了一种统一的框架，用于融合视觉、惯性、激光雷达或神经网络生成的深度数据，以及来自GNSS的全球位置信息。我们此前的研究已经针对不同的传感器模态对OKVIS2进行了扩展：视觉-惯性-GNSS[^12]、视觉-惯性-激光雷达[^13]以及视觉-惯性-深度[^14]。据作者所知，我们在此首次提出了一种基于体积地图表示、可配置的多传感器SLAM系统，它支持多种新颖的传感器组合（如视觉-惯性-激光雷达-GNSS和视觉-惯性-深度-GNSS），并通过因子图框架实现。另一个创新之处在于，我们无缝支持了相机外部参数的在线校准。图2描述了可配置多种传感器模态的OKVIS2-X系统架构。最终系统在定位与建图方面展现出无与伦比的精度和鲁棒性，在从米级到公里级范围内构建密集体积地图时也具有出色的可扩展性——我们在一系列全新的实验中验证了这一点。我们提出的方法的主要贡献如下：

- 我们提出了一种最先进的多传感器SLAM系统，能够实时融合视觉、惯性、GNSS数据，以及来自激光雷达测量或深度网络的密集建图因子。

- 我们的方法将状态估计器与体积占据建图紧密耦合，凭借子图表示，使其适用于大规模SLAM场景。

- 我们的方法支持在视觉因子中对相机与IMU的外参进行在线标定，包括相对位姿误差项。

- 我们在包含大规模场景（最长达9公里）的基准数据集上进行了全面评估，结果显示我们在视觉惯性SLAM和视觉惯性激光雷达SLAM领域取得了最先进的成果。

### II. 相关工作

- 我们将开源发布我们的框架，以促进多传感器SLAM领域的研究。

我们回顾了多传感器SLAM、稠密SLAM以及基于子图的SLAM这几个主题下最具相关性的研究成果。

---

### A. 视觉惯性SLAM

视觉惯性SLAM系统通常分为两大类：松耦合系统和紧耦合系统。松耦合估计器通常先仅利用图像计算（相对）位姿，再与IMU测量数据结合；而紧耦合方案则会同时考虑所有测量数据，通常通过IMU运动学与视觉观测的结合来实现，例如通过重投影误差的形式。后一种方法被认为精度更高。根据估计器的不同，还可分为两类：一类是通过滤波递归估计最近的一组状态，例如 [^15]；另一类则是以非线性最小二乘的方式优化一个状态窗口，例如 [^3]、[^16]。另一方面，根据残差类型的不同，直接法 [^17] 会最小化光度误差，而基于关键点的算法，如 [^3]、[^16]、[^18]，则通过最小化重投影误差来估计机器人位姿和稀疏路标。

VINS-Mono [^19] 提出了一种融合IMU预积分和重投影误差的紧耦合非线性优化型视觉惯性SLAM方法。OpenVINS [^5] 基于多状态约束卡尔曼滤波器（MSCKF [^15]），并进一步扩展了长期SLAM路标、外参和内参的在线标定功能。ORB-SLAM3 [^3] 是一种支持单目、双目及RGB-D配置的多会话SLAM的紧耦合视觉惯性SLAM系统，其在公开SLAM数据集上展现了顶尖的精度。OKVIS2 [^2] 是一种带有回环闭合功能的多摄像头SLAM系统，其位姿图因子由边缘化的路标构成。MAVIS [^20] 同样是一种基于矩阵李群进行IMU预积分的多摄像头SLAM系统。SVIn2 [^21]、[^22] 是一种将回环闭合引入因子图优化的视觉惯性框架，同时还能集成其他传感器模态，以便在水下环境中部署使用。

在基于深度学习的视觉惯性SLAM领域，DeepVIO [^23] 是一种自监督的视觉惯性里程计网络，可直接从输入的传感器测量数据中回归出轨迹。DVI-SLAM [^24] 能够学习特征度量和重投影因子的置信度图，并配备密集束调整层。DBA-Fusion [^25] 也利用DBA来紧密融合视觉惯性测量数据。研究人员在大规模环境中展示了稠密点云建图结果。不过，作为端到端SLAM系统的核心组件，DBA层往往需要占用大量的GPU内存（$\approx$ 24 GB）。

与以往的工作不同，OKVIS2-X提供了一个通用框架，可用于处理多摄像头和多模态数据，包括深度网络、激光雷达以及GNSS接收机。此外，我们的方法超越了传统的稀疏地标地图表示方式，能够构建可扩展至大型场景的密集体素占据表示。

### B. 激光雷达（视觉-惯性）SLAM

由于激光雷达传感器具备固有的高精度深度感知能力，它在机器人定位中展现出巨大优势，能够显著减少SLAM系统不可避免的漂移现象。最早成功证明激光雷达传感器适用于SLAM的系统之一是LOAM [^26]。其核心思想是提取显著的几何特征，如边缘和平面，这些特征可以在不同帧间被可靠地追踪。LOAM启发了一系列采用显著特征提取概念的研究 [^27]、[^28]。与视觉-惯性SLAM类似，激光雷达SLAM也可进一步分为基于特征的方法和直接法。诸如Kiss-ICP [^29]或CT-ICP [^30]之类的直接激光雷达SLAM方法，通常基于原始激光雷达点云运用迭代最近点（ICP）算法的变体，无需提取显著特征。[^31]、[^32]针对基于特征和直接法的激光雷达扫描建立了滑动窗口光束法平差（BA）。

融合互补的惯性测量单元（IMU）数据后，该方法便可应用于高动态系统。LIO-SAM [^33]构建了一个紧耦合的位姿图优化问题，将边缘和平面误差残差与IMU误差残差相融合。基于滤波的方法，如FAST-LIO [^34]或FAST-LIO2 [^35]，也通过融合激光雷达和IMU数据成功实现了高精度定位。与仍使用平面和边缘特征的FAST-LIO不同，FAST-LIO2是一种直接法，通过直接处理原始点云大幅提升了运算速度。

近年来，激光雷达-视觉-惯性SLAM系统越来越受欢迎。其中大量方法仍沿用LOAM的几何特征提取思路。LVI-SAM [^10]结合了视觉-惯性（VI）和激光雷达-惯性（LI）子系统，以便在复杂场景中相互补充。LI系统基于IMU预积分误差以及边缘和平面残差构建因子图。VILENS [^11]同样构建了一个包含视觉、惯性、机器人腿部里程计以及基于激光雷达的线条和平面残差的优化问题。另一类研究采用基于MSCKF [^15]的方法来实现视觉、惯性和激光雷达数据的紧耦合融合，例如LIC-Fusion [^36]及其后续版本LIC-Fusion 2.0 [^37]。R2LIVE [^38]则将视觉特征、IMU和激光雷达数据整合到迭代卡尔曼滤波器中。与前述LVI SLAM系统不同，后续的R3LIVE [^39]采用了直接法，从而在无纹理或无结构的环境中增强了鲁棒性。FAST-LIVO [^40]和FAST-LIVO2 [^9]也构建了迭代扩展卡尔曼滤波器（IEKF）框架。IEKF更新步骤包含两个连续的状态更新：基于点云到平面的激光雷达更新，以及作用于图像块而非密集像素的稀疏直接视觉更新。这两种更新均通过局部体素图以帧到地图的方式进行计算。

与以往的方法不同，所提出的OKVIS2-X能够融合激光雷达测量数据，而无需提取显著的几何特征或任何对应关系——这通常是一项计算开销较大的步骤。基于我们之前的工作[^13]，残差可以直接从密集的占据图中获取；这些占据图是逐步构建的，可直接用于后续任务。这些残差会被加入因子图优化问题中。

---

### C. 多传感器SLAM与GNSS融合

早期关于融合全局位置测量以减少视觉惯性SLAM中漂移的研究主要以基于滤波的方法为主，这些方法大多建立在[^15]中的开创性工作之上，该工作提出了扩展卡尔曼滤波器（EKF）来解决实时视觉惯性导航问题。[^41]和[^42]分别采用EKF和无迹卡尔曼滤波器（UKF）来融合不同传感器模态的数据，比如激光雷达和GNSS测量值。此外，[^43]还在MSCKF [^15]框架内在线估计IMU与GNSS的空间外参以及传感器时间偏移。[^44]提出了一种鲁棒的迭代误差状态卡尔曼滤波器（IESKF），以紧耦合的方式融合GNSS、IMU和激光雷达测量数据。MINS [^45]是一种不仅融合了激光雷达-视觉-惯性信息，还融合了轮式里程计和GNSS数据的基于滤波的系统；该系统扩展了[^5]的研究成果，证明了多传感器信息能够提升状态估计的鲁棒性。总体而言，基于滤波的方法在计算上效率较高。不过，正如[^46]中所探讨的那样，基于优化的方法由于能够重新线性化旧的状态估计，因此有望给出更优的结果。

VINS-Fusion [^18]和GOMSF [^47]是基于优化的方法的范例，它们以松耦合的后端位姿图优化的方式融合全局位置测量数据。然而，为了充分利用不同传感器模态所提供的信息，在统一的优化问题中实现紧耦合融合才是理想的选择。[^33]提出了将GNSS因子整合到激光雷达-惯性里程计中，形成因子图优化问题。[^48]提出了一种紧耦合方法，在视觉惯性里程计的优化窗口中将GNSS测量值作为全局因子，同时利用IMU预积分技术。[^49]、[^50]和[^51]也提出了基于优化的紧耦合框架，这些框架不仅考虑了全局位置测量数据，还考虑了来自原始GNSS测量的伪距和多普勒频移误差。与以往纯粹的里程计方法不同，[^52]和[^53]成功实现了将全局位置测量数据融入包含视觉回环闭合的视觉惯性SLAM中。

虽然 [^48] 假设全局位置测量是在视觉-惯性参考系中给出的，但在实际应用中，必须估计全局参考系与局部参考系之间的 4 自由度（DoF）变换。关于这种全局坐标系对齐的初始化问题，已有研究利用局部和全局位置测量的对应关系通过 SVD 方法来解决 [^47]；或者采用基于最小二乘优化的紧耦合方式来应对 [^43]、[^49]、[^50]。此外，[^43] 还提出了一种基于行进距离的、关于 4-DoF 变换可观测性的启发式准则。如同 [^47] 中所描述的以及我们之前的工作 [^12] 一样，我们将采用基于 SVD 的方法来进行 GNSS-VIO 外参的初始化，不过此次会以紧耦合的方式完成估计。与上述方法不同，为了简化问题，一旦全局参考系变得可观测，我们的方法就会将其固定下来。我们并未像 [^43] 中那样采用启发式阈值，而是引入了一种完全考虑不确定性的准则来确定所估计的 GNSS 外参的协方差。此外，我们的框架不仅支持视觉回环闭合，还提供了一种类似回环闭合的优化方法，能够有效解决 GNSS 信号中断时间较长时的漂移问题，这一点在 [^12] 中已有阐述。在本文的新实验中，我们还展示了 OKVIS2-X 如何支持在 GNSS 信号间歇性可用情况下的激光雷达-视觉-惯性 SLAM。

### D. 稠密 SLAM 与建图

此前提到的大多数 SLAM 系统仅依靠稀疏的地图表示（如稀疏的 3D 地标或特征地图）就能实现高精度的定位。然而，要部署全自主机器人执行下游任务，理解稠密的几何信息必不可少。

KinectFusion [^6] 开创了稠密 SLAM 领域，采用了截断有符号距离场（TSDF）融合以及帧到模型配准技术。ElasticFusion [^54] 用稠密的曲面元素来建模环境，在出现回环后会以非刚性方式更新这些曲面元素。CNN-SLAM [^7] 和 DeepFusion [^55] 利用卷积神经网络生成的稠密深度图，并融合运动立体视觉得到的深度数据，从而进一步提升深度图质量。Kimera [^56] 及其扩展版本 Kimera2 [^4] 提出了由视觉-惯性观测构建的动态场景图；在其地图表示中，带有语义标注的网格会通过回环闭合约束发生形变。TANDEM [^57] 将深度多视图立体网络集成到视觉里程计中，还会根据 TSDF 渲染深度图以实现图像对齐。Simplemapping [^58] 采用了 SimpleRecon [^59]，并将多视图立体网络生成的深度图与 ORB-SLAM3 [^3] 提供的位姿及稀疏地标信息融合到 TSDF 中，但建图过程不会收到任何反馈。SigmaFusion [^60] 基于 Droid-SLAM [^61] 的信息矩阵来预测稠密深度的不确定性，通过整合不确定性信息得到了噪声更少的重建结果。不过，他们的方法对内存占用较大，且建图与状态估计是相互独立的。近期，SLAM 领域涌现出一系列基于几何基础模型的新方法，其中单目 SLAM 系统 MASt3R-SLAM [^62] 便是最著名的例子之一。这些系统利用点云地图来完成建图和定位任务。

在密集神经SLAM领域，地图的几何与外观信息均得益于神经隐式表示。NICE-SLAM [^63]利用分层特征网格来表示地图，这些特征网格随后被解码为体积占据图。UncLe-SLAM [^64]在NICE-SLAM的跟踪与建图损失函数中引入了像素级的深度和颜色不确定性，以合理反映观测不确定性。Point-SLAM [^65]则避免采用基于网格的表示方法以节省内存，它仅在表面附近分配神经点，这些神经点随后被用于获取地图的占据信息与颜色。Loopy-SLAM [^66]在Point-SLAM的基础上进行了扩展，通过在神经点表示中引入回环约束，采用子图策略——将神经点锚定在每个子图中，从而构建全局一致的地图。上述密集神经SLAM方法在（多）房间规模的环境中展现了出色的外观渲染能力。然而，这些研究方法在公里级大规模场景中的可扩展性尚不明朗，这也是本文主要解决的挑战，毕竟神经表示本身具有极高的维度。

---

这种特征表示方式——或者说这些表示方法如何有效支持移动机器人导航。

相比之下，我们提出的系统能够为通用传感器配置实现状态估计器与体积自由空间映射之间的紧密耦合，正如我们之前针对深度相机 [^14] 和激光雷达传感器 [^13] 所做的研究所示。所提出的方法具备完全的概率特性，因此也能在后续任务中考虑不确定性因素。

### E. 子图方法

为了最小化长期场景中的漂移现象，最新研究通常采用子图概念。这一概念源自早期的SLAM研究，例如Atlas框架 [^67]。在此背景下，可以推导出额外的因子来对齐子图并减少漂移。[^11]利用局部点云子图，并将ICP里程计测量结果纳入因子图。Wildcat [^68]是一种基于滑动窗口优化的激光雷达-惯性里程计系统，它通过构建局部曲面子图并对齐这些子图，实现了顶尖的鲁棒性与精度。这些方法采用基于点或表面的3D表示方法。虽然它们显然可用于高精度定位与3D重建，但由于难以区分自由空间与未被观测的空间，因此这种地图表示方法并不适合导航用途。

关于体积地图上的子图构建，已有多项研究涉及。[^69]基于OctoMap [^70]构建占据子图，并通过标准的ICP配准方法对齐这些子图。Voxgraph [^71]利用视觉惯性里程计提供位姿信息，将其整合到TSDF地图中。完成后会生成欧几里得有符号距离场（ESDF），并通过位姿图优化对齐子图。新子图以固定频率创建，因此会占用较大的内存。[^72]则采用占据图作为3D表示方法。他们使用Supereight2 [^73]这种自适应分辨率映射方法，根据行进距离来生成新子图。子图会根据视觉惯性估计器的更新而重新排布。后续的[^74]通过评估新扫描的点云重叠情况来改进子图创建过程，子图的对齐则基于ICP实现。

在这项工作中，我们同样采用了子地图的概念。与[^71]、[^72]、[^74]不同，子地图的全局对齐并非与估计器解耦，而是能提供直接反馈。我们像在[^71]中那样定义无对应关系的残差，无需耗费大量精力去提取ESDF，因为我们直接利用现有的占据信息。

### III. 预备知识

### A. 符号与定义

经典的视觉惯性SLAM问题表述中包含多个不同的坐标帧。移动物体相对于固定的世界参考帧$\mathcal{F}_W$进行跟踪。我们将IMU帧记为$\mathcal{F}_S$，相机坐标帧记为$\mathcal{F}_{C_i}$，共有$i = 1 \ldots N$个相机。若要融合激光雷达数据，就需要引入激光雷达传感器帧$\mathcal{F}_L$；若要融合GNSS测量数据，则需要引入全局参考帧$\mathcal{F}_G$，这是一个与重力对齐的东-北-上（ENU）局部笛卡尔坐标系，其位置位于首次接收到的GNSS测量的全球坐标处。子地图帧记为$\mathcal{F}_M$，当新子地图被创建时，它与$\mathcal{F}_S$相对应。

刚体变换$\boldsymbol{T}_{AB} \in SE(3)$用于在两个坐标帧之间转换齐次点：$A \mathbf{r}_P = T_{ABB} \mathbf{r}_P$，其中$A \mathbf{r}_P$是点P在坐标帧$\overrightarrow{F}_A$中的位置。$T_{AB}$的旋转部分由$\mathbf{C}_{AB} \in SO(3)$表示，平移分量则由$A \mathbf{r}_B$表示。我们也用单位四元数形式$\mathbf{C}_{AB}$来表示旋转$\mathbf{q}_{AB}$。

### B. 状态定义

本工作中的状态表示为：

 $$ \mathbf{x}=\left[_{W}\mathbf{r}_{S}^{T},\mathbf{q}_{W S}{}^{T},{}_{W}\mathbf{v}^{T},\mathbf{b}_{\mathrm{g}}^{T},\mathbf{b}_{\mathrm{a}}^{T}\right]^{T}, $$

其中，$w_{RS}$、$\mathbf{q}_{WS}$和$w\mathbf{v}$分别表示IMU传感器帧在固定世界帧中的位置、姿态和速度。$\mathbf{b}_{\mathrm{g}}$和$\mathbf{b}_{\mathrm{a}}$分别对应陀螺仪和加速度计的偏差。为了融合视觉-惯性系统与全局测量数据，我们估计估计器的世界参考帧$[G\mathbf{r}_{W}^{T},\mathbf{q}_{GW}^{T}]$与GNSS参考帧$\overrightarrow{F}_{W}$之间的外部变换$\overrightarrow{F}_{G}$。若要对相机-IMU的外部参数进行在线标定，状态还会进一步扩展，加入针对$[s\mathbf{r}_{C_{i}}^{T},\mathbf{q}_{SC_{i}}^{T}]$个相机的外部参数$i=1\ldots N$，其中N为相机数量。

### IV. 体积占据映射

获取精确的地图表示对于状态估计和后续任务至关重要。大多数先进的视觉惯性SLAM系统[^3]、[^5]、[^20]、[^75]都基于图像特征构建稀疏地图，这类地图虽可用于状态估计，但由于缺乏密集的几何细节，无法用于大多数后续任务。为解决这一问题，OKVIS2-X利用基于多分辨率映射框架Supereight2的密集体积占据子地图[^73]，这些子地图既可用于状态估计，也能应用于自主导航[^76]、探索[^77]或地点识别[^78]等后续任务。

在OKVIS2-X中，我们摒弃了整体映射的概念，采用子映射策略来表示环境，从而获得了两大优势。首先，数据被分配到较小的映射中，使得数据结构更浅，进而降低了整合深度信息的计算成本。其次，每个子映射都锚定在状态估计器生成的各个关键帧状态上，从而在更新时能够调整子映射的姿态。

子映射创建时，其子映射参考坐标系$\overrightarrow{F}_M$会锚定在一个关键帧状态$\mathbf{x}^k$上，而该子映射的本体姿态$T_{WS_k}$则体现了$\overrightarrow{F}_M$与$\overrightarrow{F}_W$之间的关系。一旦$\mathbf{x}^k$发生更新，新的本体姿态$T_{WS_k}$就会同步更新$\overrightarrow{F}_M$与$\overrightarrow{F}_W$之间的刚性变换关系，以此确保子映射间的局部一致性，提升整体映射精度。每个子映射都会维护占用对数几率值，该值会通过激光雷达点云或深度图像D来更新。地图M中某点的对数几率定义为：

 $$ l\left(_{M}\mathbf{p}\right)=\log\frac{P_{\mathrm{o c c}}\left(_{M}\mathbf{p}\mid\mathbf{T}_{M C},\mathbf{D}\right)}{1-P_{\mathrm{o c c}}\left(_{M}\mathbf{p}\mid\mathbf{T}_{M C},\mathbf{D}\right)}, $$

---

其中$P_{occ}$为占用概率。与Supereight2类似，我们在整合新的深度信息时，会对对数几率占用值进行贝叶斯加法更新。对数几率均值$L(\cdot)$会按如下方式递归更新：

 $$ \begin{align*}L_{k}\left(_{M}\mathbf{p}\right)&=\frac{L_{k-1}\left(_{M}\mathbf{p}\right)w_{k-1}+l\left(_{M}\mathbf{p}\right)}{w_{k-1}+1},\\w_{k}&=\min\{w_{k-1}+1,\;w_{\max}\},\end{align*} $$

其中$w_{k}$表示观测次数，且会在$w_{max}$处饱和。这种更新机制能够缓解因深度感知不精确产生的几何不一致问题——当系统采用学习得到的深度数据时，这一问题尤为突出；同时它还能增强对场景中动态物体的鲁棒性。

我们的子映射策略基于这样一个假设：单个子映射内的漂移可忽略不计。因此，要生成新子映射需满足两个条件：其一，当前子映射中已整合了足够数量的最小深度测量值；其二，当前深度或激光雷达测量数据与已完成的子映射之间的重叠度低于阈值，或者自当前子映射创建以来已生成的关键帧数量不超过K个。关键帧准则可作为子映射内漂移程度的代理指标，而重叠度准则则能确保可以将基于子映射的对齐约束可靠地添加到状态估计器中。

通过利用占用概率作为地图表示方法，可将环境划分为自由区域、占用区域或未观测区域，这一区分对于安全的自主导航而言至关重要。与[^76]类似，路径规划器只会遍历子映射中被判定为自由的区域，因为它将未知区域视为不可通行区域；且这些路径会根据子映射的更新而发生弹性形变。

Supereight2 [^73]采用分段线性逆传感器模型来描述测量深度对应的对数几率占用值。在该模型中，对数几率为零时对应测量的表面位置。此外，深度不确定性$\sigma$也可表示为测量深度$z_{r}$的函数。在我们的研究中，我们假设激光雷达传感器的不确定性呈线性增长，而RGB-D相机的不确定性则呈二次方增长。

然而，对于学习得到的深度数据而言，这些启发式不确定性模型并不够用。例如，无纹理区域或物体边界会因相关体积模糊或视角变化而产生较高的深度不确定性。针对这一问题，我们将在下一节介绍一种深度融合方法，用于预测逐像素的不确定性。

### A. 考虑不确定性的深度融合

对于我们的下游任务而言，获取可靠的密集深度图以及相关的不确定性信息至关重要。我们沿用了先前工作[^14]中提出的核心思路：融合来自静态立体视觉和运动立体视觉的深度数据，这两类数据恰好互补——静态立体视觉即便在相机静止时也能提供基线长度稳定的可靠深度信息，而运动立体视觉则能根据相机的运动状态产生较大的基线。我们的方法并不依赖特定的网络结构，但发现Unimatch [^79]和MVS网络[^58]在真实场景中的表现优异。不过，以往的网络仅能预测视差或深度，无法输出任何不确定性信息。

![Image](images/5386770e181db2f72742a4c9c45473a18c4cec5cae81d299debade1355b443b2.jpg)

<div style="text-align: center;">Fig. 3. Predicted inverse depth (top) and its corresponding standard deviation (bottom) of (a) stereo network with the 11 cm baseline, (b) MVS network with the 50 cm maximum baseline among 8 views, and (c) depth fusion in the EuRoC dataset. (Adopted from [^14].)</div>

因此，我们在基础架构上增加了不确定性解码器，并采用拉普拉斯损失函数来进行（随机性）不确定性学习。具体而言，我们的立体视觉网络损失函数如下：

 $$ \begin{array}{r}{\mathcal{L}_{\mathrm{s t}}(\boldsymbol{\theta})=\mathcal{L}_{u}(\boldsymbol{\theta})+\mathcal{L}_{\nabla u_{x}}(\boldsymbol{\theta})+\mathcal{L}_{\nabla u_{y}}(\boldsymbol{\theta}),}\end{array} $$

其中$\theta$为网络权重，u代表视差。视差损失$\mathcal{L}_{u}$按照[^80]、[^81]中的设定，采用拉普拉斯分布进行建模，其定义为：

 $$ \mathcal{L}_{u}(\boldsymbol{\theta})=\sum_{i\in\mathcal{T}}\frac{\left|u_{i}-u_{\mathrm{g t}_{i}}\right|}{\sigma_{u_{i}}}+\log\sigma_{u_{i}}, $$

这里$\mathcal{T}$是包含立体图像对及真实视差标签的训练集。为了获得更清晰的不确定性输出，我们还加入了梯度损失$\mathcal{L}_{\nabla u}$，其定义方式与$\mathcal{L}_{u}$类似。水平和垂直方向上的梯度不确定性可由视差不确定性推导得出：

 $$ \begin{align*}\sigma_{\nabla u_{x}}&=\sqrt{\sigma_{u}^{2}(x+1,y)+\sigma_{u}^{2}(x-1,y)},\\\sigma_{\nabla u_{y}}&=\sqrt{\sigma_{u}^{2}(x,y+1)+\sigma_{u}^{2}(x,y-1)}.\end{align*} $$

我们通过线性化方法将不确定性从视差传递到深度。

 $$ \hat{d}_{\mathrm{s t}}=\frac{f_{c}b}{u},\quad\sigma_{\mathrm{s t}}=\frac{f_{c}b}{u^{2}}\sigma_{u}, $$

其中$f_{c}$为校正后的焦距，b为立体基线长度。

同样，我们对MVS网络[^58]的损失函数也做了修改：

 $$ \mathcal{L}_{\mathrm{m v s}}(\phi)=\sum_{i\in\mathcal{T}}\frac{\left|\log d_{i}-\log d_{\mathrm{g t}_{i}}\right|}{\sigma_{l_{i}}}+\log\sigma_{l_{i}}, $$

在该网络中，模型会学习对数深度的不确定性。我们通过线性化模型将对数深度转换为实际深度：

 $$ \begin{array}{r}{\hat{d}_{\mathrm{m v s}}=\exp\left(\log d_{i}\right),\quad\sigma_{\mathrm{m v s}}=\hat{d}_{\mathrm{m v s}}\sigma_{l}.}\end{array} $$

根据网络输出的逐像素估计值$(\hat{d}_{\mathrm{st}},\sigma_{\mathrm{st}})$、$(\hat{d}_{\mathrm{mvs}},\sigma_{\mathrm{mvs}})$，在假设这两个估计值相互独立的前提下，我们可以对两种深度估计结果[^82]进行最优融合：

 $$ \begin{aligned}\hat{d}_{\mathrm{fuse}}&=\sigma_{\mathrm{fuse}}^{2}\left(\sigma_{\mathrm{st}}^{-2}\hat{d}_{\mathrm{st}}+\sigma_{\mathrm{mvs}}^{-2}\hat{d}_{\mathrm{mvs}}\right),\\\sigma_{\mathrm{fuse}}^{2}&=\left(\sigma_{\mathrm{st}}^{-2}+\sigma_{\mathrm{mvs}}^{-2}\right)^{-1}.\end{aligned} $$

---

带有额外不确定性解码器的立体视觉网络和MVS网络仅在合成数据集[^83]上进行微调。图3展示了一个示例：风扇支架部分的深度信息在立体视觉网络中更为精细，而较远物体的细节则在MVS网络中更清晰。融合后的深度图自然能基于不确定性感知融合机制保留最优的深度结果。

### V. 多传感器SLAM

### A. 系统概述

OKVIS2-X提出了一种模块化的多传感器SLAM系统，专为大规模场景和机器人导航设计。它基于稀疏VI SLAM系统OKVIS2 $^{[2]}$开发，新增了在线相机-惯性测量单元外参标定功能，能够融合全球定位测量数据，还能利用激光雷达或深度传感数据生成密集地图对齐因子。图2展示了整个系统的架构，以及它相较于原始OKVIS2的扩展部分。

底层的VI SLAM系统分为视觉前端和实时估计器两部分：每当有新的（多帧）图像到达时，这两部分会同步处理图像和IMU消息。为了处理回环问题，系统会异步执行完整的因子图回环优化。如图2所示，前端负责状态初始化、关键点匹配、立体三角测量（针对连续帧以及同一多帧中的立体图像）；如果启用了相关功能，前端还会运行分割CNN来过滤掉天空中的观测数据，同时负责位置识别；一旦位置识别成功，便会进行重定位并初始化回环。随后，实时估计器会对相应的因子图进行优化，它还要负责通过边缘化旧观测数据来创建位姿图边，以及固定旧状态。一旦发生回环，估计器会将位姿图边重新转换为观测数据，然后触发围绕回环的完整图优化，该优化过程为异步执行，完成后会与实时因子图同步。

OKVIS2-X通过三个额外模块对该系统进行了扩展：深度网络、多传感器处理器和子图接口。深度网络利用立体图像供立体网络使用，同时接收来自实时估计器的机器人位姿和稀疏地标点，供MVS网络使用。这些稀疏地标点会被反投影到图像平面上，从而为网络提供深度先验。根据应用场景的不同，多传感器处理器会处理传入的GNSS测量数据以及来自激光雷达、深度相机或上述深度融合网络的几何测量数据。对于GNSS测量数据，处理器会构建全局位置残差并将其添加到实时估计器中；它还会计算自上次接收到GNSS测量数据以来所经过的时间。如果信号中断时间较长，处理器会触发一次类似回环的异步完整图优化，以补偿自上次接收测量数据以来可能累积的漂移误差。在进行密集子图对齐时，处理器会获取深度图像的位姿，或对传入的激光雷达点云应用基于IMU的运动去畸变处理；为此，它会保存一条轨迹的内部表示，且该表示始终与估计器保持同步。对于每个实时帧，系统会将相对于前一个子图的帧到地图因子添加到因子图中。

子图接口负责管理所有子图的集合。每个子图的位姿都锚定在状态估计器生成的关键帧位姿上，因此子图位姿会随着状态估计器的更新而同步更新，其使用的内部轨迹表示与多传感器处理器所用的表示一致。每次生成视觉关键帧时，系统都会检查传入测量数据与上一个子图的重叠比例，以此决定是否创建新子图。做出决定后，深度图像或激光雷达测量数据会被整合到当前活跃的子图中。子图创建完成后，优化问题中会加入密集的地图到地图因子，以保证重叠区域内子图的一致性。在后续章节中，我们将对各模块进行更详细的描述。

### B. 视觉前端

视觉前端会从多帧图像中的每一帧提取BRISK [^84] 关键点和描述符，并将它们与地图中已有的3D地标进行匹配；在此过程中会同时考虑描述符距离和投影图像距离。随后，新生成的3D地标既可以通过关键帧内所有图像的立体三角测量来初始化，也可以通过实时帧与任意关键帧之间的三角测量来初始化。是否将新帧判定为关键帧，取决于实时帧中匹配地标的占比，以及当前匹配结果在现有关键帧中的可见占比。如果重叠比例低于阈值，我们就会将当前的（多帧）图像设为新的关键帧。

经过微调的语义分割网络Fast-SCNN [^85] 可以在关键帧图像上运行。为了实现最大程度的可移植性和灵活性，推理过程会在CPU上异步执行，由于该网络效率较高且仅需处理关键帧，因此这一做法切实可行。在我们的实现中，天空中的匹配结果会被剔除。这种处理方式在场景物体移动缓慢的情况下能显著提升精度，尤其是云层这类物体——通过柯西鲁棒化机制，这类物体的观测数据不会自动被丢弃。

### C. 地点识别、重新定位与闭环检测

OKVIS2-X会维护一个DBoW2 [^86] 数据库。对当前帧进行DBoW查询后，会得到一系列作为闭环候选的匹配结果。要被认定为有效的闭环，还必须通过基于3D-2D RANSAC的额外几何验证步骤。随后，连接闭环状态的位姿图边会被“激活”，重新转化为地标和观测值（见图4(c)）；同时也会为当前匹配帧创建相应的观测值。如果已存在的新地标与旧地标相匹配，这还可能触发地标合并操作。为了优化闭环不一致性问题，误差会通过旋转平均的方式均匀分布在闭环周围，随后再进行位置不一致性的分配。之后会触发对整个位姿图的后台优化过程。优化完成后，同步过程会将优化后的状态和地标导入实时估计器，并重新对齐在此期间产生的新状态和地标。

---

![Image](images/c65420005a907deaddfcbee9189afd81099af8469da0f90faace2db9816a158e.jpg)

<div style="text-align: center;">(a) In the beginning.</div>

<div style="text-align: center;">(b) Later: posegraph creation.</div>

![Image](images/aaf75b1db1901aec43a0c5e7a2182feee673c037fa06da76ad5881a15a67fc4b.jpg)

<div style="text-align: center;">(c) Loop closure.</div>

![Image](images/07e16f0a278ef83eafc399732ae2ba158d8d9d3bec880ce3153c705b6cc15afb.jpg)

<div style="text-align: center;">(d) Modifications w. online extrinsics optimisation.</div>

<div style="text-align: center;">Fig. 4. Initially a full batch VI factor graph (a) is created and optimized. Later (b), frames with least overlap with the live frame and current keyframe are turned into posegraph poses by construction of relative pose errors under marginalization of common observations; also, old poses and speed/bias variables are fixed to keep the problem realtime capable. When a loop-closure occurs (c), respective observations and landmarks are re-activated. The proposed system furthermore supports online calibration of the IMU-camera extrinsics (d). ((a-c) Adopted from [^2].)</div>

### D. 视觉惯性SLAM

视觉惯性估计器会最小化视觉误差、惯性误差以及相对位姿误差（位姿图边），下文将对这些内容做简要介绍。图4(a)-(c)直观展示了随时间推移底层因子图的构建过程。

1) 视觉投影误差：与OKVIS2 [^2] 类似，OKVIS2-X采用第j个地标在时间戳k时第i个相机帧中的投影误差$\mathbf{e}_{\mathrm{r}}^{i,j,k}$及其在图像中对应的观测值$\tilde{\mathbf{z}}_{\mathrm{r}}^{i,j,k}$，其计算公式如下：

   $$ \begin{array}{r}{\mathbf{e}_{\mathrm{r}}^{i,j,k}=\tilde{\mathbf{z}}_{\mathrm{r}}^{i,j,k}-\mathbf{h}\left(\mathbf{T}_{S C_{i}}^{-1}\mathbf{T}_{W S_{k}}^{-1}w\mathbf{l}^{j}\right).}\end{array} $$

其中，$h(\cdot)$ 表示相机投影过程。

2) 惯性测量单元误差：在构建惯性测量单元残差时，我们采用了[^87] 中提出的惯性测量单元预积分方法。在时间步k和n之间，惯性测量单元误差为：

   $$ \mathbf{e}_{\mathrm{s}}^{k}=\tilde{\mathbf{x}}^{n}\left(\mathbf{x}^{k},\tilde{\mathbf{z}}_{\mathrm{s}}^{k,n}\right)\boxminus\mathbf{x}^{n}, $$

其中，$\tilde{\mathbf{x}}^{n}$ 表示以当前状态$\mathbf{x}^{k}$ 和惯性测量单元数据$\tilde{\mathbf{z}}_{\mathrm{s}}^{k,n}$ 为函数，在任意时间n处预测的状态。$\boxminus$ 执行常规的减法运算，但四元数除外（详见[^2]）。

3) 相对位姿图误差：时间步r（参考时刻）和c之间的相对位姿误差$\mathbf{e}_{\mathrm{p}}^{r,c}$ 由下式给出：

   $$ \mathbf{e}_{\mathrm{p}}^{r,c}=\mathbf{e}_{\mathrm{p},0}^{r,c}+\left[\begin{matrix}{s_{r}\mathbf{r}_{S_{c}}-s_{r}\mathbf{\overline{{r}}}_{S_{c}}}\\ {\mathbf{q}_{S_{r}S_{c}}\boxminus\mathbf{\overline{{q}}}_{S_{r}S_{c}},}\\ \end{matrix}\right] $$

其中，$S_r\mathbf{\bar{r}}_{S_c}$ 和 $\mathbf{\bar{q}}_{S_rS_c}$ 分别表示在IMU坐标系下标称的相对位置与姿态$\mathbf{\underline{F}}_{S_r}$。

简而言之，这些位姿图误差项是通过对帧r和c中的关节观测数据进行边缘化处理而得到的，从而排除掉路标的影响。关于如何利用最大生成树（MST）来选择待创建的位姿图边，详细解释请参阅[^2]。为了计算常数$\mathbf{e}_{p,0}^{r,c}$以及权重矩阵$\mathbf{W}_s^k$，我们将路标转换到参考坐标系$\mathbf{\underline{F}}_s S_r$中，并仅依据关节观测数据及相应路标来构建如下形式的高斯-牛顿系统$\mathbf{H}\mathbf{\hat{x}} \mathbf{\hat{x}} = \mathbf{b}$：即基于标准的重投影误差及众所周知的雅可比矩阵，最终得到以下结构：

 $$ \left[\begin{matrix}{\mathbf{H}_{\mathrm{p,p}}}&{\cdots}&{\mathbf{H}_{\mathrm{p},j}}&{\cdots}\\ {\vdots}&{\ddots}&{\mathbf{0}}&{\mathbf{0}}\\ {\mathbf{H}_{\mathrm{p},j}^{T}}&{\mathbf{0}}&{\mathbf{H}_{j,j}}&{\mathbf{0}}\\ {\vdots}&{\mathbf{0}}&{\mathbf{0}}&{\ddots}\\ \end{matrix}\right]\left[\begin{matrix}{\delta\mathbf{p}}\\ {\vdots}\\ {\delta\mathbf{l}_{j}}\\ {\vdots}\\ \end{matrix}\right]=\left[\begin{matrix}{\mathbf{b}_{\mathrm{p}}}\\ {\vdots}\\ {\mathbf{b}_{j}}\\ {\vdots}\\ \end{matrix}\right]. $$

此处仅出现一个位姿，即由$T_{S_rS_c}$所引用的相对位姿$\delta\mathbf{p}$。变量排序上，所有路标$(S^r\mathbf{l}^j)$被标记为$\delta\mathbf{l}_j$。接下来，利用舒尔补技术将路标边缘化：

 $$ \begin{array}{r l r}{{\bf H}^{\ast}}&{=}&{{\bf H}_{{\mathrm{p,p}}}-\displaystyle\sum_{i}{\bf H}_{{\mathrm{p},j}}{\bf H}_{j j}^{+}{\bf H}_{{\mathrm{p},j}}^{T},}\end{array} $$

 $$ \begin{array}{r l r}{{\bf b}^{*}}&{=}&{{\bf b}_{\mathrm{p}}-\displaystyle\sum_{j}{\bf H}_{\mathrm{p},j}{\bf H}_{j j}^{+}{\bf b}_{j},}\end{array} $$

由此便得到了简化版的高斯-牛顿系统

 $$ \mathbf{H}^{*}\delta\mathbf{p}=\mathbf{b}^{*}. $$

该系统现在围绕当前位姿$\overline{\mathbf{p}}$进行线性化，因此

 $$ \mathbf{H}^{*}\delta\mathbf{p}=\mathbf{b}^{*}-\mathbf{H}^{*}(\mathbf{p}\boxminus\mathbf{\overline{{p}}}) $$

相应的位姿图误差项所对应的高斯-牛顿系统形式如下

 $$ \mathbf{E}_{\mathrm{p}}^{r,c T}\mathbf{W}_{\mathrm{p}}^{r,c}\mathbf{E}_{\mathrm{p}}^{r,c}\delta\mathbf{p}=-\mathbf{E}_{\mathrm{p}}^{r,c T}\mathbf{W}_{\mathrm{p}}^{r,c}(\mathbf{e}_{\mathrm{p},0}^{r,c}+\mathbf{p}\boxminus\overline{{\mathbf{p}}}), $$

其中$\mathbf{E}_{\mathrm{p}}^{r,c}$表示雅可比矩阵。为了在线性化点处（即$\mathbf{p} = \overline{\mathbf{p}}$且$\mathbf{E}_{\mathrm{p}}^{r,c} = \mathbf{I}_{6}$时）使其与式(18)等价，我们可得到

 $$ \begin{array}{c}\mathbf{W}_{\mathrm{p}}^{r,c}=\mathbf{H}^{*},\\\mathbf{e}_{\mathrm{p},0}^{r,c}=-\mathbf{H}^{*+}\mathbf{b}^{*}.\end{array} $$

由于$\mathbf{H}^*$、$\mathbf{b}^*$和$\overline{\mathbf{p}}$均为常数，因此在代入$T_{S_rS_c} = T_{WS_r}^{-1} T_{WS_c}$之后，步骤r和c处关于位姿的雅可比矩阵便很容易确定。

### E. 在线相机-IMU外参标定

我们对[^2]进行了扩展，以全面支持相机-IMU外参位姿的在线标定$\boldsymbol{T}_{SC_i}, i = \{1, \ldots, N\}$。虽然这种扩展对于重投影误差而言较为简单且为人所熟知，但式(13)中所涉及的相对位姿图因子如今也依赖于$\boldsymbol{T}_{SC_i}$。因此，在边缘化共视路标之前，双视图高斯-牛顿系统会被扩充以包含外参位姿——这些外参位姿在边缘化路标后仍会保留。式(15)及以下内容保持不变，

---

![Image](images/5bea7bcd045d933b86e3cad4576f457e26ecc950f1da57266704d4ee6f0b7a89.jpg)

<div style="text-align: center;">Fig. 5. Factor Graph including dense submap alignment. Left: The realtime estimator connects set of current keyframe and non-keyframe states by IMU errors and visual reprojection errors. For every state in the optimization window, frame-to-map factors are formulated between every live state and the keyframe state associated to the last completed submap. Right: Measurements between frames can be aggregated and map-to-map factors can be added to the factor graph between submap keyframe states if the geometric overlap surpasses a threshold. (Adopted from [^13])</div>

只不过$\mathbf{H}$和$\mathbf{b}$的维度更高。式(13)也因此被扩展为

 $$ \mathbf{e}_{\mathrm{p o}}^{r,c}=\mathbf{e}_{\mathrm{p o},0}^{r,c}+\left[\begin{matrix}{s_{r}\mathbf{r}_{S_{c}}-s_{r}\mathbf{\overline{{r}}}_{S_{c}}}\\ {\mathbf{q}_{S_{r}S_{c}}\boxminus\mathbf{\overline{{q}}}_{S_{r}S_{c}}}\\ {s\mathbf{r}_{C_{1}}-s\mathbf{\overline{{r}}}_{C_{1}}}\\ {\mathbf{q}_{S C_{1}}\boxminus\mathbf{\overline{{q}}}_{S C_{1}}}\\ {\vdots}\\ {s\mathbf{r}_{C_{N}}-s\mathbf{\overline{{r}}}_{C_{N}}}\\ {\mathbf{q}_{S C_{N}}\boxminus\mathbf{\overline{{q}}}_{S C_{N}}}\\ \end{matrix}\right]\in\mathbb{R}^{6+6N}, $$

其中，$s\overline{\mathbf{r}}_{C_i}$和$\overline{\mathbf{q}}_{SC_i}$表示$i^{\mathrm{th}}$外参的线性化点，即在构建（扩充后的）相对位姿因子时所取的值。

相关的因子图结构如图4(d)所示。此外，为了规整估计问题，我们采用与机器人状态估计位姿先验相同的公式，为所有相机外参添加位姿先验$^{1}$。

### F. 子图对齐

正如第V-A节所述并在图5中所示，存在两种类型的密集子图对齐残差。帧到地图的残差可以在每个实时帧与最后一个已完成的子图所对应的关键帧之间建立。地图到地图的残差则是在两个子图所锚定的两个关键帧之间计算。不过就实际的误差公式而言，两者并无区别。给定一个与关键帧位姿$\pmb{T}_{WS_a}$相关联的已完成子图，以及与另一状态$\pmb{P}_b$相关联的点云$\pmb{T}_{WS_b}$，我们针对每个$_{S_b}$ $\pmb{p} \in \mathcal{P}_b$构建基于地图的残差，公式如下：

 $$ e_{\mathrm{m}}^{a,b}\left(_{S_{a}}\mathbf{p}\right)=\frac{d}{\sigma}=\frac{L\left(_{S_{a}}\mathbf{p}\right)}{\sqrt{\frac{L_{\mathrm{m i n}}^{2}}{9}+\sigma_{d}^{2}\left|\nabla L\left(_{S_{a}}\mathbf{p}\right)\right|^{2}}}, $$

其中$_{S_a}\mathbf{p} = T_{S_aS_b S_b}\mathbf{p}$满足$T_{S_aS_b} = T_{WS_a}^{-1}T_{WS_b}$。相应的雅可比矩阵可在附录D中找到。对于深度图像而言，点$_{S_b}\mathbf{p}$是通过反投影像素深度值得到的。地图对齐残差的核心思想是：每个测量到的点都应位于3D地图中的表面$(L = 0)$上；而该点到最近表面的距离d，可以根据占据值$L(\cdot)$推导得出，同时还有

![Image](images/ca2f976babfce16d0ccf956a2f522d14f911ee629fe2f7ff02d650091649fc81.jpg)

<div style="text-align: center;">Fig. 6. To evaluate the global position residual, the propagated state  $\hat{\mathbf{x}}^{j}$ at time step  j of the measurement is propagated from the state vector  $\mathbf{x}^{i}$ at time step  i of the camera frame by leveraging IMU preintegration. This intermediate state is only used for evaluation of the global factor. (Adopted from [^12].)</div>

占据梯度$\nabla L(\cdot)$，这里假设在表面附近呈现线性行为，这与绘图所用的传感器模型一致。距离d和地图不确定性可表示为：

 $$ d=\frac{L}{|\nabla L|},\qquad\sigma_{\mathrm{m a p}}=\frac{L_{\mathrm{m i n}}}{3|\nabla L|}. $$

$L_{min}$是一个配置参数，表示饱和的最小对数几率占据值。结合传感器特定的测量不确定性$\sigma_{d}$，我们可以利用总不确定性在方程(23)中构建加权残差。

 $$ \sigma=\sqrt{\sigma_{\mathrm{m a p}}^{2}+\sigma_{d}^{2}}. $$

我们发现，对于通常精度极高的激光雷达测量数据而言，采用各向同性的传感器不确定性假设便已足够；但对于网络生成的深度数据而言，这一假设并不成立。在这种情况下，应按照第IV-A节所述的方法，为方程(10)中的深度不确定性（其中常包含噪声观测值）逐像素分配权重，以便正确衡量其对优化损失的贡献。

### G. GNSS融合

为了融合来自GNSS等源的全局位置残差，我们扩展了状态向量，使其包含视觉惯性参考框架$T_{GW}$与全局参考框架$\overrightarrow{F}_{W}$之间的4自由度变换$\overrightarrow{F}_{G}$。该全局参考框架是以首次测量位置为原点的、与重力对齐的东-北-天（ENU）局部笛卡尔坐标系。

1) 全局位置误差：在时间步j处，测量值$z^{j}$对应的全局位置残差可表示为：

   $$ \mathbf{e}_{\mathrm{g}}^{j}=\mathbf{z}^{j}-\left[\mathbf{C}_{G W}\left(_{W}\hat{\mathbf{r}}_{S_{j}}+\hat{\mathbf{C}}_{W S_{j}S}\mathbf{r}_{A}\right)+_{G}\mathbf{r}_{W}\right]. $$

此处，$s\mathbf{r}_A$考虑了GNSS天线在IMU传感器坐标系中的位置，该位置被假定为事先已知。为了应对GNSS测量数据与相机帧的异步到达问题，$w\hat{\mathbf{r}}_{S_j}$和$\hat{\mathbf{C}}_{WS_j}$是在时间步j时IMU位姿的预测值，它们可通过时间步i时的先前状态经IMU预积分得到。这一过程如图6所示。与[^48]相比，公式(26)中的表述通过引入全局参考框架与局部参考框架之间的4自由度变换$\mathbf{T}_{GW}$，对全局残差进行了扩展。误差雅可比矩阵的推导过程见附录C。

---

2) 全局参考坐标系初始化：可靠地初始化全局参考坐标系相对于VI参考坐标系的过程至关重要。理想情况下，应基于接收到的测量数据，定义一个能够反映两个参考坐标系之间4自由度外部变换可观测性的、考虑不确定性的判定准则。首先，可以利用[^88]中介绍的基于SVD的对齐方法，通过全球测量数据与世界参考坐标系中的位姿对应关系来求得初始解。

随后，便可评估该初始解的可靠性。针对接收到的$N_{\mathrm{g}}$组测量数据，可根据公式(26)计算出全局位置误差$\mathbf{e}_{\mathrm{g}}^{j}(j=1,\ldots,N_{\mathrm{g}})$以及对应的误差雅可比矩阵$\mathbf{E}_{\mathrm{g}}^{j}$。4自由度变换的协方差矩阵$\mathbf{P}$可表示为近似海森矩阵$\mathbf{H}$的逆：

 $$ \mathbf{H}=\sum_{j=1}^{N_{\mathbf{g}}}\left(\mathbf{E}_{\mathbf{g}}^{j^{T}}\mathbf{W}_{\mathbf{g}}^{j}\mathbf{E}_{\mathbf{g}}^{j}\right), $$

where $\mathbf{W}_g^j$ is the inverse of the overall covariance matrix $\mathbf{\Sigma}_g^j$ for GNSS residuals, which considers GNSS measurement covariances as well as IMU preintegration covariances (see Appendix B for more details). We examine the variance $p_{\theta\theta}$ corresponding to the yaw angle $\theta$ in $\mathbf{P}$, and define a decision threshold $\sigma_\theta^2$ on whether the global reference frame is observable as $p_{\theta\theta} < \sigma_\theta^2$. When the yaw uncertainty falls below this threshold, $\mathbf{T}_{GW}$ can be assumed to be known and fixed.

3) GNSS对齐：这种初始化并固定GNSS外部参数的机制，能够实现全局对齐策略，从而补偿因GNSS信号中断而累积的漂移。OKVIS2-X通过固定历史较久的状态来降低优化问题的计算复杂度。每当有GNSS残差被加入图优化问题时，我们便会检查最后一个关联有全局位置因子的状态的固定状态。若图中最后一个包含GNSS因子的状态已被冻结，则说明此时已出现长时间的GNSS信号接收中断；自该状态之后，轨迹可能已累积了显著的漂移。消除这种漂移可采用类似闭环检测的全局对齐方法。检测到信号中断后，便按照前一段所述流程重新初始化GNSS外部参数。通过这一初始化过程，可得到外部变换的新估计值$T_{GW_{new}}$。原本估计的$T_{GW}$与新初始化的$T_{GW_{new}}$之间的差值可计算如下：

   $$ \begin{array}{r}{\boldsymbol{T}_{W_{\mathrm{n e w}}W}=\boldsymbol{T}_{G W_{\mathrm{n e w}}}^{-1}\boldsymbol{T}_{G W},}\end{array} $$

该差值同时也能作为轨迹漂移的估计值。利用这一增量变换与旋转平均算法，可将位置与姿态误差分配到GNSS中断期间的所有状态上。完成对齐后，便会触发对整个图的优化处理。

<div style="text-align: center;">TABLE I</div>

<div style="text-align: center;">METHOD DEFINITION BASED ON A SENSOR CONFIGURATIONS AND ESTIMATOR CAUSALITY</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td colspan="5">Sensors</td><td colspan="3">Causality</td></tr><tr><td rowspan="2">Methods</td><td rowspan="2">Visual</td><td rowspan="2">Inertial</td><td rowspan="2">Depth</td><td rowspan="2">LiDAR</td><td rowspan="2">GNSS</td><td rowspan="2">Causal</td><td rowspan="2">Non-causal</td></tr><tr></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-c</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-nc</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>✓</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-ba</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>(✓)</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-v- $^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vig- $^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vid- $^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vidg- $^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vil- $^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vilg- $^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td><td style='text-align: center; word-wrap: break-word;'>$^{{*}}$</td></tr></table>

### H. 因子图优化

上述所有因子均会被整合进整体最小化目标函数中：

 $$ \begin{aligned}c\left(\mathbf{x}\right)&=\frac{1}{2}\sum_{i}\sum_{k\in\mathcal{K}}\sum_{j\in\mathcal{J}(i,k)}\rho_{c}\left(\mathbf{e}_{\mathrm{r}}^{i,j,k}\mathbf{^{T}}\mathbf{W_{r}}\mathbf{e}_{\mathrm{r}}^{i,j,k}\right)\\&+\frac{1}{2}\sum_{k\in\mathcal{P}\cup\mathcal{K}\backslash f}\mathbf{e}_{\mathrm{s}}^{^{T}}\mathbf{W_{s}}^{k}\mathbf{e}_{\mathrm{s}}^{k}+\frac{1}{2}\sum_{r\in\mathcal{P}}\sum_{c\in\mathcal{C}(r)}\mathbf{e}_{\mathrm{p}}^{^{r,c}T}\mathbf{W_{r}}^{r,c}\mathbf{e}_{\mathrm{p}}^{^{r,c}}\\&+\frac{1}{2}\sum_{k\in\mathcal{K}}\sum_{\mathbf{p}\in\mathcal{L}_{k}}\rho_{t}\left(e_{\mathrm{m}}^{C,k^{2}}\right)+\frac{1}{2}\sum_{b\in\mathcal{M}}\sum_{a\in\mathcal{A}_{b}}\sum_{\mathbf{p}\in\mathcal{L}_{b}}\rho_{t}\left(e_{\mathrm{m}}^{a,b^{2}}\right)\\&+\frac{1}{2}\sum_{j\in\mathcal{G}}\mathbf{e}_{\mathrm{g}}^{^{T}}\mathbf{W}_{\mathrm{g}}^{j}\mathbf{e}_{\mathrm{g}}^{j}.\tag{29}\end{aligned} $$

此处，集合$\mathcal{K}$包含了最新的帧以及在$\mathcal{J}(i,k)$中观测到可见地标的关键帧。$\mathcal{P}$包含所有位姿图帧，f表示当前最新帧。$\mathcal{C}(r) \subset \mathcal{P}$是与帧r相连的所有位姿图帧的集合。此外，集合$\mathcal{L}_k$表示与帧k相关的所有地图对齐残差集合。$\mathcal{M}$是所有过往子图的集合，而$\mathcal{A}_b$是通过地图间残差与子图帧$T_{WS_b}$相连的所有子图帧的集合。C表示最后一个已完成的子图帧。$\mathcal{G}$是新增的GNSS残差集合。Cauchy稳健器$\rho_c(\cdot)$和Tukey稳健器$\rho_t(\cdot)$分别用于处理重投影误差和地图对齐误差。

### 六、评估

本次评估旨在量化OKVIS2-X在小型到大型场景中与现有先进方法相比的轨迹精度、建图精度和完整性。大型场景（行驶距离达数公里）具有极大的挑战性，因为长时间运行会产生巨大漂移，且密集建图会占用大量内存以覆盖广阔区域。为了展示OKVIS2-X在此类环境下的性能，我们选取了三个公开数据集：EuRoC [^89]、HiltOxford [^90] 和 VBR [^1]。这些数据集中的行驶距离从无人机飞行数十米（EuRoC），到手持传感器设备数百米（HiltOxford），再到汽车行驶9公里（VBR）不等。至于所提出的GNSS融合技术，

---

<div style="text-align: center;">TABLE II</div>

<div style="text-align: center;">ROOT MEAN SQUARE OF ABSOLUTE TRAJECTORY ERROR [METER] IN THE EUROC DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td rowspan="2" colspan="2"></td><td rowspan="2">Methods</td><td colspan="11">Sequence</td><td rowspan="2">Avg</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>MH01</td><td style='text-align: center; word-wrap: break-word;'>MH02</td><td style='text-align: center; word-wrap: break-word;'>MH03</td><td style='text-align: center; word-wrap: break-word;'>MH04</td><td style='text-align: center; word-wrap: break-word;'>MH05</td><td style='text-align: center; word-wrap: break-word;'>V101</td><td style='text-align: center; word-wrap: break-word;'>V102</td><td style='text-align: center; word-wrap: break-word;'>V103</td><td style='text-align: center; word-wrap: break-word;'>V201</td><td style='text-align: center; word-wrap: break-word;'>V202</td><td style='text-align: center; word-wrap: break-word;'>V203</td></tr><tr><td rowspan="4">V-SLAM</td><td style='text-align: center; word-wrap: break-word;'>C</td><td style='text-align: center; word-wrap: break-word;'>Ours-v-c</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.038</td><td style='text-align: center; word-wrap: break-word;'>0.048</td><td style='text-align: center; word-wrap: break-word;'>0.122</td><td style='text-align: center; word-wrap: break-word;'>0.123</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.054</td><td style='text-align: center; word-wrap: break-word;'>0.123</td><td style='text-align: center; word-wrap: break-word;'>0.072</td><td style='text-align: center; word-wrap: break-word;'>0.095</td><td style='text-align: center; word-wrap: break-word;'>$1.133^{{4}}$</td><td style='text-align: center; word-wrap: break-word;'>0.075</td></tr><tr><td rowspan="3">NC</td><td style='text-align: center; word-wrap: break-word;'>ORB-SLAM3 [^3] $^{{1}}$</td><td style='text-align: center; word-wrap: break-word;'>0.029</td><td style='text-align: center; word-wrap: break-word;'>0.019</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.085</td><td style='text-align: center; word-wrap: break-word;'>0.052</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.025</td><td style='text-align: center; word-wrap: break-word;'>0.061</td><td style='text-align: center; word-wrap: break-word;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>0.028</td><td style='text-align: center; word-wrap: break-word;'>$0.521^{{2}}$</td><td style='text-align: center; word-wrap: break-word;'>0.040</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-v-nc</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.022</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.076</td><td style='text-align: center; word-wrap: break-word;'>0.087</td><td style='text-align: center; word-wrap: break-word;'>0.036</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.085</td><td style='text-align: center; word-wrap: break-word;'>0.027</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>$1.125^{{2}}$</td><td style='text-align: center; word-wrap: break-word;'>0.045</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-v-ba</td><td style='text-align: center; word-wrap: break-word;'>0.016</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.029</td><td style='text-align: center; word-wrap: break-word;'>0.074</td><td style='text-align: center; word-wrap: break-word;'>0.090</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.060</td><td style='text-align: center; word-wrap: break-word;'>0.025</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>$1.068^{{2}}$</td><td style='text-align: center; word-wrap: break-word;'>0.040</td></tr><tr><td rowspan="3">VIO</td><td rowspan="3">Causal (C)</td><td style='text-align: center; word-wrap: break-word;'>OpenVINS [^5] $^{{1}}$</td><td style='text-align: center; word-wrap: break-word;'>0.183</td><td style='text-align: center; word-wrap: break-word;'>0.129</td><td style='text-align: center; word-wrap: break-word;'>0.170</td><td style='text-align: center; word-wrap: break-word;'>0.172</td><td style='text-align: center; word-wrap: break-word;'>0.212</td><td style='text-align: center; word-wrap: break-word;'>0.055</td><td style='text-align: center; word-wrap: break-word;'>0.044</td><td style='text-align: center; word-wrap: break-word;'>0.069</td><td style='text-align: center; word-wrap: break-word;'>0.058</td><td style='text-align: center; word-wrap: break-word;'>0.045</td><td style='text-align: center; word-wrap: break-word;'>0.147</td><td style='text-align: center; word-wrap: break-word;'>0.117</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Kimera2 [^4] $^{{1}}$</td><td style='text-align: center; word-wrap: break-word;'>0.100</td><td style='text-align: center; word-wrap: break-word;'>0.100</td><td style='text-align: center; word-wrap: break-word;'>0.160</td><td style='text-align: center; word-wrap: break-word;'>0.210</td><td style='text-align: center; word-wrap: break-word;'>0.150</td><td style='text-align: center; word-wrap: break-word;'>0.050</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.100</td><td style='text-align: center; word-wrap: break-word;'>0.060</td><td style='text-align: center; word-wrap: break-word;'>0.070</td><td style='text-align: center; word-wrap: break-word;'>0.190</td><td style='text-align: center; word-wrap: break-word;'>0.112</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-c</td><td style='text-align: center; word-wrap: break-word;'>0.050</td><td style='text-align: center; word-wrap: break-word;'>0.048</td><td style='text-align: center; word-wrap: break-word;'>0.085</td><td style='text-align: center; word-wrap: break-word;'>0.144</td><td style='text-align: center; word-wrap: break-word;'>0.125</td><td style='text-align: center; word-wrap: break-word;'>0.045</td><td style='text-align: center; word-wrap: break-word;'>0.046</td><td style='text-align: center; word-wrap: break-word;'>0.046</td><td style='text-align: center; word-wrap: break-word;'>0.033</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>0.071</td><td style='text-align: center; word-wrap: break-word;'>0.066</td></tr><tr><td rowspan="8">VI-SLAM</td><td rowspan="2">Causal</td><td style='text-align: center; word-wrap: break-word;'>Ours-vi-c</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.051</td><td style='text-align: center; word-wrap: break-word;'>0.102</td><td style='text-align: center; word-wrap: break-word;'>0.098</td><td style='text-align: center; word-wrap: break-word;'>0.036</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>0.033</td><td style='text-align: center; word-wrap: break-word;'>0.025</td><td style='text-align: center; word-wrap: break-word;'>0.051</td><td style='text-align: center; word-wrap: break-word;'>0.048</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vid-c</td><td style='text-align: center; word-wrap: break-word;'>0.036</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.053</td><td style='text-align: center; word-wrap: break-word;'>0.093</td><td style='text-align: center; word-wrap: break-word;'>0.086</td><td style='text-align: center; word-wrap: break-word;'>0.037</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>0.028</td><td style='text-align: center; word-wrap: break-word;'>0.028</td><td style='text-align: center; word-wrap: break-word;'>0.051</td><td style='text-align: center; word-wrap: break-word;'>0.046</td></tr><tr><td rowspan="6">Non-causal (NC)</td><td style='text-align: center; word-wrap: break-word;'>ORB-SLAM3 [^3] $^{{1}}$</td><td style='text-align: center; word-wrap: break-word;'>0.036</td><td style='text-align: center; word-wrap: break-word;'>0.033</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.051</td><td style='text-align: center; word-wrap: break-word;'>0.082</td><td style='text-align: center; word-wrap: break-word;'>0.038</td><td style='text-align: center; word-wrap: break-word;'>0.014</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.014</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.035</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>MAVIS-SLAM [^20] $^{{1}}$</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.025</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.053</td><td style='text-align: center; word-wrap: break-word;'>0.075</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.016</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>0.034</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-nc</td><td style='text-align: center; word-wrap: break-word;'>0.023</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>0.067</td><td style='text-align: center; word-wrap: break-word;'>0.056</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.014</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.019</td><td style='text-align: center; word-wrap: break-word;'>0.017</td><td style='text-align: center; word-wrap: break-word;'>0.022</td><td style='text-align: center; word-wrap: break-word;'>0.030</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vid-nc</td><td style='text-align: center; word-wrap: break-word;'>0.022</td><td style='text-align: center; word-wrap: break-word;'>0.023</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>0.068</td><td style='text-align: center; word-wrap: break-word;'>0.058</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.014</td><td style='text-align: center; word-wrap: break-word;'>0.019</td><td style='text-align: center; word-wrap: break-word;'>0.018</td><td style='text-align: center; word-wrap: break-word;'>0.016</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.030</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-ba</td><td style='text-align: center; word-wrap: break-word;'>0.017</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.029</td><td style='text-align: center; word-wrap: break-word;'>0.057</td><td style='text-align: center; word-wrap: break-word;'>0.055</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.013</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.023</td><td style='text-align: center; word-wrap: break-word;'>0.013</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.028</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vid-ba</td><td style='text-align: center; word-wrap: break-word;'>0.016</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.029</td><td style='text-align: center; word-wrap: break-word;'>0.062</td><td style='text-align: center; word-wrap: break-word;'>0.053</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.013</td><td style='text-align: center; word-wrap: break-word;'>0.019</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.012</td><td style='text-align: center; word-wrap: break-word;'>0.022</td><td style='text-align: center; word-wrap: break-word;'>0.028</td></tr></table>

我们的实验结果均为10次运行的中位数。

$^{1}$ 结果取自相应论文，OpenVINS的结果来自 [^20]。

$^{2}$ 我们将该序列视为失败的序列，因此不将其纳入平均值计算。

我们进一步在GVINS数据集 [^50] 中添加了一个具有挑战性的序列进行评估，该序列包含室内外过渡和杂乱环境。所有评估均以串行方式执行，以确保不会出现帧丢失情况。如果已知相机与IMU之间的时间偏移（例如在Hilti-Oxford数据集中），我们会将其考虑在内；同时也会考虑激光雷达和GNSS相对于IMU的时间偏移。

### A. 评估指标

我们评估了方法的多种变体，以清晰展示不同传感器配置及估计器因果性对多模态效果的影响。一方面，在表I中，“视觉”项包含重投影残差(11)和相对姿态图残差(13)；“惯性”项指预积分残差(12)；“深度网络”项基于用于子图对齐残差(23)的深度网络融合；“激光雷达”项利用激光雷达点云计算子图对齐残差(23)；而“GNSS”项则包含GNSS残差(26)。另一方面，“因果”模式仅使用截至当前时间的测量数据，“非因果”模式对应最终的闭环轨迹，而“全BA”模式则通过将相对姿态图边转换为重投影边，对整个状态的非因果轨迹进行优化。例如，Ours-vi-c表示采用因果评估的视觉-惯性配置。

为了量化估计姿态的精确度，在评估前需将估计轨迹在 $SE(3)$ 中与真实轨迹对齐。我们在EuRoC和VBR数据集中以绝对轨迹误差（ATE）的均方根误差（RMSE）来报告轨迹精度。在Hilti-Oxford数据集中，我们采用预定义的评分标准：位置误差 [^1] [^10] 厘米会被换算为 [^0] [^100] 个点。为了评估建图精度，我们先利用Marching Cubes算法 [^91] 重建子图网格，再根据子图姿态将它们合并。估计顶点和真实顶点会按照 $1\text{cm}^3$ 的体素大小进行下采样，随后对两组顶点执行点对面ICP对齐操作。我们将精度定义为所有估计顶点到真实顶点之间0.2米范围内的平均距离。完整性则定义为真实顶点中位于估计顶点0.2米范围内的比例。

### B. EuRoC数据集

该数据集提供了由无人机拍摄的立体图像以及IMU测量数据，还有Vicon房间内的真实轨迹及毫米级精度的点云 [^89]。我们选用立体图像对用于立体网络训练，左视图则用于MVS网络训练。表二汇总了ATE数值，所有竞争对手均采用立体或立体惯性配置。我们还实现了V-SLAM算法Ours-v，以展示OKVIS2-X的多功能性——该算法用恒定速度模型替换了IMU预积分项。与ORB-SLAM3相比，OKVIS2-X在轨迹精度方面表现颇具竞争力。值得注意的是，由于运动模糊、图像缺失以及曝光变化显著，V203序列对V-SLAM而言极具挑战性。正因如此，OKVIS2-X和ORB-SLAM3在该特定序列上的轨迹误差较大。

不过，在立体惯性配置下我们解决了这一大误差问题。为了与里程计方法OpenVINS [^5] 和Kimera2 [^4] 进行公平比较，我们对比了不含回环闭合功能的Ours-vi的VIO实现效果。与竞争对手相比，我们的方法将定位误差降低了41%。在包含回环闭合功能的SLAM实现中，Ours-vi在非因果评估中超越了现有最先进的方法。值得注意的是，加入回环闭合功能

---

<div style="text-align: center;">TABLE III</div>

<div style="text-align: center;">MESH ACCURACY [M] AND COMPLETENESS [%] WITH 0.2M THRESHOLD IN THE EUROC DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td colspan="4">Accuracy</td><td colspan="4">Completeness</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V101</td><td style='text-align: center; word-wrap: break-word;'>0.071</td><td style='text-align: center; word-wrap: break-word;'>0.050</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>40.58</td><td style='text-align: center; word-wrap: break-word;'>43.98</td><td style='text-align: center; word-wrap: break-word;'>42.70</td><td style='text-align: center; word-wrap: break-word;'>44.77</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V102</td><td style='text-align: center; word-wrap: break-word;'>0.077</td><td style='text-align: center; word-wrap: break-word;'>0.043</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>55.07</td><td style='text-align: center; word-wrap: break-word;'>56.45</td><td style='text-align: center; word-wrap: break-word;'>56.17</td><td style='text-align: center; word-wrap: break-word;'>58.38</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V103</td><td style='text-align: center; word-wrap: break-word;'>0.086</td><td style='text-align: center; word-wrap: break-word;'>0.062</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>47.74</td><td style='text-align: center; word-wrap: break-word;'>61.65</td><td style='text-align: center; word-wrap: break-word;'>64.59</td><td style='text-align: center; word-wrap: break-word;'>67.77</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V201</td><td style='text-align: center; word-wrap: break-word;'>0.062</td><td style='text-align: center; word-wrap: break-word;'>0.066</td><td style='text-align: center; word-wrap: break-word;'>0.068</td><td style='text-align: center; word-wrap: break-word;'>0.047</td><td style='text-align: center; word-wrap: break-word;'>41.93</td><td style='text-align: center; word-wrap: break-word;'>40.51</td><td style='text-align: center; word-wrap: break-word;'>38.53</td><td style='text-align: center; word-wrap: break-word;'>43.82</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V202</td><td style='text-align: center; word-wrap: break-word;'>0.065</td><td style='text-align: center; word-wrap: break-word;'>0.047</td><td style='text-align: center; word-wrap: break-word;'>0.064</td><td style='text-align: center; word-wrap: break-word;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>56.37</td><td style='text-align: center; word-wrap: break-word;'>58.16</td><td style='text-align: center; word-wrap: break-word;'>48.90</td><td style='text-align: center; word-wrap: break-word;'>60.01</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V203</td><td style='text-align: center; word-wrap: break-word;'>0.068</td><td style='text-align: center; word-wrap: break-word;'>0.062</td><td style='text-align: center; word-wrap: break-word;'>0.068</td><td style='text-align: center; word-wrap: break-word;'>0.044</td><td style='text-align: center; word-wrap: break-word;'>62.58</td><td style='text-align: center; word-wrap: break-word;'>60.20</td><td style='text-align: center; word-wrap: break-word;'>52.72</td><td style='text-align: center; word-wrap: break-word;'>61.76</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Avg</td><td style='text-align: center; word-wrap: break-word;'>0.072</td><td style='text-align: center; word-wrap: break-word;'>0.055</td><td style='text-align: center; word-wrap: break-word;'>0.052</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>50.71</td><td style='text-align: center; word-wrap: break-word;'>53.49</td><td style='text-align: center; word-wrap: break-word;'>50.60</td><td style='text-align: center; word-wrap: break-word;'>56.09</td></tr></table>

 $^{1}$ 这是我们使用相同指标自行得出的结果。

![Image](images/2aeb52c8957fe851ab8c1d790d473cd6cf256fc2464fadf82ecab498f6f966c0.jpg)

<div style="text-align: center;">Fig. 7. Reconstructed mesh with mesh-to-point error encoding in V102 sequence. (Left) Ours-vid and (Right) Simplemapping [^58].</div>

能够生成更精确的实时轨迹，与我们的VIO变体相比，平均误差减少了1.8厘米。此外，结合了深度网络的Ours-vid方法在因果评估中通过加入子图对齐因子，进一步提升了基线方法Ours-vi的性能。在非因果评估中，我们获得的平均精度与基线方法相当。这表明深度融合技术尚未能达到3厘米级的精度（占轨迹长度的0.04%），不过这一精度已经相当可观了。

由于我们的方法将定位与体积建图紧密耦合，我们也在表III中评估了建图精度。为了过滤掉噪声表面，我们只对梯度大于某一阈值且相邻面至少被观测三次的表面进行网格化处理。为了在单目惯性设置下与SimpleMapping [^58]进行公平比较，我们实现了Ours-vid（Mono）版本，该版本仅使用左摄像头图像作为估计器和MVS网络的输入。此外，我们还提出了Ours-vid（Stereo）版本，该版本仅将来自立体相机的深度数据用于建图。Ours-vid在精度和完整性方面均表现最佳，而Ours-vid（Mono）也优于竞争对手。从表III中可以明显看出，时序图像与立体图像的深度融合提升了建图质量。图7对比了各方法的精度，结果显示Ours-vid通过妥善处理不确定性，显著提升了房间边缘处的网格精度。

### C. Hilti-Oxford数据集

该数据集提供了来自5个摄像头的图像、IMU测量数据、由手持设备采集的32通道激光雷达点云、稀疏的真实位置信息以及毫米级精度的密集点云 [^90]。我们使用全部5个摄像头数据用于视觉惯性估计器，用2个前置摄像头数据用于立体网络，而前置左侧摄像头数据则用于MVS网络。我们在线校准摄像头与IMU之间的外部参数，关于在线校准的详细内容将在第VI-F节中介绍。

表IV总结了挑战序列中的定位得分。考虑到排行榜的特性，我们将我们方法在3次运行中的最佳得分与竞争对手进行比较。为了反映不同运行间的偏差，我们还在括号中列出了中位数得分。在视觉惯性配置下，经过最终束调整的非因果评估显著提升了位置精度，其表现也优于排行榜上已发表的竞争对手。通过引入子图对齐产生的额外残差，Ours-vid-ba进一步提高了精度。$\exp03$场景由于光线较暗，导致前置摄像头偶尔会拍到全黑图像，如图8所示。由于我们仅使用前置摄像头进行密集建图，Ours-vid-ba在控制点处的位置误差达到了1.2米，这一数值比其他序列要高。不过，排除这个具有挑战性的序列$\exp03$后，Ours-vid-ba的平均位置误差仅为7.0厘米。

我们的视觉-惯性-激光雷达配置ours-vil-★在几乎所有序列中，无论是因果估计还是精细化估计，都显示出显著的提升，尤其是在视觉条件较差的exp03序列中（见图8），系统的鲁棒性也得到了增强。就实际定位精度而言，该方法将平均位置误差降低到了4.1厘米（排除精度约为11厘米的exp07序列后，误差为2.8厘米），这一数值与建图分辨率相当。我们还将该系统与已发表排名靠前的竞争对手进行了比较。在已发表的视觉-惯性-激光雷达系统中，我们的系统排名首位，平均性能优于FAST-LIVO2 [^9]和VILENS [^11]，仅逊于纯激光雷达-惯性SLAM系统Wildcat [^68]。我们的主要瓶颈在于……

![Image](images/24f64d0948f0100d06289e43144e3b9552aec2b3c15ccd8570942ec358ae5b55.jpg)

<div style="text-align: center;">Fig. 8. Failure case in exp03 in the Hilt-Oxford dataset where the LiDAR compensates the vision failure in a dark room.</div>

---

<div style="text-align: center;">TABLE IV</div>

<div style="text-align: center;">LOCALIZATION SCORES IN THE HILTI-OXFORD DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td rowspan="2" colspan="2"></td><td rowspan="2">Methods</td><td colspan="8">Sequence</td><td rowspan="2">Total</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp01</td><td style='text-align: center; word-wrap: break-word;'>exp02</td><td style='text-align: center; word-wrap: break-word;'>exp03</td><td style='text-align: center; word-wrap: break-word;'>exp07</td><td style='text-align: center; word-wrap: break-word;'>exp09</td><td style='text-align: center; word-wrap: break-word;'>exp11</td><td style='text-align: center; word-wrap: break-word;'>exp15</td><td style='text-align: center; word-wrap: break-word;'>exp21</td></tr><tr><td rowspan="8">Visual-inertial</td><td rowspan="3">Causal</td><td style='text-align: center; word-wrap: break-word;'>OpenVINS [^5] $^{1}$</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>1.67</td><td style='text-align: center; word-wrap: break-word;'>8.75</td><td style='text-align: center; word-wrap: break-word;'>8.00</td><td style='text-align: center; word-wrap: break-word;'>4.44</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>22.86</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-c</td><td style='text-align: center; word-wrap: break-word;'>10.77 (7.69)</td><td style='text-align: center; word-wrap: break-word;'>8.18 (10.00)</td><td style='text-align: center; word-wrap: break-word;'>0.00 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>0.00 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>11.25 (6.25)</td><td style='text-align: center; word-wrap: break-word;'>10.00 (6.00)</td><td style='text-align: center; word-wrap: break-word;'>10.00 (11.11)</td><td style='text-align: center; word-wrap: break-word;'>0.00 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>50.20 (41.05)</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vid-c</td><td style='text-align: center; word-wrap: break-word;'>56.92 (26.15)</td><td style='text-align: center; word-wrap: break-word;'>18.18 (4.55)</td><td style='text-align: center; word-wrap: break-word;'>0.00 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>0.00 (1.67)</td><td style='text-align: center; word-wrap: break-word;'>4.38 (1.88)</td><td style='text-align: center; word-wrap: break-word;'>4.00 (8.00)</td><td style='text-align: center; word-wrap: break-word;'>7.78 (4.44)</td><td style='text-align: center; word-wrap: break-word;'>0.00 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>91.26 (46.69)</td></tr><tr><td rowspan="5">Non-causal</td><td style='text-align: center; word-wrap: break-word;'>BAMF-SLAM [^92] $^{2}$</td><td style='text-align: center; word-wrap: break-word;'>15.38</td><td style='text-align: center; word-wrap: break-word;'>4.55</td><td style='text-align: center; word-wrap: break-word;'>8.24</td><td style='text-align: center; word-wrap: break-word;'>1.67</td><td style='text-align: center; word-wrap: break-word;'>1.25</td><td style='text-align: center; word-wrap: break-word;'>2.00</td><td style='text-align: center; word-wrap: break-word;'>7.78</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>40.86</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>ORB-SLAM3 [^3] $^{1}$</td><td style='text-align: center; word-wrap: break-word;'>3.85</td><td style='text-align: center; word-wrap: break-word;'>1.82</td><td style='text-align: center; word-wrap: break-word;'>10.59</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>6.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>22.25</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>MAVIS SLAM [^20] $^{2}$</td><td style='text-align: center; word-wrap: break-word;'>40.00</td><td style='text-align: center; word-wrap: break-word;'>16.82</td><td style='text-align: center; word-wrap: break-word;'>16.47</td><td style='text-align: center; word-wrap: break-word;'>11.67</td><td style='text-align: center; word-wrap: break-word;'>15.00</td><td style='text-align: center; word-wrap: break-word;'>20.00</td><td style='text-align: center; word-wrap: break-word;'>26.67</td><td style='text-align: center; word-wrap: break-word;'>4.00</td><td style='text-align: center; word-wrap: break-word;'>150.62</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-ba</td><td style='text-align: center; word-wrap: break-word;'>30.00 (36.92)</td><td style='text-align: center; word-wrap: break-word;'>40.91 (30.45)</td><td style='text-align: center; word-wrap: break-word;'>12.35 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>3.33 (1.67)</td><td style='text-align: center; word-wrap: break-word;'>16.88 (17.50)</td><td style='text-align: center; word-wrap: break-word;'>44.00 (28.00)</td><td style='text-align: center; word-wrap: break-word;'>20.00 (20.00)</td><td style='text-align: center; word-wrap: break-word;'>0.00 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>167.47 (135.54)</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vid-ba</td><td style='text-align: center; word-wrap: break-word;'>53.08 (30.00)</td><td style='text-align: center; word-wrap: break-word;'>34.55 (37.27)</td><td style='text-align: center; word-wrap: break-word;'>0.00 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>13.13 (8.33)</td><td style='text-align: center; word-wrap: break-word;'>23.75 (23.75)</td><td style='text-align: center; word-wrap: break-word;'>22.00 (22.00)</td><td style='text-align: center; word-wrap: break-word;'>28.89 (26.67)</td><td style='text-align: center; word-wrap: break-word;'>2.00 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>177.59 (148.02)</td></tr><tr><td rowspan="5">(V)LiDAR</td><td rowspan="2">Causal</td><td style='text-align: center; word-wrap: break-word;'>FAST-LIVO2 [^9] $^{2}$</td><td style='text-align: center; word-wrap: break-word;'>84.62</td><td style='text-align: center; word-wrap: break-word;'>70.00</td><td style='text-align: center; word-wrap: break-word;'>44.71</td><td style='text-align: center; word-wrap: break-word;'>31.67</td><td style='text-align: center; word-wrap: break-word;'>7.50</td><td style='text-align: center; word-wrap: break-word;'>92.00</td><td style='text-align: center; word-wrap: break-word;'>24.44</td><td style='text-align: center; word-wrap: break-word;'>48.00</td><td style='text-align: center; word-wrap: break-word;'>402.93</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vil-c</td><td style='text-align: center; word-wrap: break-word;'>63.08 (50.77)</td><td style='text-align: center; word-wrap: break-word;'>33.64 (37.27)</td><td style='text-align: center; word-wrap: break-word;'>1.18 (7.06)</td><td style='text-align: center; word-wrap: break-word;'>0.00 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>11.88 (7.50)</td><td style='text-align: center; word-wrap: break-word;'>68.00 (62.00)</td><td style='text-align: center; word-wrap: break-word;'>30.00 (34.44)</td><td style='text-align: center; word-wrap: break-word;'>38.00 (34.00)</td><td style='text-align: center; word-wrap: break-word;'>245.76 (233.05)</td></tr><tr><td rowspan="3">Non-causal</td><td style='text-align: center; word-wrap: break-word;'>VILENS [^11] $^{2}$</td><td style='text-align: center; word-wrap: break-word;'>69.23</td><td style='text-align: center; word-wrap: break-word;'>49.09</td><td style='text-align: center; word-wrap: break-word;'>40.00</td><td style='text-align: center; word-wrap: break-word;'>16.67</td><td style='text-align: center; word-wrap: break-word;'>5.00</td><td style='text-align: center; word-wrap: break-word;'>68.00</td><td style='text-align: center; word-wrap: break-word;'>17.78</td><td style='text-align: center; word-wrap: break-word;'>60.00</td><td style='text-align: center; word-wrap: break-word;'>325.77</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vil-ba</td><td style='text-align: center; word-wrap: break-word;'>63.85 (72.31)</td><td style='text-align: center; word-wrap: break-word;'>51.82 (51.36)</td><td style='text-align: center; word-wrap: break-word;'>49.41 (51.18)</td><td style='text-align: center; word-wrap: break-word;'>1.67 (0.00)</td><td style='text-align: center; word-wrap: break-word;'>26.25 (24.38)</td><td style='text-align: center; word-wrap: break-word;'>100.00 (84.00)</td><td style='text-align: center; word-wrap: break-word;'>61.11 (61.11)</td><td style='text-align: center; word-wrap: break-word;'>84.00 (68.00)</td><td style='text-align: center; word-wrap: break-word;'>438.10 (412.33)</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Wildcat [^68] $^{2,3}$</td><td style='text-align: center; word-wrap: break-word;'>84.62</td><td style='text-align: center; word-wrap: break-word;'>84.55</td><td style='text-align: center; word-wrap: break-word;'>90.59</td><td style='text-align: center; word-wrap: break-word;'>45.00</td><td style='text-align: center; word-wrap: break-word;'>44.38</td><td style='text-align: center; word-wrap: break-word;'>84.00</td><td style='text-align: center; word-wrap: break-word;'>46.67</td><td style='text-align: center; word-wrap: break-word;'>84.00</td><td style='text-align: center; word-wrap: break-word;'>563.79</td></tr></table>

所有Ours结果均为基于总得分的3次运行中的最佳（中位数）得分。

鉴于挑战赛的性质，每种传感器配置下的最佳（加粗）和第二佳（下划线）得分会被突出显示，且不区分因果或非因果方法。

 $^{1}$ 结果为我们基于开源代码在立体惯性配置下进行的3次运行中所获得的最佳得分。

 $^{2}$ 结果取自排行榜 [^93]。

 $^{3}$ Wildcat仅使用激光雷达-惯性传感器。

<div style="text-align: center;">TABLE V</div>

<div style="text-align: center;">MESH ACCURACY [M] AND COMPLETENESS [%] WITH 0.2M THRESHOLD IN THE HILTI DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>Ours-vid</td><td style='text-align: center; word-wrap: break-word;'>Ours-vil</td><td style='text-align: center; word-wrap: break-word;'>Ours-vid</td><td style='text-align: center; word-wrap: break-word;'>Ours-vil</td></tr><tr><td style='text-align: center; word-wrap: break-word;'></td><td colspan="2">Accuracy</td><td colspan="2">Completeness</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp04</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>59.15</td><td style='text-align: center; word-wrap: break-word;'>77.90</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp05</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>64.04</td><td style='text-align: center; word-wrap: break-word;'>76.94</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp06</td><td style='text-align: center; word-wrap: break-word;'>0.046</td><td style='text-align: center; word-wrap: break-word;'>0.027</td><td style='text-align: center; word-wrap: break-word;'>69.63</td><td style='text-align: center; word-wrap: break-word;'>80.15</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Avg</td><td style='text-align: center; word-wrap: break-word;'>0.042</td><td style='text-align: center; word-wrap: break-word;'>0.029</td><td style='text-align: center; word-wrap: break-word;'>64.27</td><td style='text-align: center; word-wrap: break-word;'>78.33</td></tr></table>

该系统位于exp07区域，这是一条长长的走廊，由于运动方向上缺乏特征性几何结构，对激光雷达而言属于退化场景。

在制图方面，表V报告了在exp04、exp05、exp06中网络深度与激光雷达结合后的精度与完整性，这些场景均有真实点云数据可供参考。通过深度融合技术，我们获得了相当精确的制图结果（误差为4.2厘米），该技术仅在合成数据集上进行了微调 [^83]。我们通过用激光雷达替换深度网络，进一步提升了精度与完整性。深度融合技术中可整合测量数据的最大有效深度设为3米，激光雷达则设为30米。图9展示了exp06中的重建网格，其中由我们的深度网络捕捉到的几何结构清晰可见，与激光雷达融合后完整性进一步提升。

### D. VBR：罗马的视觉基准测试

VBR数据集 [^1] 配备了丰富的传感器套件，使我们能够评估不同配置下的OKVIS2-X。我们认为VBR是一个有趣的数据集，因为它包含长序列，能够验证OKVIS2-X的可扩展性。所记录的序列来自真实世界环境，带来了额外的挑战，例如动态物体、光照条件不稳定、结构化与非结构化环境等。该数据集包含双目相机图像、IMU测量数据、激光雷达点云以及用于定量评估的真实位姿数据。我们发现，由于深度平面参数化并非基于倒数的深度，MVS网络在开阔天空场景下会输出有噪声的深度值。在VBR数据集中，我们仅采用双目网络。为了进一步提升双目视差精度，我们在两级图像金字塔中对我们的不确定性增强型网络进行了微调，随后还进行了额外的优化步骤 [^79]。在该数据集中，我们评估了OKVIS2-X在所有多传感器配置下的ATE，并将其与最先进的系统ORB-SLAM3 [^3]、OpenVINS [^5] 以及FAST-LIVO [^40] 进行了比较。为确保FAST-LIVO能够处理所有测量数据，rosbags以0.2的实时速率播放。

表VI展示了VBR数据集中各SLAM系统的轨迹精度，所有系统均采用相同的标定参数进行测试。表中给出了三次评估运行的中位数轨迹误差。由于VBR中的序列非常长，最长可达9公里，我们根据传感器配置和序列规模，将子图分辨率设为10厘米或20厘米。传感器的远平面……

---

<div style="text-align: center;">TABLE VI</div>

<div style="text-align: center;">ROOT MEAN SQUARE OF ABSOLUTE TRAJECTORY ERROR [METERS] IN THE VBR DATASET.</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td rowspan="2" colspan="2"></td><td rowspan="2">Methods</td><td colspan="8">Sequence</td><td rowspan="2">Avg</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Colosseum</td><td style='text-align: center; word-wrap: break-word;'>Diag</td><td style='text-align: center; word-wrap: break-word;'>Pincio</td><td style='text-align: center; word-wrap: break-word;'>Spagna</td><td style='text-align: center; word-wrap: break-word;'>Campus0</td><td style='text-align: center; word-wrap: break-word;'>Campus1</td><td style='text-align: center; word-wrap: break-word;'>Ciampino0</td><td style='text-align: center; word-wrap: break-word;'>Ciampino1</td></tr><tr><td rowspan="8">Visual-inertial</td><td rowspan="3">Causal</td><td style='text-align: center; word-wrap: break-word;'>OpenVINS [^5]</td><td style='text-align: center; word-wrap: break-word;'>20.827</td><td style='text-align: center; word-wrap: break-word;'>24.628</td><td style='text-align: center; word-wrap: break-word;'>6.928</td><td style='text-align: center; word-wrap: break-word;'>10.023</td><td style='text-align: center; word-wrap: break-word;'>18.343</td><td style='text-align: center; word-wrap: break-word;'>4.719</td><td style='text-align: center; word-wrap: break-word;'>109.777</td><td style='text-align: center; word-wrap: break-word;'>34.921</td><td style='text-align: center; word-wrap: break-word;'>28.770</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-c</td><td style='text-align: center; word-wrap: break-word;'>1.922</td><td style='text-align: center; word-wrap: break-word;'>1.156</td><td style='text-align: center; word-wrap: break-word;'>3.368</td><td style='text-align: center; word-wrap: break-word;'>3.618</td><td style='text-align: center; word-wrap: break-word;'>3.065</td><td style='text-align: center; word-wrap: break-word;'>2.631</td><td style='text-align: center; word-wrap: break-word;'>3.721</td><td style='text-align: center; word-wrap: break-word;'>2.971</td><td style='text-align: center; word-wrap: break-word;'>2.807</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vid-c</td><td style='text-align: center; word-wrap: break-word;'>1.771</td><td style='text-align: center; word-wrap: break-word;'>1.173</td><td style='text-align: center; word-wrap: break-word;'>3.673</td><td style='text-align: center; word-wrap: break-word;'>3.569</td><td style='text-align: center; word-wrap: break-word;'>3.084</td><td style='text-align: center; word-wrap: break-word;'>2.552</td><td style='text-align: center; word-wrap: break-word;'>3.728</td><td style='text-align: center; word-wrap: break-word;'>3.002</td><td style='text-align: center; word-wrap: break-word;'>2.819</td></tr><tr><td rowspan="5">Non-causal (NC)</td><td style='text-align: center; word-wrap: break-word;'>ORB-SLAM3 [^3] $^{1}$</td><td style='text-align: center; word-wrap: break-word;'>7.713</td><td style='text-align: center; word-wrap: break-word;'>1.834</td><td style='text-align: center; word-wrap: break-word;'>1.594</td><td style='text-align: center; word-wrap: break-word;'>3.108</td><td style='text-align: center; word-wrap: break-word;'>$8.023^{{2}}$</td><td style='text-align: center; word-wrap: break-word;'>7.289</td><td style='text-align: center; word-wrap: break-word;'>$-^{{2}}$</td><td style='text-align: center; word-wrap: break-word;'>$9.443^{{2}}$</td><td style='text-align: center; word-wrap: break-word;'>$5.572^{{2}}$</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-nc</td><td style='text-align: center; word-wrap: break-word;'>1.541</td><td style='text-align: center; word-wrap: break-word;'>1.026</td><td style='text-align: center; word-wrap: break-word;'>3.301</td><td style='text-align: center; word-wrap: break-word;'>0.605</td><td style='text-align: center; word-wrap: break-word;'>3.026</td><td style='text-align: center; word-wrap: break-word;'>2.499</td><td style='text-align: center; word-wrap: break-word;'>3.490</td><td style='text-align: center; word-wrap: break-word;'>2.847</td><td style='text-align: center; word-wrap: break-word;'>2.292</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vid-nc</td><td style='text-align: center; word-wrap: break-word;'>1.450</td><td style='text-align: center; word-wrap: break-word;'>0.452</td><td style='text-align: center; word-wrap: break-word;'>3.150</td><td style='text-align: center; word-wrap: break-word;'>0.744</td><td style='text-align: center; word-wrap: break-word;'>3.037</td><td style='text-align: center; word-wrap: break-word;'>2.482</td><td style='text-align: center; word-wrap: break-word;'>3.472</td><td style='text-align: center; word-wrap: break-word;'>2.889</td><td style='text-align: center; word-wrap: break-word;'>2.210</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi-ba</td><td style='text-align: center; word-wrap: break-word;'>1.613</td><td style='text-align: center; word-wrap: break-word;'>0.931</td><td style='text-align: center; word-wrap: break-word;'>3.278</td><td style='text-align: center; word-wrap: break-word;'>0.662</td><td style='text-align: center; word-wrap: break-word;'>3.017</td><td style='text-align: center; word-wrap: break-word;'>2.471</td><td style='text-align: center; word-wrap: break-word;'>3.358</td><td style='text-align: center; word-wrap: break-word;'>2.920</td><td style='text-align: center; word-wrap: break-word;'>2.281</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vid-ba</td><td style='text-align: center; word-wrap: break-word;'>1.441</td><td style='text-align: center; word-wrap: break-word;'>0.346</td><td style='text-align: center; word-wrap: break-word;'>3.199</td><td style='text-align: center; word-wrap: break-word;'>0.807</td><td style='text-align: center; word-wrap: break-word;'>3.016</td><td style='text-align: center; word-wrap: break-word;'>2.451</td><td style='text-align: center; word-wrap: break-word;'>3.391</td><td style='text-align: center; word-wrap: break-word;'>2.995</td><td style='text-align: center; word-wrap: break-word;'>2.206</td></tr><tr><td rowspan="4">VI-LiDAR</td><td rowspan="2">Causal</td><td style='text-align: center; word-wrap: break-word;'>FAST-LIVO [^40] $^{1}$</td><td style='text-align: center; word-wrap: break-word;'>6.459</td><td style='text-align: center; word-wrap: break-word;'>1.013</td><td style='text-align: center; word-wrap: break-word;'>2.042</td><td style='text-align: center; word-wrap: break-word;'>3.963</td><td style='text-align: center; word-wrap: break-word;'>1.259</td><td style='text-align: center; word-wrap: break-word;'>0.434</td><td style='text-align: center; word-wrap: break-word;'>3.080</td><td style='text-align: center; word-wrap: break-word;'>1.548</td><td style='text-align: center; word-wrap: break-word;'>2.475</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vil-c</td><td style='text-align: center; word-wrap: break-word;'>1.517</td><td style='text-align: center; word-wrap: break-word;'>0.900</td><td style='text-align: center; word-wrap: break-word;'>2.861</td><td style='text-align: center; word-wrap: break-word;'>0.691</td><td style='text-align: center; word-wrap: break-word;'>1.955</td><td style='text-align: center; word-wrap: break-word;'>1.634</td><td style='text-align: center; word-wrap: break-word;'>1.716</td><td style='text-align: center; word-wrap: break-word;'>2.892</td><td style='text-align: center; word-wrap: break-word;'>1.771</td></tr><tr><td rowspan="2">NC</td><td style='text-align: center; word-wrap: break-word;'>Ours-vil-nc</td><td style='text-align: center; word-wrap: break-word;'>0.275</td><td style='text-align: center; word-wrap: break-word;'>0.906</td><td style='text-align: center; word-wrap: break-word;'>1.613</td><td style='text-align: center; word-wrap: break-word;'>0.326</td><td style='text-align: center; word-wrap: break-word;'>1.529</td><td style='text-align: center; word-wrap: break-word;'>0.868</td><td style='text-align: center; word-wrap: break-word;'>0.529</td><td style='text-align: center; word-wrap: break-word;'>0.582</td><td style='text-align: center; word-wrap: break-word;'>0.829</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vil-ba</td><td style='text-align: center; word-wrap: break-word;'>0.337</td><td style='text-align: center; word-wrap: break-word;'>0.891</td><td style='text-align: center; word-wrap: break-word;'>1.745</td><td style='text-align: center; word-wrap: break-word;'>0.345</td><td style='text-align: center; word-wrap: break-word;'>1.495</td><td style='text-align: center; word-wrap: break-word;'>0.875</td><td style='text-align: center; word-wrap: break-word;'>0.502</td><td style='text-align: center; word-wrap: break-word;'>0.606</td><td style='text-align: center; word-wrap: break-word;'>0.850</td></tr><tr><td colspan="3">Trajectory length [km]</td><td style='text-align: center; word-wrap: break-word;'>1.451</td><td style='text-align: center; word-wrap: break-word;'>1.037</td><td style='text-align: center; word-wrap: break-word;'>1.277</td><td style='text-align: center; word-wrap: break-word;'>1.562</td><td style='text-align: center; word-wrap: break-word;'>2.735</td><td style='text-align: center; word-wrap: break-word;'>2.925</td><td style='text-align: center; word-wrap: break-word;'>9.008</td><td style='text-align: center; word-wrap: break-word;'>5.200</td><td style='text-align: center; word-wrap: break-word;'>3.149</td></tr></table>

所有方法均报告3次运行的中位数结果。

 $^{1}$ 结果为我们基于开源实现所获得的数据。

 $^{2}$ 系统故障：若有的话，所报告的数字来自少于3次成功运行的结果（最少需运行5次）。

对于手持或驾驶场景，激光雷达的探测范围限制为30米，双目网络的探测范围则为10米或15米。

在视觉-惯性传感器配置下，OKVIS2-X在大多数序列中均展现出卓越的性能，其中利用学习得到的深度信息的版本在非因果评估中最为精确。由于OpenVINS不支持回环检测，其在包含回环的大规模场景中精度自然会有所下降。在视觉-激光雷达传感器配置下，OKVIS2-X在因果评估中的整体性能也优于FAST-LIVO，其平均误差仅为1.771米（占轨迹长度的0.06%）。我们对回环检测的支持进一步显著提升了系统性能。图1和图9展示了全局一致的子图网格示例。

OKVIS2-X的性能明显优于对比系统。此外，即便存在动态物体——尤其是在Colosseum或Spagna场景中，或者数据受损的情况下，OKVIS2-X也展现出了极强的鲁棒性。我们发现Campus*和Ciampino*这两段驾驶序列中存在整段缺失IMU测量的现象；在此类情况下，其他系统的精度会下降甚至发生系统故障。

### E. 对全球位置测量数据的抗丢包融合

为了展示OKVIS2-X针对不同传感器配置的通用性与模块化特性，以及融合全球位置测量数据的能力，我们开展了两项额外研究。第一项研究针对驾驶序列中的VBR序列Campus1。由于该数据集未提供大地测量型GNSS测量数据，我们通过对真实轨迹施加水平方向1厘米、垂直方向2厘米的高斯噪声来模拟RTK-GNSS测量数据。此外，我们还模拟了持续75秒的GNSS信号中断，对应的轨迹长度为450米。

<div style="text-align: center;">TABLE VII</div>

<div style="text-align: center;">DROPOUT-TOLERANT GNSS FUSION: ATE RMSE[M] IN VBR CAMPUS1.</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>Ours-vi</td><td style='text-align: center; word-wrap: break-word;'>Ours-vig</td><td style='text-align: center; word-wrap: break-word;'>Ours-vid</td><td style='text-align: center; word-wrap: break-word;'>Ours-vidg</td><td style='text-align: center; word-wrap: break-word;'>Ours-vil</td><td style='text-align: center; word-wrap: break-word;'>Ours-vilg</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Causal (during Dropout)</td><td style='text-align: center; word-wrap: break-word;'>2.631</td><td style='text-align: center; word-wrap: break-word;'>1.119</td><td style='text-align: center; word-wrap: break-word;'>2.552</td><td style='text-align: center; word-wrap: break-word;'>1.099</td><td style='text-align: center; word-wrap: break-word;'>1.634</td><td style='text-align: center; word-wrap: break-word;'>0.328</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Non-Causal (during Dropout)</td><td style='text-align: center; word-wrap: break-word;'>2.499</td><td style='text-align: center; word-wrap: break-word;'>0.751</td><td style='text-align: center; word-wrap: break-word;'>2.482</td><td style='text-align: center; word-wrap: break-word;'>0.750</td><td style='text-align: center; word-wrap: break-word;'>0.868</td><td style='text-align: center; word-wrap: break-word;'>0.177</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Final BA (during Dropout)</td><td style='text-align: center; word-wrap: break-word;'>2.471</td><td style='text-align: center; word-wrap: break-word;'>0.661</td><td style='text-align: center; word-wrap: break-word;'>2.451</td><td style='text-align: center; word-wrap: break-word;'>0.636</td><td style='text-align: center; word-wrap: break-word;'>0.875</td><td style='text-align: center; word-wrap: break-word;'>0.169</td></tr><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>(1.719)</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>(1.655)</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>(0.386)</td></tr></table>

所有方法的结果均为3次运行的中位数。

为了模拟真实场景，我们选择在该序列中街道更狭窄、建筑物更高的区域进行信号中断模拟。本次评估的主要目的在于验证全球位置测量数据融合的可行性，尤其是展示第V-G3节中提到的用于应对长时间信号中断的全局回环式对齐策略的有效性。表VII列出了所有可能的传感器配置（vig、vidg和vilg）下的整个轨迹的ATE数值，以及仅在GNSS信号中断期间的ATE数值。显然，在所有配置中，融合GNSS测量数据后整体轨迹误差都显著降低。此外还可以看出，视觉-惯性-激光雷达配置的高精度有助于在GNSS信号中断期间将轨迹漂移降至最低。通过回环检测和最终的BA优化，我们最终得到的轨迹RMSE ATE仅为16.9厘米，而该轨迹长度接近3公里。图10直观展示了重建结果和轨迹精度。

尽管该系统最初是为融合间歇性的高精度（RTK-）GNSS测量数据而设计的，但它同样也能从低精度的GNSS测量数据中获益。

---

![Image](images/77c597d08001b980f9a33a7a925ba626779ea23046d5d207d74937ea223665c2.jpg)

<div style="text-align: center;">Fig. 9. 3D reconstructions obtained from the volumetric submaps of OKVIS2-X. The left column showcases reconstructions when leveraging learned-depth while the ones on the right use a LiDAR sensor. The top row is obtained from the Hilt22 exp06 sequence, the bottom row is extracted from VBR's Ciampino0 sequence. A line with a distance indicates the different scales of the sequences.</div>

![Image](images/39399d88b9bf531df5ad7b38887fe185a41a1b5e9bbb7c1184457472d768efe2.jpg)

<div style="text-align: center;">Fig. 10. Reconstruction of the Campus1 (VBR) is visualized as overlay of all submaps. Causal Trajectory Estimates for our Visual-Inertial-Depth (blue) and Visual-Inertial-LiDAR (yellow) configurations with GNSS fusion are drawn together with ground-truth measurements in GNSS-accessible (dashed green) and GNSS-denied regions (dashed red). While the LiDAR-based setup aligns accurately with the ground-truth even in GNSS-denied areas, the vision-based configuration drifts significantly. The visible jump in the trajectory at the end of the GNSS dropout highlights the global alignment strategy of our approach.</div>

为了证明这一点，我们在GVINS提供的具有挑战性的数据集序列上对OKVIS2-X进行了评估[^50]。该数据集包含立体相机、IMU以及ZEDF9P GNSS传感器。作者提供了RTK解决方案以及原始GNSS测量数据（如伪距、多普勒测量值等）。我们选择在长达近3公里的复杂环境序列上进行评估，因为该序列对不同传感器模态而言充满挑战：在极亮或极暗区域图像中缺乏特征，而在杂乱环境或室内则存在GNSS信号严重干扰或完全无法接收的情况（见图11）。利用RTKLIB[^94]，我们根据原始测量数据计算出单点定位（SPP）解，并将其融合到因子图优化中。在以RTK解为参考的情况下，我们在具有有效RTK固定的区域内，对经过6自由度对齐后的RMSE ATE进行了评估。即便SPP解精度较低（RMSE ATE为19.99米），所提出的GNSS融合技术仍显著降低了视觉惯性基线的误差，从23.72米分别降至12.51米（因果情况）、8.47米（非因果情况）以及5.12米（最终批量调整结果）。需要注意的是，在此评估中，为了与GVINS进行更公平的比较，我们禁用了视觉回环闭合功能[^50]，GVINS的RMSE ATE为4.45米。这表明，尽管GNSS数据融合策略较为简单且数据精度较低（仅采用SPP，未使用多普勒速度），但整体精度仍然可观。

---

![Image](images/8124fa8701cb730d0adb41a1e83efb1f6eb907b4e170c7b3eb0a3f51eb87f272.jpg)

<div style="text-align: center;">Fig. 11. SPP Solution of GNSS and OKVIS2-X estimated trajectories overlaid in Google Maps in the complex_environment in the GVINS dataset.</div>

<div style="text-align: center;">TABLE VIII</div>

<div style="text-align: center;">ABLATION STUDY IN ONLINE EXTRINSIC CALIBRATION: ATE RMSE [M] IN THE HILTI-OXFORD DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>Online-calibration</td><td style='text-align: center; word-wrap: break-word;'>Post-calibration</td><td style='text-align: center; word-wrap: break-word;'>exp04</td><td style='text-align: center; word-wrap: break-word;'>exp05</td><td style='text-align: center; word-wrap: break-word;'>exp06</td><td style='text-align: center; word-wrap: break-word;'>Avg</td></tr><tr><td rowspan="3">Ours-vi-c</td><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>0.198</td><td style='text-align: center; word-wrap: break-word;'>0.192</td><td style='text-align: center; word-wrap: break-word;'>0.085</td><td style='text-align: center; word-wrap: break-word;'>0.158</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>0.070</td><td style='text-align: center; word-wrap: break-word;'>0.058</td><td style='text-align: center; word-wrap: break-word;'>0.067</td><td style='text-align: center; word-wrap: break-word;'>0.065</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>0.058</td><td style='text-align: center; word-wrap: break-word;'>0.044</td><td style='text-align: center; word-wrap: break-word;'>0.048</td><td style='text-align: center; word-wrap: break-word;'>0.050</td></tr><tr><td rowspan="3">Ours-vi-nc</td><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>0.157</td><td style='text-align: center; word-wrap: break-word;'>0.169</td><td style='text-align: center; word-wrap: break-word;'>0.090</td><td style='text-align: center; word-wrap: break-word;'>0.139</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>0.036</td><td style='text-align: center; word-wrap: break-word;'>0.049</td><td style='text-align: center; word-wrap: break-word;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>0.042</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>0.033</td><td style='text-align: center; word-wrap: break-word;'>0.026</td><td style='text-align: center; word-wrap: break-word;'>0.042</td><td style='text-align: center; word-wrap: break-word;'>0.034</td></tr><tr><td rowspan="3">Ours-vi-ba</td><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>0.096</td><td style='text-align: center; word-wrap: break-word;'>0.088</td><td style='text-align: center; word-wrap: break-word;'>0.061</td><td style='text-align: center; word-wrap: break-word;'>0.082</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>0.023</td><td style='text-align: center; word-wrap: break-word;'>0.022</td><td style='text-align: center; word-wrap: break-word;'>0.050</td><td style='text-align: center; word-wrap: break-word;'>0.032</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>X</td><td style='text-align: center; word-wrap: break-word;'>✓</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.028</td><td style='text-align: center; word-wrap: break-word;'>0.048</td><td style='text-align: center; word-wrap: break-word;'>0.032</td></tr></table>

所有方法均报告3次运行的中位数结果。

具有竞争力。图11展示了与SPP解叠加后的估计轨迹。可以明显看出，与视觉惯性基线相比，融合后的估计结果在信号覆盖良好的区域能与道路布局很好地吻合，即便在GNSS信号严重受扰的区域也能保持一致。

### F. 消融研究：在线外参标定

为了验证IMU到相机在线外参标定的有效性，表VIII报告了在线标定对估计轨迹精度影响的消融研究结果。其中，“在线标定”表示对外参参数进行在线优化，“事后标定”则指用我们标定的外参初始化参数并保持恒定。我们选择了Hilti-Oxford数据集中有密集真实轨迹可用的$\exp04$、$\exp05$和$\exp06$序列。在实时轨迹中，我们对所有五个相机的先验平移设置了1毫米的标准差，先验方向则设为$0.29^{\circ}$。在完整的批量调整中，由于已有初始化的位姿和路标，我们对标准差施加了更宽松的约束，以便更灵活地优化外参：分别为3毫米和$0.92^{\circ}$。可以清楚地看到，与未进行标定的情况相比，在线标定在所有序列中都提高了轨迹精度，如表VIII所示。

<div style="text-align: center;">TABLE IX</div>

<div style="text-align: center;">MAXIMUM MEMORY [GB] REQUIRED FOR DIFFERENT BENCHMARKED METHODS AND DATASETS</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>Ours-vi</td><td style='text-align: center; word-wrap: break-word;'>Ours-vid</td><td style='text-align: center; word-wrap: break-word;'>Ours-vil</td><td style='text-align: center; word-wrap: break-word;'>FAST-LIVO</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>HILTI-Oxford: exp06</td><td style='text-align: center; word-wrap: break-word;'>4.86</td><td style='text-align: center; word-wrap: break-word;'>11.62</td><td style='text-align: center; word-wrap: break-word;'>11.51</td><td style='text-align: center; word-wrap: break-word;'>3.97</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>VBR: Campus1</td><td style='text-align: center; word-wrap: break-word;'>4.50</td><td style='text-align: center; word-wrap: break-word;'>10.8</td><td style='text-align: center; word-wrap: break-word;'>25.5</td><td style='text-align: center; word-wrap: break-word;'>15.0</td></tr></table>

为了验证优化后的外参参数的有效性，我们使用未经在线标定的外参进行了额外运行评估。有趣的是，事后标定甚至改善了在线标定的结果。这表明我们估计的外参接近真实外参。此外，为了验证其对初始误差的鲁棒性，我们在完整批量调整标定后，从我们的外参出发对初始外参进行扰动，扰动范围在$\pm1$厘米至$\pm0.3^\circ$之间，且服从$\exp06$中的均匀分布。经过10次运行后，序列末尾外参的标准差仅为0.08厘米和0.02°。这表明在在线标定中，受扰动的外参成功收敛到了参考值。

### G. 可配置参数研究

我们针对关键帧数量（kf）、IMU帧数量（if）、子图因子数量（ns）以及体素分辨率（res）这两个参数，深入研究了vid模式和vil模式下轨迹误差与实时图优化耗时的情况，如图13所示。对于基于子图的对齐因子，我们将地图间因子的数量设置为帧到地图因子数量的整数倍（即5ns）。

在图13(a)所示的vid配置中，增加关键帧和IMU帧的数量会导致处理时间变长，而在所提出的参数配置下精度已达到饱和状态（灰色柱状图）。同样，增加更多子图因子并使用更精细的体素分辨率会延长计算时间，但并不一定能提升精度，因为估计出的轨迹精度已经超过了深度网络的精度上限。相比之下，在vil配置下（图13(b)），更多的子图因子能够提升精度。不过，由于占用场和梯度中存在噪声，更精细的体素分辨率并不总能带来更好的轨迹估计结果。此外，由于vil模式下子图因子起主导作用，增加更多关键帧和IMU帧也并不能显著提升精度。总体而言，目前的参数配置在深度相机和激光雷达两种场景下都能在精度与计算时间之间取得良好平衡。

### H. 时间与内存开销

在本节中，我们将证明该系统能够实时运行，且整个架构及其各模块的内存效率足以支持大规模SLAM任务的执行。我们首先在图12中展示了SLAM系统各主要组件的运行时间：视觉前端、实时图优化、子图构建接口以及深度网络。所有时间数据均在以下环境中测量：

---

![Image](images/4f69858407c1877990690a0a0b94c1c13c89e34bdead922d8d415b35a6b1a8e3.jpg)

![Image](images/054ab57f43a12f741c51df2500a30bc2c7ddf8920d2f3e44d61cc46748165797.jpg)

![Image](images/23654752dfc4cfdf09e90ccf014b771f44f11956a6cb6dcd960bb4817182e1f9.jpg)

<div style="text-align: center;">Fig. 12. Timing breakdown of OKVIS2-X with different configurations in ms per frame. Each bar represents a thread, and multiple threads run in parallel, therefore, the total timing is not the sum of each bar. The networks, depth fusion, and map integration do not necessarily have to be performed for every frame or LiDAR scan. Also, HILTI-Oxford uses five cameras on the rig, and VBR employs high-resolution cameras amenable to higher keypoint numbers.</div>

![Image](images/e8f3c531befea5daaded05feb1e352ea7baf230d1743bf606ef02596f0943cc9.jpg)

![Image](images/7231d3503fc58e500f99c9f5e5cfb5effffc10872258a01fd511f06e5584c915.jpg)

<div style="text-align: center;">Fig. 13. Parameter study on OKVIS2-X (a) across all sequences in EuRoC and (b) Campus0 in the VBR dataset with the number of keyframes  $k_f$, imu frames if, submap factors  $n_s$, and the voxel resolution  $r_s$ in cm. Each vertical bar represents 10, 50, 90 percentiles in non-causal ATE, while the horizontal bar means 10, 50, 90 percentiles in the real-time optimization timings per frame. The statistics comes from 10 independent runs.</div>

配备i7-13700 CPU和RTX-3080 10GB GPU的台式机。得益于我们的多线程处理方式以及对内部队列的利用，我们将子图构建接口与状态估计器在计算上解耦。因此，并非所有运行时间都是连续的，直接将这些时间相加也无法得到处理多帧数据所需的总时间。

在EuRoC数据集的MH05场景中，我们的视觉-惯性融合配置Ours-vi的运行频率可达47 Hz；采用深度网络配置时，MVS线程处理8视图图像的频率可达13 Hz。需要注意的是，立体网络和深度融合模块是与MVS网络一同在GPU上运行的。此外，网络和地图集成模块并不需要对每一帧都进行处理。在Hliti-Oxford数据集的exp06场景中，Ours-vi能以10 Hz的频率处理全部五路输入图像，而Ours-vid的处理频率为8 Hz，其瓶颈在于实时优化图的计算。Ours-vil的实时估计器运行频率为8 Hz，而由于激光雷达点云数据量较大，子图构建接口的运行频率仅为3.5 Hz。在Campus1场景中，Ours-vi的运行频率为16 Hz，而来自GNSS的全球位置融合所带来的额外开销微乎其微。需要注意的是，VBR场景中使用的分辨率较高的摄像头会提取出更多的关键点。如第VI-D节所述，我们在立体网络中采用了额外的细化步骤[^79]，这会增加推理时间。总体而言，我们的不确定性感知深度网络在所有数据集上都能实现至少13 Hz的实时处理能力。不过需要强调的是，许多参数都存在权衡关系：可以牺牲少量精度来换取更快的运行速度，这使得包括网络、深度或激光雷达集成模块在内的整个系统也能在搭载NVIDIA Orin NX的无人机上运行（详见补充视频）。

为了对运行时间进行基准测试，我们测量了总耗时（挂钟时间），并将其除以Ours-vi和ORB-SLAM3处理的帧总数。需要注意的是，由于OKVIS2-X和ORB-SLAM3的算法结构不同，无法直接比较各个模块的运行时间，因此我们认为挂钟时间的对比是最公平的衡量标准。在MH05数据集上，我们的方法效率更高（38.1毫秒对比64.7毫秒）；在exp06数据集上，两者的表现相当（112.3毫秒对比116.1毫秒）。在Campus1数据集上，尽管我们的方法运行时间更长（107.3毫秒对比69.1毫秒），但其精度明显更优（2.499米），而ORB-SLAM3的精度仅为7.289米，因此这种权衡是值得的。

在表IX中，我们展示了OKVIS2-X在不同传感器配置和数据集下所需的最大内存。不出所料，使用密集体素地图会导致其内存需求高于基于路标的视觉惯性SLAM系统。与FAST-LIVO [^40]相比，我们的方法需要更多内存，

---

因为我们的地图以体素形式存储空闲区域和已占用区域，而FAST-LIVO仅存储降采样后的点云。不过，我们的地图表示可直接用于下游任务，尤其是确保无碰撞的自主导航。此外，所提出方法的内存需求即便在消费级计算机上也能满足，甚至可以在无人机上运行。在$\exp{06}$中，Ours-vid和Ours-vil的内存占用相近：由于我们的重叠策略，Ours-vid会生成更多子图，尽管使用激光雷达时映射的体积更大。在GPU内存占用方面，Ours-vid用于多视图立体匹配网络和立体网络的内存仅3.51GB。

### 七、结论

在本文中，我们提出了一种多传感器SLAM系统OKVIS2-X，它是对基于稀疏路标的OKVIS2 [^2]的重要扩展。为了实现大规模环境下的高鲁棒性和高精度定位与建图，我们引入了与状态估计器紧密耦合的体素占据子图技术。此外，我们还提出了一种统一的框架，能够整合多种模态数据，包括视觉、惯性、神经网络生成的深度信息、激光雷达产生的点云，以及GNSS接收机提供的全局位置信息。我们在多个基准数据集上对OKVIS2-X进行了全面评估，涵盖了轨迹精度、建图精度、内存占用和运行时间等方面，从而为现有最先进的SLAM系统确立了新的基准。值得注意的是，采用视觉-惯性-激光雷达配置时，我们能够在9公里的行驶过程中实现亚米级的轨迹精度，同时完成密集体素占据的建图。

未来，我们希望进一步突破界限，纳入传感器时间偏移的在线校准、激光雷达和GNSS天线的外参标定、多会话重定位、开放词汇场景理解、动态物体检测，以及在复杂的大规模环境中随时间变化的检测功能。此外，采用按需的本地在线地图保存与加载机制，有望实现更精细的占据分辨率以及实际上无限大的地图扩展性。

## 附录

### A. 误差状态定义

我们通常对线性化点附近的位姿使用扰动$\delta\chi_T = [\delta\mathbf{r}, \delta\boldsymbol{\alpha}]$来表示平移$\overline{\mathbf{r}}$和朝向$\overline{\mathbf{C}}$：

 $$ \mathbf{r}=\bar{\mathbf{r}}+\delta\mathbf{r},\qquad\mathbf{C}=\mathrm{E x p}\left(\delta\alpha\right)\overline{{\mathbf{C}}}, $$

其中额外的下标用于指示相应的变换。在此基础上，我们进一步将误差状态定义为$\delta\chi = [\delta\chi_{T_{WS}}, \delta\chi_{sb}]$，其中$\delta\chi_{sb}$表示速度和偏差的加性扰动。

### B. GNSS协方差

公式(26)中GNSS残差的整体协方差矩阵$\Sigma_{\mathrm{g}}^{j}$由GNSS测量协方差矩阵$\Sigma_{\mathrm{g}_0}^{j}$和IMU预积分协方差$\Sigma_{x}^{j}$组成。因此，$\Sigma_{\mathrm{g}}^{j}$可通过以下方式推导得出：

 $$ \begin{array}{r}{\mathbf{\Sigma}_{\mathrm{g}}^{j}=\mathbf{\Sigma}_{\mathrm{g}_{0}}^{j}+\mathbf{J}\mathbf{\Sigma}_{\mathbf{x}}^{j}\mathbf{J}^{T}.}\end{array} $$

此处，J表示$\mathbf{e}_{\mathrm{g}}^{j}$关于$\hat{\chi}_{T_{WS_{j}}}$的雅可比矩阵。

### C. GNSS雅可比矩阵

基于IMU测量得到的预测位姿$\hat{T}_{WS}$，以及GNSS坐标系与世界坐标系之间的相对位姿$T_{GW}$，方程(26)中全局位置残差关于位姿误差状态的雅可比矩阵如下所示：

 $$ \begin{align*}\frac{\partial\mathbf{e}_{\mathrm{g}}^{j}}{\partial\delta\hat{\chi}_{T_{W S_{j}}}}&=\begin{bmatrix}-\overline{\mathbf{C}}_{GW}&\overline{\mathbf{C}}_{GW}\left[\overline{\mathbf{C}}_{W S S}\mathbf{r}_{A}\right]^{\times}\end{bmatrix},\\\frac{\partial\mathbf{e}_{\mathrm{g}}^{j}}{\partial\delta\chi_{T_{G W}}}&=\begin{bmatrix}-\mathbf{I}_{3\times3}&\left[\overline{\mathbf{C}}_{GW}\left(_{W}\overline{\mathbf{r}}_{S}+\overline{\mathbf{C}}_{W S S}\mathbf{r}_{A}\right)\right]^{\times}\end{bmatrix}.\end{align*} $$

预测位姿并非位姿图优化中的待估计状态。由于预测测量值是时间步长i处估计状态的函数，即$\hat{\mathbf{x}}^{j} = f(\mathbf{x}^{i})$，我们实际上可以在估计状态中对$\mathbf{x}^{i}$进行优化。关于$\mathbf{x}^{i}$的雅可比矩阵可利用链式法则和标准IMU预积分雅可比矩阵来计算，详见[^87]。

### D. 子图对齐雅可比矩阵

由于密集对齐残差（公式(23)）仅依赖于位姿状态，而与速度和偏差无关，因此我们可以利用链式法则计算关于帧$\underline{\mathscr{F}}_{S_{a}}$和$\underline{\mathscr{F}}_{S_{b}}$的位姿的雅可比矩阵：

 $$ \frac{\partial e_{\mathrm{m}}^{a,b}}{\delta\mathcal{X}_{T_{W S_{k}}}}=\frac{\partial e_{\mathrm{m}}^{a,b}}{\partial_{S_{a}}\mathbf{p}}\frac{\partial_{S_{a}}\mathbf{p}}{\delta\mathcal{X}_{T_{W S_{k}}}}, $$

其中$k \in \{a, b\}$。各雅可比矩阵如下：

 $$\begin{aligned}\frac{\partial e_{\mathrm{m}}^{a,b}}{\partial_{S_{a}}\mathbf{p}}&=\frac{\nabla L}{\sqrt{\frac{L_{\min}^{2}}{9}+\left|\nabla L\right|^{2}\sigma_{d}^{2}}}\\\frac{\partial_{S_{a}}\mathbf{p}}{\delta\mathcal{X}_{T_{W S_{a}}}}&=\overline{\mathbf{C}}_{S_{a}W}\begin{bmatrix}-\mathbf{I}_{3}&\left[\overline{\mathbf{C}}_{W S_{b}S_{b}}\mathbf{p}+W\overline{\mathbf{r}}_{S_{b}}-W\overline{\mathbf{r}}_{S_{a}}\right]\end{bmatrix}^{\times}\end{aligned}$$

## 参考文献

[^1]: L. Brizi, E. Giacomini, L. D. Giammarino, S. Ferrari, O. Salem, L. D. Rebotti, and G. Grisetti, “VBR: A Vision Benchmark in Rome,” in 2024 IEEE International Conference on Robotics and Automation (ICRA), 2024.

[^2]: S. Leutenegger, "OKVIS2: Realtime scalable visual-inertial SLAM with loop closure," arXiv preprint arXiv:2202.09199, 2022.

[^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, 2021.

[^4]: M. Abate, Y. Chang, N. Hughes, and L. Carlone, “Kimera2: Robust and Accurate Metric-Semantic SLAM in the Real World,” in International Symposium on Experimental Robotics. Springer, 2023, pp. 81–95.

## [^5]: 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

---

[^6]: R. A. Newcombe, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim, A. J. Davison, P. Kohi, J. Shotton, S. Hodges, and A. Fitzgibbon, "KinectFusion: Real-time dense surface mapping and tracking," in 2011 10th IEEE international symposium on mixed and augmented reality. IEEE, 2011.

[^7]: K. Tateno, F. Tombari, I. Laina, and N. Navab, “CNN-SLAM: Real-time dense monocular slam with learned depth prediction,” in Proceedings of the IEEE conference on computer vision and pattern recognition, 2017.

[^8]: 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, 2017.

[^9]: C. Zheng, W. Xu, Z. Zou, T. Hua, C. Yuan, D. He, B. Zhou, Z. Liu, J. Lin, F. Zhu et al., "FAST-LIVO2: Fast, direct lidar-inertial-visual odometry," IEEE Transactions on Robotics, 2024.

[^10]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled LiDAR-visual-inertial odometry via smoothing and mapping," in 2021 IEEE international conference on robotics and automation (ICRA). IEEE, 2021.

[^11]: D. Wisth, M. Camurri, and M. Fallon, "VILENS: Visual, inertial, lidar, and leg odometry for all-terrain legged robots," IEEE Transactions on Robotics, vol. 39, no. 1, 2022.

[^12]: S. Boche, X. Zuo, S. Schaefer, and S. Leutenegger, “Visual-inertial SLAM with tightly-coupled dropout-tolerant GPS fusion,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022.

[^13]: S. Boche, S. B. Laina, and S. Leutenegger, “Tightly-Coupled LiDAR-Visual-Inertial SLAM and Large-Scale Volumetric Occupancy Mapping,” in 2024 IEEE International Conference on Robotics and Automation (ICRA), 2024.

[^14]: J. Jung, S. Boche, S. B. Laina, and S. Leutenegger, “Uncertainty-aware visual-inertial slam with volumetric occupancy mapping,” in 2025 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2025.

[^15]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” in Proceedings 2007 IEEE international conference on robotics and automation. IEEE, 2007.

[^16]: 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, 2015.

[^17]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 40, no. 3, 2017.

[^18]: T. Qin, S. Cao, J. Pan, and S. Shen, “A general optimization-based framework for global pose estimation with multiple sensors,” arXiv preprint arXiv:1901.03642, 2019.

[^19]: 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, 2018.

[^20]: Y. Wang, Y. Ng, I. Sa, A. Parra, C. Rodriguez-Opazo, T. Lin, and H. Li, "MAVIS: Multi-Camera Augmented Visual-Inertial SLAM using SE 2 (3) Based Exact IMU Pre-integration," in IEEE International Conference on Robotics and Automation (ICRA), 2024.

[^21]: S. Rahman, A. Q. Li, and I. Rekleitis, “SVIn2: An Underwater SLAM System using Sonar, Visual, Inertial, and Depth Sensor,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019.

[^22]: S. Rahman, A. Quattrini Li, and I. Rekleitis, “SVIn2: A multi-sensor fusion-based underwater SLAM system,” The International Journal of Robotics Research, vol. 41, no. 11-12, 2022.

[^23]: L. Han, Y. Lin, G. Du, and S. Lian, “DeepVIO: Self-supervised deep learning of monocular visual inertial odometry using 3d geometric constraints,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019.

[^24]: X. Peng, Z. Liu, W. Li, P. Tan, S. Y. Cho, and Q. Wang, “DVI-SLAM: A dual visual inertial slam network,” in 2024 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2024.

[^25]: Y. Zhou, X. Li, S. Li, X. Wang, S. Feng, and Y. Tan, “DBA-Fusion: Tightly Integrating Deep Dense Visual Bundle Adjustment with Multiple Sensors for Large-Scale Localization and Mapping,” IEEE Robotics and Automation Letters, 2024.

[^26]: J. Zhang and S. Singh, "LOAM: Lidar odometry and mapping in real-time." in Robotics: Science and systems, vol. 2, no. 9, 2014.

[^27]: T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized Lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.

[^28]: J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV,” in 2020 IEEE international conference on robotics and automation (ICRA). IEEE, 2020.

[^29]: I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, and C. Stachniss, "KISS-ICP: In defense of point-to-point ICP-simple, accurate, and robust registration if done the right way," IEEE Robotics and Automation Letters, vol. 8, no. 2, 2023.

[^30]: P. Dellenbach, J.-E. Deschaud, B. Jacquet, and F. Goulette, "CTICP: Real-time elastic LiDAR odometry with loop closure," in 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022.

[^31]: Z. Liu and F. Zhang, “BALM: Bundle adjustment for Lidar mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 2, 2021.

[^32]: L. Wiesmann, E. Marks, S. Gupta, T. Guadagnino, J. Behley, and C. Stachniss, "Efficient LiDAR bundle adjustment for multi-scan alignment utilizing continuous-time trajectories," arXiv preprint arXiv:2412.11760, 2024.

[^33]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIO-SAM: Tightly-coupled Lidar inertial odometry via smoothing and mapping," in 2020 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2020.

[^34]: W. Xu and F. Zhang, “FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated kalman filter,” IEEE Robotics and Automation Letters, vol. 6, no. 2, 2021.

[^35]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, "FAST-LIO2: Fast direct LiDAR-inertial odometry," IEEE Transactions on Robotics, vol. 38, no. 4, 2022.

[^36]: X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, "LIC-Fusion: LiDAR-inertial-camera odometry," in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019.

[^37]: X. Zuo, Y. Yang, P. Geneva, J. Lv, Y. Liu, G. Huang, and M. Pollefeys, "LIC-Fusion 2.0: LiDAR-inertial-camera odometry with sliding-window plane-feature tracking," in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020.

[^38]: J. Lin, C. Zheng, W. Xu, and F. Zhang, "R2LIVE: A Robust, Real-Time, LiDAR-Inertial-Visual Tightly-Coupled State Estimator and Mapping," IEEE Robotics and Automation Letters, vol. 6, no. 4, 2021.

[^39]: J. Lin and F. Zhang, "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package," in 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022.

[^40]: C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, "FAST-LIVO: Fast and tightly-coupled sparse-direct LiDAR-inertial-visual odometry," in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022.

[^41]: S. Lynen, M. W. Achtelik, S. Weiss, M. Chli, and R. Siegwart, “A robust and modular multi-sensor fusion approach applied to MAV navigation,” in 2013 IEEE/RSJ international conference on intelligent robots and systems. IEEE, 2013.

[^42]: S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar, “Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft MAV,” in 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014.

[^43]: W. Lee, K. Eckenhoff, P. Geneva, and G. Huang, “Intermittent GPS-aided VIO: Online initialization and calibration,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020.

[^44]: J. Gao, J. Sha, H. Li, and Y. Wang, “A Robust and Fast GNSS-Inertial-LiDAR Odometry with INS-Centric Multiple Modalities by IESKF,” IEEE Transactions on Instrumentation and Measurement, 2024.

[^45]: W. Lee, P. Geneva, C. Chen, and G. Huang, “MINS: Efficient and Robust Multisensor-Aided Inertial Navigation System,” Journal of Field Robotics, 2023.

[^46]: H. Strasdat, J. Montiel, and A. J. Davison, “Real-time monocular SLAM: Why filter?” in 2010 IEEE International Conference on Robotics and Automation. IEEE, 2010.

[^47]: R. Mascaro, L. Teixeira, T. Hinzmann, R. Siegwart, and M. Chli, "GOMSF: Graph-optimization based multi-sensor fusion for robust uav pose estimation," in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018.

[^48]: G. Cioffi and D. Scaramuzza, “Tightly-coupled fusion of global positional measurements in optimization-based visual-inertial odometry,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020.

## [^49]: J. Liu, W. Gao, and Z. Hu, “Optimization-based visual-inertial SLAM tightly coupled with raw GNSS measurements,” in 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021

---

[^50]: S. Cao, X. Lu, and S. Shen, “GVINS: Tightly coupled GNSS–visual–inertial fusion for smooth and consistent state estimation,” IEEE Transactions on Robotics, vol. 38, no. 4, 2022.

[^51]: H. Zhang, C.-C. Chen, H. Vallery, and T. D. Barfoot, “GNSS/Multi-Sensor Fusion Using Continuous-Time Factor Graph Optimization for Robust Localization,” IEEE Transactions on Robotics, 2024.

[^52]: J. Cremona, J. Civera, E. Kofman, and T. Pire, “GNSS-stereo-inertial SLAM for arable farming,” Journal of field robotics, vol. 41, no. 7, 2024.

[^53]: X. Li, L. Zhang, X. Zhang, and Z. Jiao, "LC-GVINS: A Robust, Accurate, Monocular Visual-Inertial-GNSS SLAM with Loop Closure," in 2023 IEEE International Conference on Mechatronics and Automation (ICMA). IEEE, 2023.

[^54]: T. Whelan, R. F. Salas-Moreno, B. Glocker, A. J. Davison, and S. Leutenegger, “ElasticFusion: Real-time dense SLAM and light source estimation,” The International Journal of Robotics Research, vol. 35, no. 14, 2016.

[^55]: T. Laidlow, J. Czarnowski, and S. Leutenegger, “DeepFusion: Real-time dense 3D reconstruction for monocular SLAM using single-view depth and gradient predictions,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019.

[^56]: A. Rosinol, A. Violette, M. Abate, N. Hughes, Y. Chang, J. Shi, A. Gupta, and L. Carlone, "Kimera: From SLAM to spatial perception with 3D dynamic scene graphs," The International Journal of Robotics Research, vol. 40, no. 12-14, 2021.

[^57]: L. Koestler, N. Yang, N. Zeller, and D. Cremers, “Tandem: Tracking and dense mapping in real-time using deep multi-view stereo,” in Conference on Robot Learning. PMLR, 2022.

[^58]: Y. Xin, X. Zuo, D. Lu, and S. Leutenegger, “SimpleMapping: Real-time visual-inertial dense mapping with deep multi-view stereo,” in 2023 IEEE International Symposium on Mixed and Augmented Reality (ISMAR). IEEE, 2023.

[^59]: M. Sayed, J. Gibson, J. Watson, V. Prisacariu, M. Firman, and C. Godard, “SimpleRecon: 3D reconstruction without 3D convolutions,” in European Conference on Computer Vision. Springer, 2022.

[^60]: A. Rosinol, J. J. Leonard, and L. Carlone, “Probabilistic volumetric fusion for dense monocular SLAM,” in Proceedings of the IEEE/CVF Winter Conference on Applications of Computer Vision, 2023.

[^61]: 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, 2021.

[^62]: R. Murai, E. Dexheimer, and A. J. Davison, “MASt3R-SLAM: Real-time dense SLAM with 3D reconstruction priors,” in Proceedings of the Computer Vision and Pattern Recognition Conference, 2025.

[^63]: Z. Zhu, S. Peng, V. Larsson, W. Xu, H. Bao, Z. Cui, M. R. Oswald, and M. Pollefeys, "NICE-SLAM: Neural implicit scalable encoding for SLAM," in Proceedings of the IEEE/CVF conference on computer vision and pattern recognition, 2022.

[^64]: E. Sandström, K. Ta, L. Van Gool, and M. R. Oswald, “Uncle-SLAM: Uncertainty learning for dense neural SLAM,” in Proceedings of the IEEE/CVF International Conference on Computer Vision, 2023.

[^65]: E. Sandström, Y. Li, L. Van Gool, and M. R. Oswald, “Point-SLAM: Dense neural point cloud-based SLAM,” in Proceedings of the IEEE/CVF International Conference on Computer Vision, 2023.

[^66]: L. Liso, E. Sandström, V. Yugay, L. Van Gool, and M. R. Oswald, "Loopy-SLAM: Dense neural SLAM with loop closures," in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2024.

[^67]: M. Bosse, P. Newman, J. Leonard, M. Soika, W. Feiten, and S. Teller, "An atlas framework for scalable mapping," in 2003 IEEE International Conference on Robotics and Automation, vol. 2. IEEE, 2003.

[^68]: M. Ramezani, K. Khosoussi, G. Catt, P. Moghadam, J. Williams, P. Borges, F. Pauling, and N. Kottege, "Wildcat: Online continuous-time 3D lidar-inertial SLAM," arXiv preprint arXiv:2205.12595, 2022.

[^69]: B.-J. Ho, P. Sodhi, P. Teixeira, M. Hsiao, T. Kusnur, and M. Kaess, “Virtual occupancy grid map for submap-based pose graph SLAM and planning in 3D environments,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.

[^70]: A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, "OctoMap: An efficient probabilistic 3D mapping framework based on octrees," Autonomous robots, vol. 34, pp. 189–206, 2013.

[^71]: V. Reijgwart, A. Millane, H. Oleynikova, R. Siegwart, C. Cadena, and J. Nieto, “Voxgraph: Globally consistent, volumetric mapping using signed distance function submaps,” IEEE Robotics and Automation Letters, vol. 5, no. 1, 2019.

[^72]: Y. Wang, N. Funk, M. Ramezani, S. Papatheodorou, M. Popović, M. Camurri, S. Leutenegger, and M. Fallon, “Elastic and efficient

“大规模探索任务中的激光雷达重建”，载于2021年IEEE国际机器人与自动化会议（ICRA）。IEEE出版社，2021年。

[^73]: N. Funk, J. Tarrio, S. Papatheodorou, M. Popović, P. F. Alcantarilla, and S. Leutenegger, “Multi-resolution 3D mapping with explicit free space representation for fast and accurate mobile robot motion planning,” IEEE Robotics and Automation Letters, vol. 6, no. 2, 2021.

[^74]: Y. Wang, M. Ramezani, M. Mattamala, S. T. Digumarti, and M. Fallon, “Strategies for large scale elastic and semantic LiDAR reconstruction,” Robotics and Autonomous Systems, vol. 155, p. 104185, 2022.

[^75]: 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, 2019.

[^76]: S. Barbas Laina, S. Boche, S. Papatheodorou, D. Tzoumanikas, S. Schaefer, H. Chen, and S. Leutenegger, “Scalable autonomous drone flight in the forest with visual-inertial SLAM and dense submaps built without LiDAR,” arXiv preprint arXiv:2403.09596, 2024.

[^77]: S. Papatheodorou, S. Boche, S. B. Laina, and S. Leutenegger, “Efficient submap-based autonomous MAV exploration using visual-inertial slam configurable for LiDARS or depth cameras,” in 2025 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2025.

[^78]: J. Knights, S. B. Laina, P. Moghadam, and S. Leutenegger, “Solver: Submap oriented lidar-visual re-localisation,” in 2025 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2025.

[^79]: H. Xu, J. Zhang, J. Cai, H. Rezatofighi, F. Yu, D. Tao, and A. Geiger, "Unifying flow, stereo and depth estimation," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 45, no. 11, 2023.

[^80]: A. Kendall and Y. Gal, “What uncertainties do we need in Bayesian deep learning for computer vision?” Advances in Neural Information Processing Systems, vol. 30, 2017.

[^81]: M. Poggi, F. Aleotti, F. Tosi, and S. Mattoccia, “On the uncertainty of self-supervised monocular depth estimation,” in IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 2020.

[^82]: N. A. Carlson, “Federated square root filter for decentralized parallel processors,” IEEE Transactions on Aerospace and Electronic Systems, vol. 26, no. 3, 1990.

[^83]: 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.

[^84]: S. Leutenegger, M. Chli, and R. Y. Siegwart, “BRISK: Binary robust invariant scalable keypoints,” in 2011 International conference on computer vision. Ieee, 2011.

[^85]: R. P. Poudel, S. Liwicki, and R. Cipolla, "Fast-SCNN: Fast semantic segmentation network," arXiv preprint arXiv:1902.04502, 2019.

[^86]: D. Gálvez-López and J. D. Tardos, “Bags of binary words for fast place recognition in image sequences,” IEEE Transactions on Robotics, vol. 28, no. 5, 2012.

[^87]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, 2016.

[^88]: Z. Zhang and D. Scaramuzza, “A tutorial on quantitative trajectory evaluation for visual (-inertial) odometry,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.

[^89]: M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W. Achtelik, and R. Siegwart, "The EuRoC micro aerial vehicle datasets," The International Journal of Robotics Research, 2016. [Online]. Available: <http://ijr.sagepub.com/content/early/2016/01/21/0278364915620033.abstract>

[^90]: 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, 2022.

[^91]: W. E. Lorensen and H. E. Cline, “Marching cubes: A high resolution 3D surface construction algorithm,” in Seminal graphics: pioneering efforts that shaped the field, 1998, pp. 347–353.

[^92]: W. Zhang, S. Wang, X. Dong, R. Guo, and N. Haala, “BAMF-SLAM: Bundle adjusted multi-fisheye visual-inertial slam using recurrent field transforms,” in IEEE International Conference on Robotics and Automation (ICRA), 2023.

[^93]: “HILTI SLAM Challenge 2022 Leaderboard,” 1 2025. [Online]. Available: <https://hilti-challenge.com/leader-board-2022.html>

## [^94]: T. Takasu and A. Yasuda, “Development of the low-cost RTK-GPS receiver with an open source program package RTKLIB,” in International Symposium on GPS/GNSS, vol. 1. International Convention Center Jeju Korea Seogwipo-si, Republic of Korea, 2009

---

![Image](images/96fa90f8852888f14f6ef98637ec5971855bcefb2958945b70319f5bb02fed8f.jpg)

Simon Boche目前正于德国慕尼黑工业大学（TUM）移动机器人实验室（MRL）攻读博士学位，师从Stefan Leutenegger教授。他此前在TUM分别获得了机器人、认知与智能专业的硕士学位以及工程科学专业的学士学位。他的研究兴趣包括多传感器融合、定位与建图，以及移动机器人的自主导航。

![Image](images/ebb5897670d41ff4f01a00a20b55793cde91306de379a1d1fa2d2f5089307592.jpg)

Jaehyung Jung目前是慕尼黑工业大学（TUM）移动机器人实验室（MRL）的博士后研究员。他分别在首尔国立大学获得航空航天工程专业的博士和硕士学位，在釜山国立大学获得航空航天工程专业的学士学位。他的研究兴趣包括状态估计和机器人感知。

![Image](images/1f96ad6a1e99a86818e10e8009cd99c7da95bbd3fde62c0b23536d99aef24020.jpg)

Sebastián Barbas Laina目前正于慕尼黑工业大学（TUM）移动机器人实验室（MRL）攻读博士学位，师从Stefan Leutenegger教授。他此前在瑞典皇家理工学院获得了系统、控制与机器人专业的硕士学位，在马德里理工大学（UPM）获得了以电气工程为方向的工业工程学士学位。他的研究兴趣包括定位与建图、多传感器融合，以及移动机器人的自主导航。

![Image](images/b23e8f2a91f49cd397dcb80c3815ffa1ff7c5bf4097ccc460f23a9a66525b9e5.jpg)

自2025年初以来，斯特凡·洛伊特内格一直是苏黎世联邦理工学院机械与过程工程系的移动机器人学副教授。在此之前，他于2021年作为终身教职助理教授加入慕尼黑工业大学（TUM）。2014年至2020年间，他曾是帝国理工学院的（高级）讲师，目前仍在该校担任荣誉Reader职务。

---

## 补充材料

在补充材料中，我们提供了因篇幅限制而未写入正文的额外细节。这些细节主要涉及在线外参标定的消融研究（第VI-F节）以及利用真实GNSS数据演示GNSS融合的过程（第VI-E节）。

## 在线外参标定——详细内容

在实际应用中，对初始外参误差的鲁棒性至关重要。为了验证OKVIS2-X对初始外参先验扰动的鲁棒性，我们开展了一项额外实验。该实验在第VI-F节的末尾仅做了简要总结，此处将予以详细说明。

在多摄像头与IMU外参标定方面的贡献，体现在Hilti-Oxford数据集中的序列$\exp06$上，该序列包含5个摄像头。这5个摄像头的每个轴$(i=1,2,\cdots,5)$均受到均匀分布扰动，

 $$ \overline{{\boldsymbol{T}}}_{S C_{i}}=\boldsymbol{T}_{S C_{i}}\left[\begin{matrix}\mathrm{E x p}(\delta\alpha_{i})&\delta r_{i}\\ 0_{1\times3}&1\end{matrix}\right], $$

其中$\delta\alpha_i \sim \mathcal{U}(-0.3^\circ, 0.3^\circ)$，

 $$ \delta r_{i}\sim\mathcal{U}(-1\mathrm{c m},1\mathrm{c m}). $$

我们采用主论文中定义的相同符号。例如，$\underline{F}_S$ 和 $\underline{F}_{C_i}$ 分别代表 IMU 和 $i^{\text{th}}$ 相机坐标系。由于我们不知道真实的外参值，因此扰动后的外参 $\overline{T}_{SC_i}$ 是在经过校准的外参 $T_{SC_i}$ 基础上产生的，该校准外参是通过对所有可用测量数据进行完整束调整得到的。图 14a–14e 展示了在 10 次运行中这五个相机到 IMU 外参的收敛情况。大约在 190 秒后出现的突然修正是由于闭环检测提供了额外信息，从而导致外参发生突变。综合 10 次运行的结果来看，在线估计的外参…

![Image](images/e100e37444ad018e0eb89a7242d63eb4f7479e3928a200e8263fd005827e9e89.jpg)

<div style="text-align: center;">(a)</div>

<div style="text-align: center;">(b)</div>

<div style="text-align: center;">(c)</div>

![Image](images/70de15bbde4e70e26054a7ad256d8bdcf151c624c45df8d7c47b21dbffc86a6d.jpg)

![Image](images/07d837f23e2cd6b907913cb764152c5d5f7014db560156b9cfecc697e706f841.jpg)

![Image](images/7999a6229df50989199a6ddeba61a06e9f487b5aec95ed3762615be6c977696d.jpg)

![Image](images/c32fd6c3871112f37ee9ccbaa8a62d12929d5fe5cef316658f77bcde4441fade.jpg)

![Image](images/68d34cc00fcf82701c9166631c0287c16228be91bbefad3f066bd82a1ed925bf.jpg)

![Image](images/21f99392c7271eed5b231578493f0821fcc3285caef1c8ed5039dc1cae033e13.jpg)

![Image](images/ae921282c4295150ca7aee6e77675ed77b59d04b6054e3b2f9af2ab7f28e5f02.jpg)

![Image](images/2ca6dffaed17097645858b0852e8633df9110d0bdf04dade66a1ebb8769a3688.jpg)

<div style="text-align: center;">(d)</div>

![Image](images/d870141492ddd9f4daadc9457f448a7ed9db436b6abb52feaeeaf85b8377a1e8.jpg)

![Image](images/aeb671a8084f5b4abd7cfdc22342c4d75384afae252bfd452f71a150caf9060a.jpg)

![Image](images/fb9247f6ab7f53f9e0993629a0e4494793f418d1e56a920cafae683674da886e.jpg)

![Image](images/397fa49028f531b458b606186c69092ebc3cd56e3dcf6f0a3b4d8812a546571c.jpg)

<div style="text-align: center;">(e)</div>

<div style="text-align: center;">Fig. 14. Camera-IMU extrinsic perturbation study in  $\exp^{06}$, Hilt-Oxford dataset for 5 cameras. Each line represents the difference between online calibration and the post-calibrated extrinsics in 10 independent runs.</div>

---

在 OKVIS2-X 中，序列末尾的标准偏差为 0.08 厘米，且 $0.02^{\circ}$。本研究证明了在线标定方法对初始误差具有较强的鲁棒性。

## GNSS融合——实验详情

首先，我们想详细阐述本文中所采用的GNSS测量紧耦合融合的定义。我们发现，文献中对紧耦合融合的定义并不十分清晰。按照我们的定义，我们遵循[^48]等研究中的表述，即“紧耦合”指的是在非线性优化层面将IMU和视觉等异构传感器模态紧密关联。这意味着，来自不同传感器的残差会在同一个统一的优化问题中被最小化；与之相对的是[^18]等研究中的做法，他们将全局融合作为一个独立的、松耦合的并行位姿图优化过程来处理。

此外，我们希望在第六节E部分进一步阐述相关实验细节，该实验验证了OKVIS2-X处理真实世界且可能存在误差的GNSS数据的能力。该实验采用了GVINS作者在其原始论文中发布的数据集[^50]（<https://github.com/HKUST-Aerial-Robotics/GVINS-Dataset>）。该数据集包含立体相机、惯性测量单元以及ZED-F9P GNSS传感器。作者提供了RTK解算结果以及原始GNSS测量数据（如伪距、多普勒测量值等）。正如我们在论文中所提到的，我们选择基于近3公里长的复杂环境序列进行评估，因为该序列对各类传感器而言挑战性极强：包括光线过强或过暗区域的无特征图像，以及杂乱环境或室内场景下GNSS信号严重受干扰甚至完全缺失的情况（见图11）。我们利用RTKLIB[^94]计算出单点定位（SPP）解算结果，将其作为低端消费级GNSS设备可提供的测量数据，并用作所提出的GNSS融合算法的输入。值得一提的是，我们并未能达到论文中最初所述的SPP精度水平。在有有效RTK解算结果的区域内，SPP解的均方根误差ATE为19.99米，而[^50]中所述的数值为6.036米。即便剔除误差超过40米的异常值后，SPP解的均方根误差ATE仍高达9.01米。表X汇总了OKVIS2-X（采用未经过滤的SPP解算结果）、GVINS以及SPP的最终精度表现。均方根误差ATE是在轨迹完成完整的六自由度对齐后计算得出的。需要注意的是，为了与GVINS进行更公平的比较，评估OKVIS2-X时也未纳入视觉回环闭合机制。此次评估

<div style="text-align: center;">TABLE X</div>

表明：尽管该系统最初设计用于融合间歇性高精度（RTK）GNSS测量数据，但它同样能从低精度GNSS测量数据中获益。

<div style="text-align: center;">RMSE ATE [M] OF THE DIFFERENT APPROACHES IN THE COMPLEX_ENVIRONMENT SEQUENCE.</div>

不过，我们要强调的是，所提出的系统并非宣称在GNSS融合方面具有普遍优越性。我们认可GVINS的出色工作，认为借鉴他们基于原始测量数据（如伪距、多普勒测量值）定义的误差项公式，有望显著改进我们的系统。

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>Causal</td><td style='text-align: center; word-wrap: break-word;'>Non-Causal</td><td style='text-align: center; word-wrap: break-word;'>Final BA</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>RTKLIB (SPP)</td><td style='text-align: center; word-wrap: break-word;'>19.99 $^{1}$</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>-</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>GVINS</td><td style='text-align: center; word-wrap: break-word;'>4.45</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>-</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vi</td><td style='text-align: center; word-wrap: break-word;'>23.72</td><td style='text-align: center; word-wrap: break-word;'>23.72</td><td style='text-align: center; word-wrap: break-word;'>23.74</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours-vig</td><td style='text-align: center; word-wrap: break-word;'>12.51</td><td style='text-align: center; word-wrap: break-word;'>8.47</td><td style='text-align: center; word-wrap: break-word;'>5.12</td></tr></table>

 $^{1}$ RMSE ATE是针对序列中非室内部分进行评估的。