Visual-Inertial Monocular SLAM with Map Reuse论文阅读笔记

摘要

近年来，在视觉惯性里程计领域取得了显著进展，并致力于通过高精度和鲁棒性计算传感器提供的增量运动信息。然而现有方法存在闭环能力不足的问题即使传感器持续 revisit同一个位置轨迹估计仍会累积漂移误差在这项研究中我们提出了一种创新性的紧耦合视觉惯性同步定位与建图系统该系统不仅实现了闭环反馈还能有效重用生成的地图从而在已有图层实现零漂移定位效果特别是在应用方面该方法适用于各种相机配置但特别针对单目相机这一普遍存在的尺度模糊问题给出了突破性的解决方案同时我们还开发了一种高效的 IMU 初始化算法能够在短时间内精确计算尺度方向速度以及传感器偏置参数在最近开展的微型飞行器公共数据集测试中该系统展现出优异性能在11个测试序列中实现了典型比例因子误差控制在1%并达到厘米级精度水平通过与最新序列中的视觉惯性里程计技术对比分析表明得益于地图重用机制和无累加漂移特性本方法在准确度方面具有明显优势

视觉惯性预积分

该系统接收一系列来自惯性测量单元（IMU）的测量数据与连续拍摄的单目相机帧作为其输入信息。在传统的针孔摄像机模型中使用投影函数π：ℝ³→Ω将相机参考系C中的三维点XC∈ℝ³映射到图像平面Ω⊂ℝ²上的二维点xc。

其中,[fu fv]^T表示焦距,[cu cv]^T表示原点偏移量,该投影函数未考虑成像过程中的畸变。在提取图像的关键点时,其坐标位置得以维持,以便它们能够利用该投影函数与相应的投影点进行配准。

惯性测量单元（以其参考系标记为B）通过定期间隔∆t对传感器测得的加速度aB和角速度ωB进行记录（频率通常达到数百赫兹）。除传感器噪声外，在此过程中还存在由加速度计与陀螺仪缓慢变化所引入的偏差项ba与bg影响。在计算运动时需扣除重力gW的影响。基于此，在世界参考系W中惯性测量单元方位RWB∈S0(3)、位置WpB与速度WvB的时间离散演变为下式所示：

两个相邻的关键帧之间的时间段内物体的运动状态可基于一系列中间测量值及其对应的ΔR、Δv和Δp进行预融合计算。通过借鉴该文献中提出的On-Manifold Preintegration方法实现对实时视觉-惯性融合定位系统的增量优化：

其中Jacobian J_{() }^{a} and J_{() }^{a}表征了在隐式重新积分预积分情况下偏置变化的一阶近似。当inertial measurement unit (IMU) measurements arrive, the pre-integrations and Jacobian matrices can be effectively updated. cameras and IMUs are considered rigidly connected, with the transformation matrix TCB = [RCB | CpB] operating between reference systems that have been calibrated.

ORB-SLAM视觉惯性系统

1.跟踪

视觉惯性融合算法主要负责在每帧周期内实时获取并融合传感器的姿态信息、运动速度以及IMU偏差数据。这种算法能够可靠地推断相机的姿态，并与基于单目视觉的传统方法不同，在预测过程中采用了不同的策略：首先将局部地图中的关键点与当前帧的关键特征进行匹配；随后通过最小化所有匹配点的特征重投影误差以及IMU误差项来估计当前时刻的状态参数（如位置、姿态等）。如图2所示，在采用闭环回路的情况下（即根据本地建图模块或闭环线圈是否存在来决定是否更新全局地图），系统的自洽性和定位精度会得到显著提升。

当地图发生更新后进行追踪操作时（图2a），IMU误差项将在当前帧j与其之前的最后一个关键帧i之间建立连接：其中j表示当前帧而i表示最后一个关键帧。

其中，给定匹配 k 的特征重投影错误Eproj定义如下：

其中x^{k}表示图像中的关键点坐标，在全局坐标系中对应的地图点为x_{w}^{k}。与其相关联的信息矩阵\sum{k}反映了这些对应关系的质量和可靠性。其误差项E_{IMU}由以下公式表示：

