[论文阅读]：An Efficient Schmidt-EKF for 3D Visual-Inertial SLAM

论文阅读：An Efficient Schmidt-EKF for 3D Visual-Inertial SLAM

• [1. Introduction](#1. Introduction)
• [2. Related Work](#2. Related Work)
• [3. Visual-Inertial SLAM](#3. Visual-Inertial SLAM)
• • [3.1. IMU Propagation](#3.1. IMU Propagation)
  • [3.2. Camera Measurement Update](#3.2. Camera Measurement Update)
• [4. Schmidt-EKF based VI-SLAM](#4. Schmidt-EKF based VI-SLAM)
• • [4.1. VIO Features: MSCKF Update](#4.1. VIO Features: MSCKF Update)
  • [4.2. SLAM Features: EKF Update](#4.2. SLAM Features: EKF Update)
  • [4.3. Map Features: Schmidt-EKF Update](#4.3. Map Features: Schmidt-EKF Update)
  • • [4.3.1 Keyframe-aided 2D-to-2D Matching](#4.3.1 Keyframe-aided 2D-to-2D Matching)
    • [4.3.2 Schmidt-EKF Update](#4.3.2 Schmidt-EKF Update)
  • [4.4. Computational Complexity Analysis](#4.4. Computational Complexity Analysis)
• [5. Monte-Carlo Simulation Results](#5. Monte-Carlo Simulation Results)
• [6. Real-World Experimental Results](#6. Real-World Experimental Results)
• • [6.1. Vicon Loops Dataset](#6.1. Vicon Loops Dataset)
  • [6.2. Nighttime Multi-Floor Dataset](#6.2. Nighttime Multi-Floor Dataset)
• [7.Conclusions and Future Work](#7.Conclusions and Future Work)

摘要： 在移动和可穿戴传感器系统中实现厘米级精度定位具有重要的实际应用意义。本文提出了一种新颖的高精度、高效的视觉惯性（VI）SLAM算法，称为Schmidt-EKF VI-SLAM（SEVIS），该算法以紧耦合的方式最优融合IMU测量和单目图像，实现有界误差的三维运动跟踪。具体而言，我们将施密特卡尔曼滤波器（Schmidt Kalman filter）公式进行调整，选择性地将信息丰富的特征包含在状态向量中，并在这些特征变得成熟后将其视为扰动参数（或施密特状态）。这种建模上的变化使得我们不再需要不断更新施密特状态（或其协方差），从而显著节省了计算开销，同时仍然允许EKF正确地考虑它们与活动状态之间的交叉相关性。由此，我们在地图大小方面实现了线性计算复杂度，而不像标准SLAM系统那样呈现二次复杂度。为了充分利用地图信息来限制导航漂移，我们提倡高效的关键帧辅助2D-2D特征匹配，找到当前2D视觉测量与3D地图特征之间的可靠对应关系。所提出的SEVIS在仿真和实验中得到了广泛验证。

1. Introduction

   实现移动和可穿戴设备（如智能手机和微型无人机（MAV））的厘米级定位精度，对于实际应用具有巨大的潜力。提供精确3D导航的最有前景方法之一是通过融合视觉和惯性传感器测量（即视觉惯性导航系统，VINS）【30, 13, 22, 21, 17, 15】。这一定位解决方案的优势在于其成本低且广泛应用，同时能够提供与更昂贵的传感器（如LiDAR）相媲美的定位和姿态估计（精度相当）。目前，针对VINS问题已有多种算法可供选择，包括视觉惯性（VI）SLAM【19, 45】和视觉惯性里程计（VIO）【30, 29, 22】，例如扩展卡尔曼滤波器（EKF）【30, 20, 14, 22, 17, 16, 50, 37】、无迹卡尔曼滤波器（UKF）【10, 4】以及批处理或滑动窗口优化方法【46, 18, 21, 33, 52, 45, 40】。其中，基于EKF的方法由于其高效性，仍然是资源受限设备中最受欢迎的方法。尽管当前的方法在短时间内能够在小规模环境中表现良好（例如，参见【13, 22, 15】），但由于其在感知、内存和计算资源上的限制，若未得到妥善解决，通常会导致任务时长短或实时估计器性能难以实现，从而无法满足长期、大规模部署在挑战性环境中的需求。

   在本文中，我们将主要聚焦于基于EKF的VI-SLAM，而非VIO。VI-SLAM的优势在于它能够构建周围环境的地图，这使得"回环闭合"成为可能，从而限定长期导航漂移。VIO系统则不构建地图，因此无法利用先前观测的信息来帮助提高估计性能。然而，SLAM的最大缺点之一是，其计算复杂度随着地图中特征点数量的增加而呈二次增长，这使得在没有简化假设的情况下，SLAM系统通常无法在资源受限的传感器平台（如移动设备）上实时运行。为了解决这个计算复杂度问题，我们利用计算高效的多状态约束卡尔曼滤波器（MSCKF）【30】并选择性地将若干特征（例如，n 个特征）保留在状态向量中，作为环境地图的一部分，使得系统能够长时间使用这些特征，并从而允许隐式回环闭合以限定漂移。然而，这样做仍然会像标准EKF-based SLAM那样表现出 O ( n 2 ) O(n_2) O(n2) 的计算复杂度。

   通过观察到如果特征接近其稳态（即变得成熟/收敛），其估计不会有显著变化，我们可以通过避免对这些成熟的地图特征执行EKF更新来获得显著的计算节省，同时仍然考虑它们的不确定性。为此，我们采用了施密特卡尔曼滤波器（SKF）【44】，将地图特征视为"扰动参数"，这些特征将不再更新，但它们的协方差和与其他状态的交叉相关性仍然会在EKF更新中得到利用。因此，这使得计算复杂度仅为 O ( n ) O(n) O(n)，使得我们提出的施密特EKF视觉惯性SLAM（SEVIS）能够显著更适合在资源受限的传感器平台上运行。

具体而言，本文的主要贡献包括：

• 我们设计了一种高精度、高效的基于施密特EKF的VI-SLAM算法（即SEVIS），该算法利用施密特卡尔曼滤波器（SKF）公式，使得环境地图的并行估计成为可能，支持长期回环闭合来限定导航漂移，同时保持线性计算复杂度。
• 我们提出了一种基于关键帧辅助的2D到2D匹配方案，用于解决将2D视觉测量与3D地图特征匹配的挑战性数据关联问题，无需执行3D到2D匹配（这对于稀疏的3D环境地图可能不适用）。这种2D到2D匹配不受估计性能的影响，支持长期回环闭合并能够从极端漂移中恢复。
• 我们在蒙特卡罗仿真和实际实验中广泛验证了所提出的SEVIS算法，展示了我们的系统所带来的适用性和性能提升。计算需求的实验研究进一步表明，所提出的SEVIS在构建和维护基于3D特征的地图时仍能保持实时性

2. Related Work

   尽管SLAM估计器通过联合估计传感器平台的位置和周围环境中特征的位置，能够轻松地结合回环闭合约束来限制定位误差，并且在过去三十年里吸引了大量的研究关注【8, 1, 6, 3】，但也有大量的研究致力于开放环路VIO系统（例如【30, 13, 14, 22, 17, 50, 37, 49, 53, 5, 2, 15, 40】）。例如，针对VIO开发了一种混合MSCKF/SLAM估计器【23】，它保留可以在滑动窗口之外持续跟踪的特征，将其作为SLAM特征加入状态中，并在特征丢失时将其移除。

   由于无法在没有简化假设（例如将关键帧位姿和/或地图特征视为完美，即零不确定性）的情况下保持计算高效，因此执行大规模VI-SLAM来实现精确定位具有挑战性。许多方法使用来自不同关键帧的特征观测来限制轨迹上的漂移（例如【34, 21】），其中大多数方法采用双线程架构，优化小窗口的局部关键帧和特征，而后台线程则解决包含回环闭合约束的长期稀疏位姿图【11, 32, 24, 40, 39】。例如，VINS-Mono【40, 39】在局部滑动窗口和全局批优化中都使用回环闭合约束。在局部优化过程中，关键帧的特征观测提供了隐式的回环闭合约束，而问题规模通过假设关键帧位姿是完美的（从而将其从优化中移除）保持较小，同时它们的全局批处理过程优化相对位姿图。在【31】中，提出了一种双层估计器，使用MSCKF执行实时运动跟踪，并在回环闭合检测时触发全局束调整（BA）。这种方法允许以一致的方式重新线性化并纳入回环闭合约束，但需要大量额外的开销时间，因为滤波器必须等待束调整完成。一种基于大规模地图的VINS【25】假设存在一个压缩的先验地图，其中包含特征位置及其不确定性，并通过与先验地图中的特征匹配来约束全局定位。然而，最近的Cholesky-Schmidt-KF【9】显式考虑了先验地图的不确定性，采用地图信息矩阵的稀疏Cholesky因子，并通过将地图划分为更多子地图以提高效率来进一步放松不确定性。与此不同，本文提出了一种单线程Schmidt-EKF VI-SLAM方法，允许在不牺牲实时性能的情况下完全概率性地融合测量数据，并支持构建和利用环境地图以无限制地限制长期导航漂移。

3. Visual-Inertial SLAM

   VI-SLAM的过程通过优化融合相机图像和IMU（陀螺仪和加速度计）测量数据，提供传感器平台的6自由度姿态估计，并重建环境特征的三维位置（地图）。在本节中，我们简要描述了在EKF框架下的VI-SLAM，该框架作为我们提出的SEVIS算法的基础。

   VI-SLAM的状态向量包含IMU导航状态 x I x_I xI和滑动窗口中的历史IMU（或相机）位姿 x C x_C xC，如同在MSCKF【30】中所定义的那样，以及以全局坐标系表示的地图特征的位置 x S x_S xS:

其中， G I k q ‾ {G}^{I{k}}\overline{q} GIkq 是单位四元数，用于参数化从全局参考系 {G} 到IMU局部参考系 { I k I_k Ik} 的旋转 ,在 k k k时刻的旋转【47】； b ω b_ω bω和 b a b_a ba 分别是陀螺仪和加速度计的偏置， G v I k ^{G}v_{I_{k}} GvIk和 G p I k ^{G}p_{I_{k}} GpIk 分别是IMU在全局坐标系中表示的速度和位置。克隆状态 x C x_{C} xC包含在滑动窗口中的 m m m个历史IMU位姿，而地图状态 x S x_{S} xS包含 n n n 个特征。通过状态分解（公式(1)），相应的协方差矩阵可以分块表示为：

3.1. IMU Propagation

   惯性状态 x I x_{I} xI使用传入的IMU测量数据（线性加速度 a m a_m am和角速度 ω m ω_m ωm）根据以下通用的非线性IMU运动学方程进行传播【7】：

其中， n a n_a na和 n ω n_ω nω是IMU测量的零均值高斯白噪声。我们在当前估计值处对该非线性模型进行线性化，然后通过EKF公式将状态协方差矩阵向前传播至下一时刻：

其中， Φ k − 1 Φ_{k-1} Φk−1是系统的雅可比矩阵， Q k − 1 Q_{k-1} Qk−1是活动状态的离散噪声协方差矩阵【30】。由于随着协方差矩阵的大小或IMU采样率（例如大于200Hz）增长，重复计算上述协方差传播可能变得计算上不可行，因此我们采用以下方法对状态转移矩阵和噪声协方差进行累积

并设定初始条件： Φ ( i = 0 ) = I Φ(i=0)=I Φ(i=0)=I 和 Q ( i = 0 ) = 0 Q(i=0)=0 Q(i=0)=0。在累积了 Φ ( i + 1 ) Φ(i+1) Φ(i+1) 和 Q ( i + 1 ) Q(i+1) Q(i+1) 后，我们直接应用它们来传播 P k − 1 ∣ k − 1 P_{k−1∣k−1} Pk−1∣k−1。

3.2. Camera Measurement Update

   假设相机经过标定，时刻 k k k 的角点特征测量为三维点 C k P f i C_{k}P_{f}^{i} CkPfi在当前相机坐标系 { C k C_{k} Ck} 上的透视投影到图像平面上，即：

其中， n f n_{f} nf是协方差为 R f R_f Rf的零均值白噪声。在（11）式中，{ I C q ‾ , C P I {^{C}{I}\overline{q},} \quad {^{C}P{I}} ICq,CPI}是相机和IMU之间的外部旋转和平移，这些变换可以通过离线进行相机-IMU外部标定获得【28】。对于EKF的使用，我们对公式（10）进行线性化，得到以下残差：

其中， H k H_{k} Hk通过如下方式计算（假设 i=1）：

一旦计算出测量雅可比矩阵和残差，我们就可以应用标准的EKF更新方程来更新状态估计和误差协方差【26】。

4. Schmidt-EKF based VI-SLAM

   已知EKF状态估计和协方差更新的计算复杂度与地图特征的数量呈二次关系【38】，这使得VI-SLAM的直接实现对于实时运行来说过于昂贵。利用SKF【44】，我们提出了一种新的施密特EKF VI-SLAM（SEVIS）算法，减轻了这种二次复杂度。其关键思想是选择性地将地图特征视为状态向量中的"扰动参数"（即施密特状态 $x_{s}$），这些特征的均值和协方差将不再更新，但它们与活动状态$x_{A}$的交叉相关性仍然被利用并更新。

   特别地，所提SEVIS的IMU传播与标准EKF（第3.1节）中的处理完全相同。接下来，我们主要聚焦于使用单目图像的更新，这是我们SEVIS的核心，但该方法也很容易扩展到立体系统。随着相机-IMU传感器对在环境中的移动，特征通过基于描述子的跟踪进行追踪。首先检测FAST特征41，并为每个特征提取ORB描述子43。使用OpenCV【36】的"BruteForceHamming"KNN描述符匹配器来寻找匹配关系，之后我们执行比率测试（通过前两个返回值）以确保有效匹配，并使用8点RANSAC来剔除额外的离群点。一旦找到视觉轨迹，我们使用三种类型的跟踪特征来高效地更新状态估计和协方差：（i）VIO特征，这些特征是偶然的，仅能在短时间内跟踪；（ii）SLAM特征，这些特征比VIO特征更稳定，可以在当前滑动窗口之外进行跟踪；（iii）地图特征，这些是成熟且信息丰富的SLAM特征，在施密特状态中保留，持续时间不确定。

4.1. VIO Features: MSCKF Update

   对于那些在当前窗口中失去活跃跟踪的特征（称为VIO特征），我们执行标准MSCKF更新【30】。特别地，我们首先通过束调整（BA）对这些特征进行三角化，以计算特征的雅可比矩阵 H f H_{f} Hf（参见公式(14)），然后将 r k r_{k} rk（参见公式(13)）投影到 H f H_{f} Hf的左零空间（即 N T H f = 0 N^{T}H_{f}=0 NTHf=0)，以得到与特征无关的测量残差：

其中， H x H_{x} Hx是关于当前滑动窗口中导航状态的测量雅可比矩阵堆叠而成的， R f ′ = N T R f N R_{f}^{'}=N^{T}R_{f}N Rf′=NTRfN是推导出的噪声协方差【30】。

4.2. SLAM Features: EKF Update

   对于那些可以在当前滑动窗口之外可靠跟踪的特征，我们将其初始化到活动状态中，并按照标准的基于EKF的VI-SLAM更新方式进行处理（参见第3.2节）。需要注意的是，SLAM特征不会永远保持活跃，而是会被移动到施密特状态，作为扰动参数（参见第4.3节），或者如【23】中所述，为了节省计算成本，将其边缘化处理。

4.3. Map Features: Schmidt-EKF Update

   如果我们像在MSCKF【30】中那样，通过线性边缘化特征来执行VIO【51】，尽管能够实现高效性，但导航误差可能会无限增长；另一方面，如果通过持续维护状态中的特征（地图）来执行完整的VI-SLAM，尽管能够获得更高的精度，但计算成本可能会变得难以承受。特别是，SLAM中需要解决两个关键挑战：（i）由于地图特征数量的增加而导致的计算复杂度增长；（ii）在状态向量中检测活动跟踪特征是否与先前映射的特征匹配的数据关联问题。这促使我们设计了SEVIS算法，通过SKF构建环境的稀疏特征地图，在保持必要效率的同时防止长期漂移。

4.3.1 Keyframe-aided 2D-to-2D Matching

   为了解决数据关联的挑战，对于状态向量中已经包含的地图特征的三维位置，一种直接的方法是通过3D到2D的投影（即将3D地图特征投影到当前帧上），来找到当前视觉测量与映射特征的对应关系，这在文献中被广泛使用（例如【9, 25】）。然而，在典型的SLAM场景中，估计3D点特征的地图并将其与当前特征匹配通常是稀疏的。例如，我们发现，在一个具有多层室内环境的场景中，即使地图中有多达600个特征，也只有大约10个特征可以成功投影回活动帧。此外，如果状态估计中存在不可忽略的漂移（这在实践中是不可避免的），则投影特征可能无法与当前图像观察到的空间区域对应，从而无法利用地图信息来减少导航误差。

   基于这些原因，我们提倡使用2D到2D匹配进行数据关联，借助"关键帧"来观察环境中先前的区域。这种方法能够提供高质量的估计，并且不受漂移的影响。每个关键帧包含与状态向量中地图特征对应的部分提取特征，因此，如果我们将活动特征轨迹与先前的关键帧进行匹配，就可以找到新跟踪特征与状态中先前映射特征之间的对应关系。

   具体来说，我们首先查询关键帧数据库，以检索与当前帧最接近的关键帧。为此，可以使用不同的位置识别方法，例如DBoW2【12】和CALC【27】，来找到最佳候选关键帧。在检索后，我们通过确保可以计算当前帧与候选关键帧之间的基础矩阵，执行额外的几何验证，这种方法在数据库中能够提供极高的关键帧匹配质量。在检索到匹配的关键帧后，我们基于描述子从当前帧的特征到关键帧的所有提取特征进行匹配，并随后使用8点RANSAC剔除离群点。我们现在可以得到当前帧特征轨迹与关键帧地图特征之间的对应关系。图1可视化了这一过程。

4.3.2 Schmidt-EKF Update

   为了在执行SLAM的同时利用地图约束限制导航误差并实现显著的计算节省，我们采用SKF方法【44】，将地图特征视为扰动参数，仅跟踪它们与活动状态之间的交叉相关性，同时在更新过程中允许对它们进行概率性利用。具体而言，我们计算施密特EKF的增益矩阵如下：

其中， H A H_{A} HA和 H s H_{s} Hs分别是与活动状态和施密特状态特征相关的测量雅可比矩阵（参见公式(13)）， S k S_{k} Sk 是残差协方差：

   为了减少计算复杂度，我们不更新地图特征的扰动参数（施密特状态），因此，如SKF中所述，我们将与施密特状态对应的增益设为零，即 $$。结果是，状态估计更新如下：

协方差通过其分块形式高效地更新：

到此为止，我们已完全利用当前相机测量信息来更新SEVIS状态估计和协方差（参见公式(1)和公式(5)）。所提出SEVIS的主要步骤概述如算法1所示

4.4. Computational Complexity Analysis

   在这里，我们通过详细的分析展示了所提出的SEVIS的计算效率，表明其复杂度与地图特征的数量呈线性关系。此效率将在第6节的实验数据中进一步展示。
Propagation: 传播的主要计算成本来自于矩阵乘法 Φ k − 1 P A S k − 1 ∣ k − 1 Φ_{k-1}P_{AS_{k-1|k-1}} Φk−1PASk−1∣k−1（参见公式(7)），其中 Φ k − 1 Φ_{k−1} Φk−1​是一个方阵，大小为 dim( x A x_A xA),而 P A S k − 1 ∣ k − 1 P_{AS_{k-1|k-1}} PASk−1∣k−1是一个胖矩阵，大小为 O ( n ) O(n) O(n)。由于地图特征的数量远远超过活动状态的大小，因此总成本为 O ( n ) O(n) O(n)。
Update: Propagation之后，我们通过将传播的状态附加到活动克隆状态 x C x_{C} xC上来扩展状态。这是一个 O ( n ) O(n) O(n) 的计算，因为我们只需要在活动协方差矩阵 p A A k p_{AA_{k}} pAAk上附加一行一列，然后在施密特交叉相关项 p A S k p_{AS_{k}} pASk上附加一行，最终得到一个 O ( n ) O(n) O(n) 的操作。SLAM特征初始化遵循相同的逻辑，也是一个 O ( n ) O(n) O(n) 的操作。在更新过程中，直接计算的操作可能会导致计算成本达到 O ( n 2 ) O(n^2) O(n2) ，尤其是在当前帧与地图中所有特征同时匹配的情况下。然而，仔细检查公式(20)会发现，如果 H S k H_{S_{k}} HSk 的大小为 O ( n ) O(n) O(n)，那么计算 S k S_{k} Sk将是 O ( n 2 ) O(n^2) O(n2) 。然而，由于实际环境的广阔和视点的限制，这种情况在实践中并不常见。因此，我们将每次更新中使用的特征数量限制在远低于 n n n的数量，额外的特征可以在未来的时刻处理，从而使得每个时间步的计算复杂度保持在 O ( n ) O(n) O(n) 。
Management: 我们将矩阵 P A A k P_{AA_{k}} PAAk、 P A S k P_{AS_{k}} PASk和 P S S k P_{SS_{k}} PSSk作为独立实体进行管理，并预分配 P S S k P_{SS_{k}} PSSk至允许的最大特征数，以防止内存分配操作带来的额外开销。当将状态从活动状态 x A x_{A} xA移动到施密特状态 x S x_{S} xS时，我们特别小心确保该操作保持 O ( n ) O(n) O(n) 级别的复杂度.具体来说，我们首先将 P A A k P_{AA_{k}} PAAk中相关的列块复制到 P S A k P_{SA_{k}} PSAk的最后一列，然后将 P A S k P_{AS_{k}} PASk 中相关的行块复制到预分配的 P S S k P_{SS_{k}} PSSk的最后一行和最后一列，从而得到一个总成本为 O ( n ) O(n) O(n) 的操作。对活动状态进行边缘化是 O ( n ) O(n) O(n) 的，因为它需要从 P A S k P_{AS_{k}} PASk矩阵中删除一行，这可以通过将所有剩余行复制到被删除的行上并覆盖被删除的条目来实现。在从施密特状态 P S S k P_{SS_{k}} PSSk 中边缘化地图特征时，我们通过用最后插入的地图特征覆盖要删除的状态对应的行和列，从而使操作保持在 O(n)计算复杂度。

5. Monte-Carlo Simulation Results

   为了验证所提出的SEVIS后端估计引擎，我们首先通过已知的测量-特征对应关系，进行蒙特卡洛仿真，模拟视觉-惯性SLAM，其中一个单目视觉惯性传感器平台沿着一个圆形轨迹在一个圆柱形区域内移动，并观察一系列环境特征。传感器和轨迹的仿真参数列在表1中。

   特别地，我们比较了三种VINS算法，以揭示所提出的SEVIS的优势：（i）基础的VIO方法，它由MSCKF和6个SLAM特征组成（参见【23】）。这些SLAM特征在离开视野时被显式地边缘化。（ii）基础的SLAM方法，它使用相同的MSCKF窗口，但增加了90个SLAM特征。不同于上述VIO，这些SLAM特征永远不会被边缘化，因此可以用于（隐式）回环闭合。（iii）所提出的SEVIS算法，它由与基础VIO相同的MSCKF窗口和6个SLAM特征组成，同时增加了90个地图特征，这些特征被建模为扰动参数。当SLAM特征离开视野时，它们被移入施密特状态，成为地图特征，如算法1所述。

   图2展示了50次蒙特卡洛仿真运行的平均根和方差误差（RSSE）性能。正如预期，基础VIO随着时间的推移在方位和位置上积累了漂移，而基础SLAM提供了有界误差性能，并且没有长期漂移。值得注意的是，位置RSSE会略微振荡，具体取决于相对于初始回环闭合的位置。这是因为EKF在纠正这些误差时能力有限，因为它无法像基于优化的方法那样重新线性化过去的测量【48】。更重要的是，显然所提出的SEVIS算法也没有积累长期漂移，尽管其精度略低于基础SLAM。然而，考虑到SEVIS在地图特征数量方面的计算复杂度为线性，而基础SLAM的复杂度为二次，精度的这种降低是一个值得接受的代价。

6. Real-World Experimental Results

   我们进一步评估了基础的MSCKF-based VIO（没有地图特征）、基础的完整VI-SLAM和所提出的SEVIS在真实数据集上的表现。接下来，我们首先检查估计器的精度和计算开销，然后在一个具有挑战性的夜间多层数据集上评估这些系统，结果表明所提出的SEVIS能够稳健地扩展到现实应用中。

6.1. Vicon Loops Dataset

   我们首先在Vicon回环数据集【21】上验证了所提出的系统，该数据集覆盖1.2公里的单一房间，收集时长为13分钟。一个手持VI传感器【35】提供了灰度立体图像对和惯性信息，同时使用Vicon运动跟踪系统以200Hz的频率捕捉完整的6自由度地面真值。地图特征的最大数量设置为600个，以确保在整个轨迹上的实时性能，图像以0.5Hz的频率插入到查询关键帧数据库中，并且在活动状态下最多同时使用5个SLAM特征。展示的结果包括三种不同配置：（i）基础VIO方法，增加了5个SLAM特征，（ii）基础VI-SLAM方法，增加了600个SLAM/地图特征，以及（iii）所提出的SEVIS方法，增加了600个地图特征，并利用施密特公式实现计算增益。

   我们使用两种不同的误差度量评估了所提出的方法：绝对轨迹误差（ATE）和相对误差（RE）。有关这些误差度量的详细定义，读者可以参考【54】。除了我们的基准方法和所提出的方法外，我们还评估了VINS-Mono【40, 39】来提供与当前利用回环闭合信息的最先进方法的比较。如表2和图4所示，所提出的SEVIS能够以高精度进行定位，并且在完整基础VI-SLAM系统的水平上执行。从RE来看，显然地图特征的纳入防止了长期漂移，并提供了更高的精度，表现在随着轨迹段长度增加，RE几乎保持恒定。所提出的SEVIS提供了一个计算上可行的滤波器，具有与完整基础VI-SLAM相似的精度，并且与VINS-Mono的性能竞争（尽管VINS-Mono利用了批优化）。

   所提出的SEVIS算法相比于完整协方差SLAM的主要优势在于计算复杂度的降低。这一点在不同算法的运行时间中得到了显著体现。如图5所示，我们评估了三种系统并收集了我们的实现的计时统计数据。所提出的SEVIS能够保持实时性能（20Hz相机意味着我们需要将总计算时间保持在0.05秒以内），而基础VI-SLAM方法在使用600个地图特征时，更新峰值超过了计算限制的四倍。这是由于完整协方差更新的复杂度为 O ( n 2 ) O(n^2) O(n2)。需要注意的是，在传播阶段会有额外的开销，因为协方差矩阵的对称性需要在整个矩阵中强制执行，而不仅仅是活动元素，以确保数值稳定性。

6.2. Nighttime Multi-Floor Dataset

   我们进一步在一个具有挑战性的室内夜间多层数据集上验证了所提出的系统，该数据集面临多个挑战，包括低光环境、长曝光时间、低对比度图像以及不适合进行特征提取的运动模糊（见图6）。如果可以提取特征，生成的描述符匹配效果较差，原因是噪声较大且梯度较小。与Vicon回环数据集相比，在更新过程中使用了更多的离群点，导致估计器跳跃较大并出现不正确的修正。我们强调，所提出的SEVIS在这些场景下能够恢复，因为借助关键帧辅助的2D到2D匹配对估计漂移或不良估计器表现不敏感，并且地图特征的更新可以正确地修正并防止不正确的漂移。

   我们使用Realsense ZR300传感器【3】收集了20分钟的灰度单目鱼眼图像和惯性读数，轨迹跨越了两层楼，总长1.5公里。我们还执行了相机到IMU的在线标定，以进一步优化制造商驱动程序提供的变换。最多允许700个地图点以确保足够覆盖地图区域，关键帧以4Hz的频率插入查询数据库，以确保所有地图特征的充分覆盖，同时每次活动状态下最多使用2个SLAM特征。基础VIO和所提出的SEVIS生成的轨迹如图7所示。显然，地图特征的纳入防止了基础VIO所经历的长期漂移，后者在偏航和z轴方向上显示出较大的误差。由于此数据集没有地面真值，按照常规做法，我们计算了轨迹的起止误差，理想情况下应为零，因为传感器平台已返回到起始位置。基础VIO的误差为4.67米（占轨迹距离的0.31%），而所提出的SEVIS的误差仅为0.37米（占轨迹距离的0.02%）。

7.Conclusions and Future Work

   在本文中，我们开发了高精度、高效的SEVIS算法，它采用了SKF公式用于长期视觉惯性SLAM。特别地，SEVIS中地图特征的概率性纳入允许实现有界导航漂移，同时保持线性计算复杂度。为此，当前视觉测量与3D地图特征的关键帧辅助2D到2D特征匹配极大地促进了地图信息的充分利用。我们进行了广泛的蒙特卡洛仿真和实际实验，结果表明，地图特征的纳入大大提高了长期精度，而所提出的SEVIS仍然能够在不影响估计器性能的情况下保持实时性能。未来，我们将研究如何改进地图特征的质量，以支持长期定位，并进一步评估我们的系统在资源受限的移动传感器平台上的表现。