摘要
近年来，视觉惯性里程计技术取得了优异的成果，旨在用高精度和鲁棒计算传感器的增量运动。然而，这些方法缺乏闭环能力，即使传感器不断重新访问同一个地方，轨迹也会积累漂移。在这项工作中，提出了一种新型的紧耦合视觉惯性同步定位和构图系统，可以闭环重用其地图，实现已构图区域的零漂移定位。虽然该方法可以应用于任何相机配置，但它解决了单目相机最常见的问题，其规模模糊。还提出了一种新颖性 IMU 初始化方法可在几秒钟内计算陀螺仪和加速度计的尺度、重力方向、速度和偏差。在最近的微型飞机公共数据集中 11 实现了序列中的测试系统 1% 典型的比例因子误差和厘米精度。将重访与序列中视觉惯性里程计的最新技术进行比较，证明该方法具有更好的准确性，因为地图重用和无漂移积累。
视觉惯性预积分
视觉惯性ORB-SLAM输入是一系列惯性测量单元测量值和单目摄像机帧。考虑投影函数 π \pi π : R 3 \mathbb{R}^{3} R3→?传统针孔摄像机模型将摄像机参考C中的3D点XC ∈ R3转换为平面xc∈?? R 2 \mathbb{R}^{2} R2上的2D点: 
其中，[fu fv]T为焦距，[cu cv]T投影函数是原点偏移，不考虑相机镜头的畸变。当提取图像上的关键点时，它们的坐标不会被破坏，因此它们可以使用投影函数来匹配投影点。
惯性测量单元(参考B)以规则间隔t测量传感器的加速度aB和角速度ωB，通常是几百赫兹。除传感器噪声外，这两种测量还分别受到加速度计和陀螺仪缓慢变化的偏置ba和bg影响。此外，加速度计受重力影响，需要减去重力gW计算运动的影响。惯性测量单元的方位RWB∈s0(3)、位置WpB和速度WvB世界参考系W中的离散演变可计算如下：
根据中间所有测量，两个连续关键帧之间的运动可以预融合R、?v和?p定义On-Manifold Preintegration for Real-Time Visual-Inertial Odometry本文描述的最新惯性测量单元预积分： 其中雅可比 J ( ) a J_{()}^{a} J()a和 J ( ) a J_{()}^{a} J()a说明了在隐式重新计算预积分的情况下改变偏差的效果的一阶近似。当惯性测量单元测量值到达时，预积分和雅可比矩阵都可以有效地迭代计算。摄像机和IMU被认为是刚性连接的，转换矩阵TCB=[RCB | CpB] 在其校准已知的参考系统之间 。
ORB-SLAM视觉惯性系统
1.跟踪
视觉惯性跟踪负责以帧速率跟踪传感器姿态、速度和IMU偏差。这能够非常可靠地预测相机姿势，而不是像原始单目系统那样使用特殊的运动模型 。预测摄影机姿势后，将投影局部地图中的地图点并与帧上的关键点匹配。然后，通过最小化所有匹配点的特征重投影误差和IMU误差项来优化当前帧 j 。如图2所示，根据本地建图或闭环线程是否更新地图，该优化是不同的。 当在地图更新之后执行跟踪时（图2a），IMU误差项将当前帧 j 与最后一个关键帧 i 链接：
其中，给定匹配 k 的特征重投影错误Eproj定义如下： 其中 x k x^{k} xk是图像中的关键点位置， x w k x_{w}^{k} xwk是世界坐标中的地图点， ∑ k \sum{k} ∑k是与关键点比例相关联的信息矩阵。IMU误差项 E I M U E_{IMU} EIMU为：
式中， ∑ I \sum{I} ∑I是预积分的信息矩阵 ， ∑ R \sum{R} ∑R随机游走偏差， ρ \rho ρ是鲁棒核函数。使用高斯牛顿法优化后（图2b），得到的估计和Hessian矩阵用作下一次优化的先验。 假设没有地图更新（图2c），下一帧 j+1 将通过链接到帧 j 并使用在上一优化结束时计算的先验值进行优化（图2b）： Eprior是一个先验值， 其中 (¯·) 和 ∑ p \sum{p} ∑p是先前优化产生的估计状态和 Hessian 矩阵（图 2b）。在此优化之后（图 2d），帧 j 被边缘化。这种链接两个连续帧并使用先验的优化被重复（图 2e-f），直到地图发生变化，此时先验将不再有效，并且跟踪将再次将当前帧链接到最后一个关键帧（图 2a）。请注意，图 2 (e-f)是优化，始终在局部模式下执行，因为地图没有更新。
局部建图
局部建图线程在插入新的关键帧后执行局部 BA。它优化了最后 N 个关键帧（局部窗口）和这 N 个关键帧看到的所有点。所有其他共享局部点观察的关键帧（即在covisibility graph 中连接到任何局部关键帧），但不在局部窗口中，对总代价有影响，但在优化期间是固定的（固定窗口）。关键帧 N + 1 始终包含在固定窗口中，因为它约束 IMU 状态。图 3 说明了原始 ORB-SLAM 和视觉惯性 ORB-SLAM 中的局部 BA 之间的差异。 代价函数是 IMU 误差项和重投影误差项的组合。与仅视觉相比，视觉惯性版本更复杂，因为每个关键帧有 9 个额外的状态（速度和偏差）需要优化，必须为实时性能选择合适的局部窗口大小。
局部建图还负责关键帧管理。原始的 ORB-SLAM 策略丢弃了冗余关键帧，因此如果在建图良好的区域进行定位，则地图大小不会增加。这个策略在使用 IMU 信息时是有问题的，它限制了连续关键帧的运动。连续关键帧之间的时间差异越长，IMU 提供的信息越弱。因此，如果不会使局部 BA 的窗口中的两个连续关键帧相差超过 0.5 秒，允许地图丢弃冗余关键帧。为了能够执行完整的 BA，在闭环之后或随时优化地图，不允许任何两个连续的关键帧相差超过 3 秒。如果关闭带有 IMU 约束的全局 BA，只需要限制局部窗口中关键帧之间的时间偏移。
闭环
当返回到已建立地图的区域时，闭环线程旨在减少探索过程中累积的漂移。位置识别模块将最近的关键帧与过去的关键帧进行匹配。通过计算刚体变换来验证此匹配，刚体变换将对齐关键帧之间的匹配点。最后进行优化，以减少轨迹中累积的误差。 这种优化在大型地图中可能非常占用资源，因此选择执行姿势图优化，这降低了复杂性。因为忽略了结构，并且表现出良好的收敛性。
与原始ORB-SLAM不同，在6个自由度（DoF）上执行姿势图优化，而不是7个自由度，因为可以观察到比例尺度。这个位姿图忽略了 IMU 信息，没有优化速度或 IMU 偏差。因此，通过根据相关关键帧的校正方向旋转它们来校正速度。虽然这不是最优的，但偏差和速度应该是局部准确的，以便在位姿图优化后立即继续使用 IMU 信息。之后在并行线程中执行完整的 BA，优化所有状态，包括速度和偏差。
IMU初始化
提出了一种方法来计算尺度、重力方向、速度和 IMU 偏差的视觉惯性全 BA 的初始估计，给定一组由单目 SLAM 算法处理的关键帧。这个想法是让单目 SLAM 运行几秒钟，假设传感器执行的运动使所有变量都可以观察到。虽然我们建立在 ORB-SLAM上，但可以使用任何其他 SLAM。唯一的要求是任何两个连续的关键帧在时间上接近，以减少 IMU 噪声集成。
初始化分为更简单的子问题：（1）陀螺仪偏差估计，（2）尺度和重力近似，考虑没有加速度计偏差，（3）加速度计偏差估计，以及尺度和重力方向细化，以及（4）速度估计。
陀螺仪偏差估计
陀螺仪偏差可以仅从两个连续关键帧的已知方向估计。假设偏差变化可以忽略不计，优化恒定偏差bg，从而使所有连续关键帧对的陀螺仪积分和从ORB-SLAM计算的相对方向之间的差异最小化： 其中N是关键帧的数量。 R W B = R W C R C B R_{WB}=R_{WC}R_{CB} RWB=RWCRCB, R W C R_{WC} RWC是由ORB-SLAM计算的旋转矩阵， R C B R_{CB} RCB是相机的标定矩阵。∆ R i , i + 1 R_{i, i+1} Ri,i+1是两个连续关键帧之间的陀螺仪积分。 使用零偏差种子的高斯牛顿来求解，类似表达式的解析雅可比矩阵可在On-Manifold Preintegration for Real-Time Visual-Inertial Odometry论文中找到。
尺度和重力近似（无加速计偏差）
一旦估计了陀螺仪偏差，就可以对速度和位置进行预积分，正确旋转加速度测量值以补偿陀螺仪偏差。 ORB-SLAM 计算的相机轨迹的尺度是任意的。因此在相机坐标系B和IMU坐标系C之间转换时需要包含一个尺度因子s，即： 将 上式代入与两个连续关键帧的位置相关的方程，并忽略此时的加速度计偏差，如下： 目标是通过求解这些变量的线性方程组来估计 s 和 gW。为了避免求解 N 个速度并降低复杂度，考虑三个连续关键帧之间的两个关系 ，并使用速度关系，得到以下表达式： 为了清晰表示，将关键帧i、i+1、i+2写为1、2、3， 然后，将三个连续关键帧的所有关系堆叠到一个系统中 A 3 ( N − 2 ) ∗ 4 x 4 ∗ 1 = B 3 ( N − 2 ) ∗ 1 A_{3\left ( N-2 \right )*4}x_{4*1}=B_{3\left ( N-2 \right )*1} A3(N−2)∗4x4∗1=B3(N−2)∗1,它可以通过奇异值分解（SVD）得到比例因子 s s s 和重力向量 g W g_{W} gW ,注意，因为有3（N− 2)个 方程和4个未知数，因此至少需要4个关键帧。
加速计偏差估计、尺度和重力方向细化
到目前为止，在计算标度和重力时，还没有考虑加速计偏差。为了提高可观测性，引入了迄今为止没有考虑的新信息，即重力大小G。 考虑具有重力方向的惯性参考 I， g I ^ \hat{g_{I}} gI^ = {0, 0, −1}。和已经计算的重力方向 g W ^ = g W ∗ ∥ g W ∗ ∥ \hat{g_{W}}=\frac{g_{W}^{*}}{\left \| g_{W}^{*} \right \|} gW^=∥gW∗∥gW∗,可以通过下式计算旋转 RWI： 现在将重力矢量表示为： 其中 RWI 可以在 I 中仅使用围绕 x 和 y 轴的两个角度进行参数化，因为与重力对齐的围绕 z 轴的旋转对 gW 没有影响。这种旋转可以使用扰动 δθ 来优化： 其一阶近似 将 (17) 代入 (11) 并且现在包括加速度计偏差的影响，得到： 考虑如（12）中的三个连续关键帧，可以消除速度并得到以下关系： 其中， 其中 [ ] ( : , 1 : 2 ) \left [ \right ](:,1:2) [](:,1:2)表示矩阵的前两列。堆叠三个连续关键帧之间的所有关系，形成了一个线性方程组 A 3 ( N − 2 ) ∗ 6 x 6 ∗ 1 = B 3 ( N − 2 ) ∗ 1 A_{3\left ( N-2 \right )*6}x_{6*1}=B_{3\left ( N-2 \right )*1} A3(N−2)∗6x6∗1=B3(N−2)∗1,可以通过 SVD 求解得到尺度比例因子 s∗、重力方向校正 δ θ x y ∗ \delta \theta _{xy}^{*} δθxy∗和加速度计偏差 b a ∗ b{_{a}^{*}} ba∗。在这种情况下，有 3(N - 2) 个方程和 6 个未知数，再次需要至少 4 个关键帧来求解系统。
可以计算条件数（即最大奇异值和最小奇异值之间的比率）来检查问题是否条件良好（即传感器执行了使所有变量都可观察到的运动）。可以重新线性化并迭代解决方案，但实际上发现第二次迭代不会产生明显的改进。
速度估计
在方程（12）和（19）中考虑了三个连续关键帧的关系，因此得到的线性系统没有对应于速度的 3N 个附加未知数。现在可以使用等式 (18) 计算所有关键帧的速度，因为比例、重力和偏差是已知的。为了计算最近关键帧的速度，使用（3）中的速度关系。
重定位后的偏置重新初始化
当系统在很长一段时间后重新定位时，使用位置识别，通过求解 (9) 重新初始化陀螺仪偏差。加速度计偏差是通过求解简化的 (19) 来估计的，其中唯一未知的是偏差，因为规模和重力是已知的。使用仅用视觉定位的 20 个连续帧来估计两个偏差。
实验
评估了所提出的 IMU 初始化方法，在 EuRoC 数据集中的 Visual-Inertial ORB-SLAM。EuRoC 数据集包含从微型飞行器 (MAV) 记录的 11 个序列，在两个不同的房间和一个工业环境中飞行。根据照明、纹理、快/慢动作或运动模糊，序列分为简单、中等和困难。该数据集提供了 20Hz 的同步全局快门 WVGA双目图像，以及 200Hz 的 IMU 测量和轨迹地面实况。这些特性使其成为测试视觉惯性 SLAM 系统的真正有用的数据集。实验仅在具有 8Gb RAM 的 Intel Core i74700MQ 计算机中进行处理左侧图像。
IMU初始化
在序列 V2 01 easy 中评估 IMU 初始化。每次 ORB-SLAM 插入关键帧时，都会从头开始初始化。以较低的帧速率运行序列，以便重复初始化不会干扰系统的正常行为。目标是在初始化算法处理更多关键帧（即更长的轨迹）时分析变量的收敛性。图 4 显示了估计的规模和 IMU 偏差。 可以看出，在 10 到 15 秒之间，所有变量已经收敛到稳定值，并且估计的比例因子确实接近其最佳值。通过相似变换将估计的轨迹与 groundtruth 对齐来计算最佳比例因子。图 4 还显示了 (19) 的条件数，表明需要一些时间才能得到一个条件良好的问题。这证实了传感器必须执行使所有变量都可观察到的运动，尤其是加速度计偏差。图 4 中的最后一行显示了初始化算法所花费的时间，呈现出线性增长。这种复杂性是在 (12) 和 (19) 中不包括速度的结果，当使用 SVD 求解这些系统时，这将导致二次复杂度。将初始化细分为更简单的子问题会产生一种非常有效的方法。
提出的初始化允许开始融合 IMU 信息，因为重力、偏差、尺度和速度是可靠估计的。对于 EuRoC 数据集，观察到 15 秒的 MAV 探索总是给出准确的初始化。作为未来的工作，作者想研究一个自动标准来决定何时可以认为初始化成功，因为我们观察到条件数的绝对阈值不够可靠。
SLAM 评估和与比较
评估了视觉惯性 ORB-SLAM 在 EuRoC 数据集的 11 个序列中的准确性。当 MA V 开始探索时，开始处理序列。局部 BA 的局部窗口大小设置为 10 个关键帧，IMU 初始化在单目 ORB-SLAM 初始化 15 秒后执行。系统在 IMU 初始化后立即执行完整的 BA。表 I 显示了每个序列的关键帧轨迹的平移均方根误差 (RMSE)。 使用原始 Vicon 和 Leica 地面实况作为已使用 IMU 的后处理。观察到视觉惯性传感器与 Vicon Room 2 序列中 -0.2 秒和机器大厅中 0.2 秒的原始地面实况之间的时间偏移，在对齐两条轨迹时进行了校正。还测量了理想的比例因子，以最佳地对齐估计的轨迹和地面实况。这个比例因子可以看作是轨迹和重建的残余比例误差。系统成功地实时处理了所有这些序列，除了 V1 03 困难，运动太极端，单目系统的生存时间为 15 秒。
系统能够以米制比例恢复运动，比例误差通常低于 1%，在 30 平方米的房间环境中达到 3 厘米的精度，在 300 平方米的工业环境中达到 8 厘米的精度。为了显示由于比例误差导致的精度损失，如果系统能够恢复真实比例，还显示了RMSE，见GT scale列。通过在执行结束时执行视觉惯性全BA，可以进一步提高精度和比例估计，见全BA列。
然而，视觉惯性BA的成本更高，局部BA的局部窗口必须小于仅视觉情况下的窗口。这解释了在没有完全BA的情况下GT缩放视觉惯性结果稍差的原因。事实上，视觉惯性全BA通常在7秒内收敛15次迭代，而仅视觉全BA在不到1秒的时间内收敛5次迭代。
为了测试视觉惯性ORB-SLAM重用先前地图的能力，在同一环境中连续运行所有序列。处理第一个序列并执行完整的BA。然后运行其余的序列，在这里系统重新定位并继续执行SLAM。然后，将累积的关键帧轨迹与groundtruth进行比较。由于存在以前的地图，系统现在能够在序列V1 03 difficult中定位摄像机。
V1、V2 和 MH 环境的 RMSE 分别为 0.037、0.027 和 0.076，比例因子误差为 1.2%、0.1% 和 0.2%。最终的完整 BA 的影响可以忽略不计，因为已经在第一个序列结束时执行了完整的 BA。这些结果表明，重新访问同一场景时没有漂移累积，因为所有序列的 RMSE 不大于单个序列。
最后，将视觉惯性 ORB-SLAM 与最先进的双目相机直接视觉惯性里程计 进行了比较，这也显示了 Vicon Room 1 序列的结果，可以进行直接比较。图 5 显示了相对姿势误差 (RPE) 。为了计算方法的 RPE，需要恢复帧轨迹，因为后端只优化了关键帧。为此，在跟踪帧时，存储到参考关键帧的相对变换，以便可以在执行结束时从估计的关键帧位姿中检索帧位姿。
在实验结束时，还没有运行完整的 BA。可以看到，视觉惯性里程计方法的误差随着行驶距离而增长，因为地图重用，视觉惯性 SLAM 系统不会累积误差。双目方法能够在 V1 03 困难的情况下工作，而单目方法则失败了。单目 SLAM 成功地恢复了米尺度，并在短路径中达到了相当的精度，其中 SLAM 的优势与里程计相比可以忽略不计。