式中，\sum{I}是预积分的信息矩阵 ，\sum{R}随机游走偏差，\rho是鲁棒核函数。使用高斯牛顿法优化后（图2b），得到的估计和Hessian矩阵用作下一次优化的先验。 假设没有地图更新（图2c），下一帧 j+1 将通过链接到帧 j 并使用在上一优化结束时计算的先验值进行优化（图2b）：

Eprior是一个先验值，

其中( ¯· ) 和\sum{p}代表了先前优化所生成的估计状态与Hessian矩阵（见图2b）。在此轮优化完成后（见图2d），帧j被边缘化处理。这种连接两个连续帧并利用先验进行优化的过程被反复应用（见图2e-f），直到全局地图发生改变为止。值得注意的是，在这一过程中（见图2e-f），由于全局地图尚未更新完毕，在局部模式下执行该过程直至全局地图发生变化。一旦全局地图发生改变，则需要重新将当前帧与最后一个关键帧建立连接关系（见图2a）。

局部建图

在插入新的关键帧后，该局部建图线程执行局部BA优化。这一过程不仅优化了最近的N个关键帧（即本地窗口），同时也考虑了这些N个关键帧能够观测到的所有特征点。值得注意的是，在covisibility图中与任何本地关键帧相连的其他局部分析的关键帧（即那些不属于本地窗口的），也会对总计算成本产生影响。然而，在优化阶段保持固定（固定窗口）。因此，在固定窗口期间始终包含第N+1个关键帧的原因在于其受IMU状态的限制。如图3所示，在原始ORB-SLAM和视觉惯性ORB-SLAM算法中展示了两种不同类型的局部BA实现过程。

代价函数是由 IMU 误差项与重投影误差项两部分组成的。相较于仅依赖视觉信息的情况而言，视觉惯性版本更加复杂，在每个关键帧上需要优化求解 9 个额外的状态参数（速度与偏差），因此在实现时需要合理选择局部窗口的范围以保证实时性。

局部构建图不仅负责关键帧的管理工作。传统的 ORB-SLAM 策略舍弃了冗余的关键帧，在建图效果较好的区域进行定位时，地图尺寸不会有所增加。然而该方法在利用 IMU 信息时存在不足之处。具体而言，在连续关键帧之间的时间间隔过长时，IMU 提供的信息质量会显著下降。因此，在实际应用中必须确保任意两个连续的关键帧之间的时差不超过 0.5 秒才能有效丢弃冗余关键帧。只有当相邻关键帧的时间偏移控制在 3 秒以内时才能保证全局优化过程的顺利进行，并在此基础上实现全局地图的实时更新与优化

闭环

当机器人返回已建立的地图区域时,闭环线程设计目标是降低探索过程中的累积漂移量.位置识别模块负责匹配最近与过去的关键帧对.通过计算刚体变换矩阵来验证该匹配,刚体变换矩阵用于对齐关键帧之间的对应点.最后一步优化过程以减少轨迹中累积的误差量.这种优化策略在处理大型地图数据时可能会消耗较多资源,因此选择执行姿势图优化算法,这有助于降低算法复杂度.

与原始ORB-SLAM相比本系统仅在6个自由度（DoF）上实现了姿势图优化而非7个自由度由于能够观测到比例尺度因而采用了这一设计方案

IMU初始化

我们提出了一种基于视觉惯性的全巴尔（BA）算法来初步估计尺度、重力方向、速度以及IMU偏差，并将其应用于由单目SLAM算法处理的一组关键帧数据中。具体而言，在这种方案中，默认情况下单目SLAM运行几秒钟即可满足所需条件，并默认情况下认为，在传感器运动过程中所有变量均可被观测到。值得注意的是，在本研究中我们主要基于ORB-SLAM框架进行开发，并在此基础上进行了相应的改进工作以适应特定需求；同时为了提高算法效率并减少计算复杂度，在后续优化阶段我们对一些核心参数进行了合理的调整；最后通过大量实验验证表明该方法能够有效提升定位精度并显著降低计算开销

初始化过程划分为若干更简单的子问题：其中第一阶段主要针对陀螺仪偏置进行估算第二阶段则基于规模与重力场的模型对位置进行近似第三阶段假设无加速度计误差的影响并进一步细化规模与重力方向第四阶段则基于上述分析可推导出物体的速度

陀螺仪偏差估计

可以通过仅利用两个连续关键帧的方向信息来估计陀螺仪偏差；假设陀螺仪偏移量的变化可被忽略，则可通过优化恒定偏移量B_g的方式，在相邻关键帧间的陀螺仪积分与其通过ORB-SLAM方法获得的方向一致性上实现最小化。

其中N代表关键帧的数量。
其中R_{WB}= R WC R CB, 而R WC是由ORB-SLAM计算得到的旋转矩阵, R CB则是相机标定所得校正矩阵。
\Delta R_i,i+1表示相邻两个关键帧之间由陀螺仪积分得到的变化量。
通过采用零偏差初始种子的高斯-牛顿优化算法进行求解, 与之类似的解析雅可比矩阵在《On-Manifold Preintegration for Real-Time Visual-Inertial Odometry》一文中有所体现。

尺度和重力近似（无加速计偏差）

当陀螺仪偏差被估计后

改写说明

该研究旨在利用这些变量的线性方程组来估算 s 和 gW 的值。为了减少计算 N 个速度所带来的复杂度，并非直接解决每个速度的问题，而是基于三个连续的关键帧间的关系，并应用速度关系建立以下数学模型。

为了清晰表示，将关键帧i、i+1、i+2写为1、2、3，

随后，在一个系统中整合三个连续的关键帧之间的相互作用：即矩阵 A_{3\left ( N-2 \right )*4}x_{4*1}=B_{3\left ( N-2 \right )*1} 的构建过程；通过应用奇异值分解（SVD），可以提取出比例因子s以及重力向量g_W；值得注意的是：由于该方程组中共有 3(N−2) 个方程而未知数数目为四：从而推导出所需的参数数量最少需要四个关键帧就可以完成求解

加速计偏差估计、尺度和重力方向细化

在目前的计算中尚未纳入加速度计偏差的影响，在该领域仍存在研究空白。

现在将重力矢量表示为：

其中 RWI 在 I 中仅限于利用围绕 x 和 y 轴的两个角度来进行参数化；这是因为与重力方向一致的绕 z 轴旋转对于 gW 的影响可以忽略不计。这种绕 x-y 平面旋转的方式可以通过微小扰动 δθ 来实现优化：

其一阶近似

将 (17) 代入 (11) 并且现在包括加速度计偏差的影响，得到：

考虑如（12）中的三个连续关键帧，可以消除速度并得到以下关系：

其中，

其中矩阵的前两列由\left [ \right ](:,1:2)表示；将三个连续关键帧之间的关系进行堆叠后构建了一个线性方程组A_{3(N-2):6}x_{6×1}=B_{3(N-2):1}；通过奇异值分解（SVD）方法可以求解出尺度比例因子s^*、重力方向校正量\delta\theta_{xy}^*以及加速度计偏差b_a^*；在这种情况下共有3(N−2)个方程而未知数数量为6；再次需要至少4个关键帧才能求解这个系统

用于计算问题的条件数（即最大与最小奇异值的比值），我们可以评估问题是否具有良好的可解性（亦即传感器是否实现了对所有变量进行观测的能力）。通常可以通过重新线性化并迭代求解方案来处理这类问题；然而，在实际操作中发现经过两次迭代并未带来显著的改善。

速度估计

考虑到方程（12）与（19）涉及三个连续的关键帧之间的关系，在分析这些关系时会得到一个线性系统，在这个系统中未引入与速度相关的额外未知数数量为3N。现可通过等式 (18) 计算所有关键帧的速度值，并利用已知的比例、重力和偏差参数实现这一过程。为了求取最近的关键帧速度值，则可利用关系式（3）。

重定位后的偏置重新初始化

当系统在长时间运行后进行定位时, 基于位置识别的方法进行操作, 以重新计算陀螺仪偏置. 加速度计偏置可通过简化后的模型进行计算, 其中唯一的未知量是偏置参数. 因为系统的大小和所处环境中的重力场均为已知条件, 所以只需利用这些信息即可完成估算. 基于视觉信息连续获取20个帧的数据来进行参数估算.

实验

进行了评估，在EuRoC数据集中采用了Visual-Inertial ORB-SLAM技术。该数据集由微型飞行器(MAV)记录了11个连续拍摄的序列，在两个不同房间及一个工业环境中持续运行了三个多小时。这些序列依据其环境特征被划分为三类：简单（良好光照及缓慢动作）、中等（复杂光照及快速动作）或困难（运动模糊）。该数据集基于20Hz同步全局快门拍摄能力提供了WVGA分辨率双目图像，并通过200Hz采样率捕捉了IMU测量值及真实轨迹数据。这些特性使其成为测试视觉惯性SLAM系统理想的数据资源。实验仅在配备8GB内存及采用Intel Core i7-4700MQ处理器配置的高性能计算机上运行左侧图像序列

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 进入探索阶段时进入处理阶段。将局部分配给窗口大小设定为 10 关键帧。初始化延迟达单目 ORB-SLAM 初始化后的 15 秒执行。随后立即启动并完成全部的BA过程。详细列出了各序列关键帧轨迹及其平移均方根误差（RMSE）于表I中

基于Vicon平台及Leica地面实测数据开展IMU后处理工作

该系统采用米制比例进行运动恢复操作，在实际应用中表现稳定。其比例误差通常控制在1%以内，并且根据测试结果，在面积为30平方米的室内环境中可实现厘米级精度，在面积达300平方米时仍可维持约8厘米级别的精度水平。为了直观展示由于比例误差带来的精度损失情况，则对系统进行了相应处理并记录数据（见GT scale列）。优化后的方法通过执行全局视觉惯性BA进一步提升系统的整体性能表现（见全BA列）。

然而，在全息双目场景下进行的视觉惯性布料检测成本显著高于基于单眼摄像头的方法。为了确保精度，在局部区域采样时, 局部区域的采样窗口尺寸必须严格小于基于单眼摄像头的情况。这一现象表明, 在未完全实现全局布料检测时, GT缩放后的视觉惯性结果略显不足. 实际上, 在每7秒钟即可完成15次迭代计算的情况下,"全息双目BA"的速度远超基于单一视网膜的方法——后者只需不到一秒时间即可完成五次迭代计算

本研究旨在评估视觉惯性ORB-SLAM系统的重用能力，在所有连续采集的视觉数据基础上实现动态环境下的实时定位与建图能力测试。随后对初始框架进行全局优化，并对后续数据进行解算，并持续更新状态估计。基于先前构建的地图信息，在V1_03_difficult场景中实现了 camera pose 的实时重构。

这些实验中采用的三个环境变量（V1和V2以及MH）均进行了多次测试与验证，在不同条件下分别得到了均方根误差为R_{\text{MSE}} = \{R_1, R_2, R_3\}的结果数据集。其中各参数的比例因子误差分别为P_1 = \{P_{\text{ratio}}\}等值。通过实验对比分析可知，在基于全局优化的一次性BA处理下不会产生显著影响效果的变化。此外由于在初始序列处理后立即完成了全局优化步骤，在后续的数据融合过程中不会引入额外的影响因素。实验结果表明，在多次重新访问同一场景的情况下也不会积累漂移效应的现象出现，并且所有测试数据集中的R_{\text{MSE}}值都未超过各自独立测试条件下的最大阈值限制

最后对比视觉惯性 ORB-SLAM 和当前最先进的双目相机基于直接视觉惯性的里程计系统展示了它们之间的性能差异这也表明 Vicon Room 1 序列数据具有可比性 图 5 显示了这些方法带来的相对姿势误差（RPE）。为了评估计算方法所带来的姿态误差（RPE）需要恢复完整的帧轨迹这是因为后端优化仅针对关键帧进行了调整因此在完成目标跟踪后需将估计出的关键帧位姿与参考关键帧进行匹配从而获取每 frame 的位姿信息

在实验结束时（尚未完成BA运行），仍无法完成视觉惯性里程计系统的完整校准过程。可以看出（可以看出），基于地图重用的方法能够有效减少视觉惯性SLIM系统的累积误差（因为地图重用）。双目SLIM系统能够在V1.03复杂场景中稳定运行（困难情况下工作），而单目SLIM系统未能实现目标（失败）。单眼SLIM系统成功实现了米尺级定位（单目SLAM成功地恢复了米尺度），并在短路径测试中表现优异（其中SLIM的优势与里程计相比可以忽略不计）。