【Basaalt】Visual-Inertial Mapping with Non-Linear Factor Recovery论文解读

基于非线性因子恢复的视觉-惯性建图

整段核心逻辑串联
这是论文摘要的核心句，高度概括了本文的研究问题、核心方法、关键创新、实际效果和实验结论，逻辑链清晰：
发现问题→VIO 局部精度高但信息难直接用于全局建图，纯视觉建图存在姿态角观测性问题、鲁棒性差；
提出方法→用非线性因子恢复从 VIO 中提取并浓缩轨迹核心信息，重构非线性因子；
实现手段→将 VIO 因子与回环检测约束结合，通过光束平差法做全局优化，构建全局一致地图；
核心创新 / 贡献→VIO 因子让全局地图的横滚 / 俯仰角可观测，提升建图的鲁棒性和精度；实验验证→在公共基准数据集上，证明本文方法优于当前主流算法。
简单通俗总结
本文提出了一种新的视觉 - 惯性建图方法：把视觉 - 惯性里程计（VIO）中积累的运动轨迹信息，通过 "非线性因子恢复" 提炼成一组核心的约束因子，
再把这组因子和回环检测的约束结合起来，用光束平差法做全局优化，最终得到全局一致的地图。这组从 VIO 来的因子能让地图的倾斜角度
（横滚 / 俯仰）被精准约束，还能让建图更稳、更准，而且在公开数据集的实验中，这个方法比现在最好的方法表现还要好。

弗拉迪斯拉夫·乌先科¹、尼古拉斯·德梅尔¹、戴维·舒伯特¹、约尔格·施图克勒²、丹尼尔·克雷默斯¹

¹德国慕尼黑工业大学 ²德国图宾根马克斯·普朗克智能系统研究所

摘要 相机与惯性测量单元（IMU）是实现自运动估计和环境建图的互补传感器，二者融合可提升视觉-惯性里程计（VIO）系统的精度与鲁棒性。然而，在全局一致建图中融合视觉和惯性信息并非易事：利用图像序列估计运动和几何特征需要大基线，因此多数系统采用时间间隔较大的关键帧进行处理；而惯性数据的有效性会随时间间隔的增加快速衰减，经过数秒的积分后，其包含的有效信息通常所剩无几。

本文提出一种基于非线性因子恢复的方法，从视觉-惯性里程计中提取视觉-惯性建图的相关信息，重构出一组非线性因子，以最优方式近似VIO累积的轨迹信息。为得到全局一致的地图，我们将这些因子与回环检测约束结合，通过光束平差法完成优化。VIO因子能使全局地图的横滚角和俯仰角具备可观测性，同时提升建图的鲁棒性与精度。在公共基准数据集上的实验表明，该方法的性能优于当前主流方法。

一、引言

视觉-惯性里程计（VIO）是机器人、增强现实等领域中用于跟踪相机运动的主流方法。融合视觉和IMU测量数据能够充分发挥两种传感器的互补优势，从而提高估计的精度和鲁棒性。通常，相机轨迹和地图的优化是在由近期相机帧和IMU测量值构成的小窗口内局部进行的，但这种方法不可避免地会导致估计结果出现漂移。

计算机视觉领域对视觉-惯性建图的全局一致优化研究较少。虽然理论上可将该优化问题构建为融合IMU测量值的光束平差问题，但直接实现会因帧数量过多导致优化参数激增，造成计算上的不可行。为控制计算量，光束平差法会将高帧率的相机图像二次采样为数量更少的关键帧。VIO中常用的做法是对连续帧间的IMU测量值进行预积分，但若为提高优化效率选择时间间隔较远的关键帧，传感器噪声的累积会使预积分的IMU测量值对轨迹的约束作用大幅减弱。低帧率同样会影响由视觉和惯性信息估计得到的速度和偏置的质量，而这些参数是利用IMU预积分进行位姿预测的关键。

本文提出一种新方法，将视觉-惯性建图构建为基于高帧率视觉和惯性测量值的光束平差问题。该方法并非直接对所有帧的相机轨迹进行优化，而是采用分层策略：首先以相机的原始帧率得到局部VIO估计结果；当关键帧从当前局部VIO优化窗口中被移除并边缘化后，提取非线性因子来近似关键帧间相机运动所包含的累积视觉-惯性信息；最终将关键帧和非线性因子用于全局光束平差层的优化。

在VIO层，本文方法采用适用于快速精准跟踪的图像特征；在建图层，选用具有独特性且对光照和视角变化鲁棒的关键点，以适配回环检测任务。通过该设计，方法能够融合高帧率下的IMU信息、短期视觉跟踪信息，以及低帧率下的关键点匹配和回环检测信息，实现全局一致的建图（见图1）。所提取的因子还能使地图保持与重力方向对齐，并在视觉信息不足的帧之间建立约束。同时，该方法无需估计速度和偏置，有效降低了优化问题的复杂度。

图1 基于估计的重力方向绘制的地图正射俯视图（EuRoC数据集的MH 05序列）。为得到与重力对齐的全局一致地图，从VIO的边缘化先验中恢复非线性因子，并与基于关键点的光束平差法融合。绿色线条为光束平差因子形成的关键帧连接，红色线条为恢复的相对位姿因子形成的关键帧连接。此外，每个关键帧都有一个恢复因子，用于抑制与VIO中观测到的重力方向的偏差。

本文的主要贡献如下：

1. 提出一种全新的分层视觉-惯性建图方法，通过非线性因子恢复将基于关键点的光束平差法与惯性信息、短期视觉跟踪相融合；
2. 设计的VIO子系统作为建图方法的底层模块，在多数测试序列上的轨迹估计精度优于当前主流方法，这一结果通过合理融合图像块跟踪、路标表示、一阶估计雅可比矩阵、边缘化策略等组件实现（详见第四节）；
3. 与其他将IMU预积分测量值同样用于建图的主流系统不同，本文将高帧率的视觉-惯性信息浓缩为从VIO层边缘化先验中提取的非线性因子，不仅降低了优化问题的复杂度，还在最终与重力对齐的地图中得到了更优的位姿估计结果。

读者可参考演示视频并查阅该系统的开源实现，地址为：https://vision.in.tum.de/research/vslam/basalt

二、相关工作

2.1 视觉-惯性里程计

早期的视觉-惯性里程计方法主要基于滤波器实现。在紧耦合滤波器中，预测步骤通常利用IMU测量值传播当前相机状态估计结果，再基于相机图像对状态进行递归校正。滤波器的一个显著缺陷是，一旦测量值被融合，非线性测量模型和状态转移模型的线性化点便无法更改。

固定滞后平滑器（即基于优化的方法）会在由近期帧构成的局部优化窗口内，基于当前状态重新进行线性化，将视觉-惯性状态估计构建为针对关键帧和IMU测量值的完整光束平差问题，并通过边缘化窗口内的旧帧将问题简化为计算上可处理的规模。连续的重新线性化、窗口化优化和边缘化先验的维护，提升了方法的估计精度。

上述方法需要舍弃在被边缘化关键帧中观测到的关键点和观测值，以保持边缘化先验的稀疏结构。熊等人提出利用非线性因子恢复实现稀疏的边缘化先验，同时无需舍弃观测关键点的相关信息，该方法能进一步优化关键点并提高估计精度，但仅适用于局部光束平差，与本文研究存在本质区别。

2.2 视觉-惯性建图

目前针对视觉和惯性测量值实现全局一致建图的研究较少。卡夏诺夫等人在基于关键帧的视觉-惯性里程计方法基础上，增加了带回环检测的位姿图优化层，位姿图由VIO的关键帧及其相对位姿估计结果构建。穆-阿塔尔等人将IMU测量值通过预积分融入基于关键帧的SLAM系统，将IMU测量值预积分为关键帧间的伪测量值，他们发现预积分测量值的精度会随时间衰减，因此将局部光束平差中关键帧的时间间隔限制为0.5秒，全局光束平差中限制为3秒。该方法的另一缺陷是需要估计每个关键帧处的相机速度和IMU偏置，由于关键帧的时间二次采样间隔较大，视觉测量值对这些参数的约束效果远不如本文方法。

施耐德等人采用了类似的方法，将预积分的IMU测量值融入优化过程。秦等人提出融合VIO和4自由度位姿优化的视觉-惯性建图方法，固定横滚角和俯仰角两个自由度，仅对其余自由度进行优化。本文同样利用视觉-惯性测量值约束横滚角和俯仰角，但通过概率建模提取非线性因子，考虑了这些参数的不确定性，并在全局概率优化中与其他信息进行权衡。

三、预备知识

本文中，矩阵用粗体大写字母表示（如R\boldsymbol{R}R），向量用粗体小写字母表示（如ξ\boldsymbol{\xi}ξ）。刚体位姿表示为(R,p)∈SO(3)×R3(\boldsymbol{R},\boldsymbol{p}) \in SO(3) \times \mathbb{R}^3(R,p)∈SO(3)×R3，必要时也用变换矩阵T∈SE(3)\boldsymbol{T} \in SE(3)T∈SE(3)表示。旋转矩阵R\boldsymbol{R}R的增量更新定义为R⊕ξ=Exp(ξ)R\boldsymbol{R} \oplus \boldsymbol{\xi} = \text{Exp}(\boldsymbol{\xi})\boldsymbol{R}R⊕ξ=Exp(ξ)R（其中ξ∈R3\boldsymbol{\xi} \in \mathbb{R}^3ξ∈R3）；两个旋转矩阵R1\boldsymbol{R}_1R1和R2\boldsymbol{R}_2R2的差值计算为R1⊖R2=Log(R1R2−1)\boldsymbol{R}_1 \ominus \boldsymbol{R}_2 = \text{Log}(\boldsymbol{R}_1\boldsymbol{R}_2^{-1})R1⊖R2=Log(R1R2−1)，满足(R⊕ξ)⊖R=ξ(\boldsymbol{R} \oplus \boldsymbol{\xi}) \ominus \boldsymbol{R} = \boldsymbol{\xi}(R⊕ξ)⊖R=ξ。

本文中，Exp:R3→SO(3)\text{Exp}: \mathbb{R}^3 \to SO(3)Exp:R3→SO(3)由反对称算子(R3→so(3))(\mathbb{R}^3 \to \mathfrak{so}(3))(R3→so(3))和矩阵指数(so(3)→SO(3))(\mathfrak{so}(3) \to SO(3))(so(3)→SO(3))复合而成，将旋转向量映射为对应的旋转矩阵；其逆运算为Log:SO(3)→R3\text{Log}: SO(3) \to \mathbb{R}^3Log:SO(3)→R3。对于平移、速度、偏置等其他变量，将⊕\oplus⊕和⊖\ominus⊖定义为普通的加法和减法。

定义状态s\boldsymbol{s}s为多个旋转和向量变量的元组，函数r(s)r(\boldsymbol{s})r(s)依赖于该状态，输出可为旋转或向量。增量ξ∈Rn\boldsymbol{\xi} \in \mathbb{R}^nξ∈Rn为状态s\boldsymbol{s}s中所有变量的增量构成的堆叠向量，则函数关于该增量的雅可比矩阵定义为：
Jr(s)=lim⁡ξ→0r(s⊕ξ)⊖r(s)ξJ_{r(\boldsymbol{s})}=\lim _{\boldsymbol{\xi} \to 0} \frac{r(\boldsymbol{s} \oplus \boldsymbol{\xi}) \ominus r(\boldsymbol{s})}{\boldsymbol{\xi}}Jr(s)=ξ→0limξr(s⊕ξ)⊖r(s)

其中，s⊕ξ\boldsymbol{s} \oplus \boldsymbol{\xi}s⊕ξ表示利用对应的⊕\oplus⊕算子，将ξ\boldsymbol{\xi}ξ中各分段增量作用于s\boldsymbol{s}s的每个分量，⊖\ominus⊖的运算规则同理。该极限按分量求解，因此雅可比矩阵为矩阵形式。对于欧几里得量，此定义即为常规的导数，同时对作为函数值和自变量的旋转进行了扩展。更多细节及其他可能的表示形式可参考相关文献。

在非线性最小二乘问题中，需最小化如下形式的函数：
E(s)=12r(s)⊤Wr(s)E(\boldsymbol{s})=\frac{1}{2}r(\boldsymbol{s})^\top \boldsymbol{W} r(\boldsymbol{s})E(s)=21r(s)⊤Wr(s)

该式为带块对角权重矩阵W\boldsymbol{W}W的残差和的平方范数，此时r(s)r(\boldsymbol{s})r(s)为纯向量值。在当前状态s\boldsymbol{s}s附近，可对残差进行线性近似，得到：
E(s⊕ξ)=E(s)+ξ⊤Jr(s)⊤Wr(s)+12ξ⊤Jr(s)⊤WJr(s)ξE(\boldsymbol{s} \oplus \boldsymbol{\xi})=E(\boldsymbol{s})+\boldsymbol{\xi}^\top J_{r(\boldsymbol{s})}^\top \boldsymbol{W} r(\boldsymbol{s})+\frac{1}{2} \boldsymbol{\xi}^\top J_{r(\boldsymbol{s})}^\top \boldsymbol{W} J_{r(\boldsymbol{s})} \boldsymbol{\xi}E(s⊕ξ)=E(s)+ξ⊤Jr(s)⊤Wr(s)+21ξ⊤Jr(s)⊤WJr(s)ξ

该近似能量的最优解可通过高斯-牛顿增量求得：
ξ∗=−(Jr(s)⊤WJr(s))−1Jr(s)⊤Wr(s)\boldsymbol{\xi}^*=-\left(J_{r(\boldsymbol{s})}^\top \boldsymbol{W} J_{r(\boldsymbol{s})}\right)^{-1} J_{r(\boldsymbol{s})}^\top \boldsymbol{W} r(\boldsymbol{s})ξ∗=−(Jr(s)⊤WJr(s))−1Jr(s)⊤Wr(s)

据此，可通过迭代更新状态si+1=si⊕ξ∗\boldsymbol{s}{i+1}=\boldsymbol{s}{i} \oplus \boldsymbol{\xi}^*si+1=si⊕ξ∗直至收敛。

四、视觉-惯性里程计

本文将相机-IMU组合的增量运动跟踪构建为固定滞后平滑问题。首先利用基于图像块的光流法，在连续帧的二维图像平面上跟踪一组稀疏点；随后在光束平差框架中，结合点的重投影误差项和IMU传播误差项，对每一帧进行优化；为保持优化问题的参数规模固定，对旧状态进行边缘化处理。本节将详细阐述上述各个步骤。

4.1 KLT跟踪

算法的第一步是利用FAST角点检测器在帧中检测一组稀疏关键点，再基于KLT稀疏光流法在连续帧序列中跟踪这些点的运动。为实现快速、精准且鲁棒的跟踪，本文将逆组合法与对亮度缩放不变的图像块相异度范数相结合。已有学者提出将零归一化互相关（ZNCC）用于光照不变的光流估计，但本文采用局部缩放平方差和（LSSD），其计算成本低于其他方法。

将图像块跟踪问题构建为：估计连续两帧中对应图像块间的SE(2)SE(2)SE(2)变换T\boldsymbol{T}T，使图像块间的差异按选定范数最小化。本质上是最小化残差的平方和，每个残差定义为：
ri(ξ)=It+1(Txi)It+1‾−It(xi)It‾∀xi∈Ωr_{i}(\boldsymbol{\xi })=\frac {I_{t+1}(\boldsymbol{T}x_{i})}{\overline{I_{t+1}}}-\frac {I_{t}(x_{i})}{\overline{I_{t}}} \quad \forall x_{i}\in \Omegari(ξ)=It+1It+1(Txi)−ItIt(xi)∀xi∈Ω

其中，It(x)I_t(x)It(x)为第ttt帧在像素位置xxx处的亮度值，Ω\OmegaΩ为定义图像块的图像坐标集合，It‾\overline{I_t}It为第ttt帧中该图像块的平均亮度值。图像块及跟踪结果的可视化见图2。

图2 本文系统估计的KLT跟踪结果示例。尽管曝光时间发生变化，该方法仍能准确估计图像中图像块间的SE(2)SE(2)SE(2)变换。

该图 / 说明的核心实验价值（对应论文第四节 KLT 跟踪模块的创新）
论文在传统 KLT 算法基础上做了两个关键改进，图 2 是对这两个改进效果的直接验证：

采用局部缩放平方差和（LSSD） 作为图像块的相似度度量准则，替代传统的平方差和（SSD）/ 零归一化互相关（ZNCC），在保证计算效率的同时，
实现对光照 / 曝光变化的鲁棒性；
将 KLT 跟踪的目标定义为估计SE(2)空间的刚体变换，而非单纯的像素平移，更贴合相机平面运动的物理实际，提升跟踪的准确性。

简单来说，图 2 是一个 "抗曝光干扰" 的对比实验可视化：用实际结果证明，本文改后的 KLT 算法，即使相机曝光参数变了、图像亮暗变了，也能稳稳跟踪住特征点的运动，而这是 VIO 层获取准确视觉运动信息的基础 ------ 只有底层特征跟踪准了，后续的视觉 - 惯性融合、轨迹估计才会精准。
通俗总结
图 2 展示的是我们算法里 KLT 特征跟踪的实际效果，核心想证明：就算拍摄时相机的曝光时间变了、图像亮暗不一样了，我们改进的 KLT 方法也能准确算出连续画面中，同一个特征区域的平移和旋转变化，不会因为亮度变化跟丢或跟错特征点，这是我们整个 VIO 系统能精准跟踪相机运动的重要基础。

为提升对图像中大幅位移的鲁棒性，采用金字塔分层策略：先在最粗的金字塔层跟踪图像块，再逐步在更精细的层级上优化。对于外点滤波，不采用基于误差的绝对阈值，而是将图像块从当前帧跟踪至目标帧后再反向跟踪，验证一致性；若反向跟踪后无法回到初始位置，则将该点判定为外点并舍弃。

4.2 视觉-惯性光束平差

融合KLT跟踪得到的特征位置误差项，以及基于IMU预积分测量值的IMU误差项，实现相机运动估计。

本文全程使用以下坐标系：WWW为世界坐标系，III为IMU坐标系，CiC_iCi为第iii个相机的坐标系（立体视觉系统中用z˙\dot{z}z˙表示相机索引）。估计从IMU坐标系到世界坐标系的变换TWI∈SE(3)\boldsymbol{T}{WI} \in SE(3)TWI∈SE(3)，从相机坐标系到IMU坐标系的变换TICi\boldsymbol{T}{IC_i}TICi以及投影函数πi\pi_iπi由标定得到，视为固定已知量。为构建重投影误差，定义从相机坐标系到世界坐标系的变换TWCi\boldsymbol{T}{WC_i}TWCi，该量并非额外的优化变量，实际中可由TWI\boldsymbol{T}{WI}TWI和TICi\boldsymbol{T}_{IC_i}TICi计算得到。

在不同时刻，优化的状态定义为：
s={sk,sf,sl}\boldsymbol{s}=\left\{ \boldsymbol{s}_k,\boldsymbol{s}_f,\boldsymbol{s}_l\right\}s={sk,sf,sl}

其中，sk\boldsymbol{s}_ksk包含nnn个旧关键帧的IMU位姿，sf\boldsymbol{s}_fsf包含mmm个最新帧（可能为承载路标的关键帧）的IMU位姿、速度和偏置，sl\boldsymbol{s}_lsl包含路标信息。该问题的图形化表示见图5(a)。路标相对首次观测到它的关键帧进行存储，由相机坐标系中的单位长度方向向量和到路标的逆距离表示。本文系统中仅关键帧承载路标，这是其与普通帧的核心区别。

图3 用于表示单位向量的球极投影几何解释。两个参数定义了蓝色坐标系中XY平面上的一个点，从(0,0,−1)⊤(0,0,-1)^\top(0,0,−1)⊤发射射线，与黑色单位球面的交点即为对应的三维单位向量。图中红色、绿色、黄色为三个示例点，虚线为与球面相交的射线，箭头为得到的单位向量。

4.2.1 三维单位向量的表示

为避免优化中引入额外约束并减少优化变量数量，本文采用二维的最小表示形式对三维空间中的观测方向向量进行参数化。相关文献对多种参数化方法进行了全面综述，并提出一种基于SO(3)SO(3)SO(3)旋转的新方法，其关于二维增量的导数形式简洁。

本文采用基于球极投影的参数化方法，由二维坐标(u,v)⊤(u,v)^\top(u,v)⊤生成单位长度的观测方向向量：
(xyz)=(ηuηvη−1),η=21+u2+v2\begin{pmatrix} x \\ y \\ z \end{pmatrix}=\begin{pmatrix} \eta u \\ \eta v \\ \eta-1 \end{pmatrix}, \quad \eta=\frac{2}{1+u^2+v^2} xyz = ηuηvη−1 ,η=1+u2+v22

该参数化方法仅使用乘法、除法等简单运算（相较于其他方法所需的三角函数运算），计算效率高，且对所有uuu和vvv均有定义。其几何解释见图3，唯一无法用有限的uuu和vvv表示的方向向量为Z轴负方向(0,0,−1)⊤(0,0,-1)^\top(0,0,−1)⊤，但在实际应用中，相机的视场角有限，无法观测到后方的点，因此这一限制无实际影响。

4.2.2 重投影误差

运动估计的第一个信息源为重投影误差。若在主帧h(i)h(i)h(i)中观测到的点iii，在目标帧ttt的图像坐标zitz_{it}zit处被检测到，则残差定义为：
rit=zit−πc(t)(Tt−1Th(i)qi(u,v,d))r_{it}=z_{it}-\pi_{c(t)}\left(\boldsymbol{T}{t}^{-1} \boldsymbol{T}{h(i)} q_{i}(u, v, d)\right)rit=zit−πc(t)(Tt−1Th(i)qi(u,v,d))
qi(u,v,d)=(x(u,v)y(u,v)z(u,v)d)⊤q_{i}(u, v, d)=\begin{pmatrix}x(u, v) & y(u, v) & z(u, v) & d\end{pmatrix}^\topqi(u,v,d)=(x(u,v)y(u,v)z(u,v)d)⊤

其中，c(t)c(t)c(t)为拍摄帧ttt的相机索引，Tt\boldsymbol{T}tTt表示帧ttt拍摄时刻的TWCc(t)\boldsymbol{T}{WC_{c(t)}}TWCc(t)，Th(i)\boldsymbol{T}_{h(i)}Th(i)的定义同理。齐次点坐标qi(u,v,d)q_i(u,v,d)qi(u,v,d)的前三个分量由4.2.1节的最小表示形式(u,v)(u,v)(u,v)计算得到，第四个分量ddd为逆距离。由于投影函数与尺度无关，无需对qiq_iqi进行归一化，这使得该表示形式在ddd趋近或等于0时，仍具有数值稳定性。

4.2.3 IMU误差

运动估计的第二个信息源为IMU数据。为处理高帧率的IMU测量值，将连续的多个IMU测量值预积分为一个伪测量值。在帧iii和帧jjj之间添加IMU因子时，类似相关文献的方法计算伪测量值Δs=(ΔR,Δv,Δp)\Delta \boldsymbol{s}=(\Delta \boldsymbol{R},\Delta \boldsymbol{v},\Delta \boldsymbol{p})Δs=(ΔR,Δv,Δp)。

首先利用原始加速度计测量值atrawa_t^{raw}atraw和陀螺仪测量值ωtraw\omega_t^{raw}ωtraw，计算经偏置校正后的加速度at=atraw−b‾iaa_t=a_t^{raw}-\overline{b}_i^aat=atraw−bia和旋转角速度ωt=ωtraw−b‾ig\omega_t=\omega_t^{raw}-\overline{b}_i^gωt=ωtraw−big；在整个预积分时间段内，固定对应的偏置b‾ia\overline{b}_i^abia和b‾ig\overline{b}_i^gbig，并通过线性近似考虑这些变量的变化。

为帧iii的时间戳tit_iti分配初始状态增量Δsti=(I,0,0)\Delta \boldsymbol{s}{t_i}=(\boldsymbol{I},\boldsymbol{0},\boldsymbol{0})Δsti=(I,0,0)，随后对所有满足ti<t≤tjt_i<t \leq t_jti<t≤tj的IMU时间戳ttt，进行如下更新：
ΔRt+1=ΔRtExp(ωt+1Δt)\Delta \boldsymbol{R}{t+1}=\Delta \boldsymbol{R}{t} \text{Exp}\left(\omega{t+1} \Delta t\right)ΔRt+1=ΔRtExp(ωt+1Δt)
Δvt+1=Δvt+ΔRtat+1Δt\Delta \boldsymbol{v}{t+1}=\Delta \boldsymbol{v}{t}+\Delta \boldsymbol{R}{t} a{t+1} \Delta tΔvt+1=Δvt+ΔRtat+1Δt
Δpt+1=Δpt+ΔvtΔt\Delta \boldsymbol{p}{t+1}=\Delta \boldsymbol{p}{t}+\Delta \boldsymbol{v}_{t} \Delta tΔpt+1=Δpt+ΔvtΔt

由此，Δst+1\Delta \boldsymbol{s}{t+1}Δst+1可表示为Δst\Delta \boldsymbol{s}tΔst、at+1a{t+1}at+1和ωt+1\omega{t+1}ωt+1的函数：
Δst+1=f(Δst,at+1,ωt+1)\Delta \boldsymbol{s}{t+1}=f\left(\Delta \boldsymbol{s}{t}, a_{t+1}, \omega_{t+1}\right)Δst+1=f(Δst,at+1,ωt+1)

其对应的雅可比矩阵为Jf=[Jfs,Jfa,Jfg]J_f=[J_f^s,J_f^a,J_f^g]Jf=[Jfs,Jfa,Jfg]。此外，截至t+1t+1t+1的所有fff迭代运算，可将Δst+1\Delta \boldsymbol{s}{t+1}Δst+1表示为偏置的函数：
Δst+1=gt+1(bia,big)\Delta \boldsymbol{s}{t+1}=g_{t+1}\left(b_i^a, b_i^g\right)Δst+1=gt+1(bia,big)

从初始值为零开始，利用链式法则通过JfJ_fJf递归计算对应的雅可比矩阵Jgt+1=[Jgt+1a,Jgt+1g]J_{g_{t+1}}=[J_{g_{t+1}}^a,J_{g_{t+1}}^g]Jgt+1=[Jgt+1a,Jgt+1g]：
Jgt+1a=JfsJgta−JfaJ_{g_{t+1}}^a=J_f^s J_{g_t}^a-J_f^aJgt+1a=JfsJgta−Jfa
Jgt+1g=JfsJgtg−JfgJ_{g_{t+1}}^g=J_f^s J_{g_t}^g-J_f^gJgt+1g=JfsJgtg−Jfg

最终，gtjg_{t_j}gtj的雅可比矩阵记为JgJ^gJg和JaJ^aJa。偏置的微小变化可表示为相对于线性化点的增量：bia=b‾ia+ϵab_i^a=\overline{b}_i^a+\epsilon^abia=bia+ϵa，big=b‾ig+ϵgb_i^g=\overline{b}_i^g+\epsilon^gbig=big+ϵg，则Δs\Delta \boldsymbol{s}Δs可近似为：
Δs~(bia,big)=Δs(b‾ia,b‾ig)⊕(Jaϵa+Jgϵg)\Delta \tilde{\boldsymbol{s}}(b_i^a,b_i^g)=\Delta \boldsymbol{s}(\overline{b}_i^a,\overline{b}_i^g)\oplus (J^a\epsilon^a+J^g\epsilon^g)Δs~(bia,big)=Δs(bia,big)⊕(Jaϵa+Jgϵg)

其中，Δs~=(ΔR~,Δv~,Δp~)\Delta \tilde{\boldsymbol{s}}=(\Delta \tilde{\boldsymbol{R}},\Delta \tilde{\boldsymbol{v}},\Delta \tilde{\boldsymbol{p}})Δs~=(ΔR~,Δv~,Δp~)。进而计算残差：
rΔR=log(ΔR~Rj⊤Ri)r_{\Delta \boldsymbol{R}}=\text{log} \left(\Delta \tilde{\boldsymbol{R}} \boldsymbol{R}_j^\top \boldsymbol{R}i\right)rΔR=log(ΔR~Rj⊤Ri)
rΔv=Ri⊤(vj−vi−gΔt)−Δv~r{\Delta \boldsymbol{v}}=\boldsymbol{R}_i^\top\left(\boldsymbol{v}_j-\boldsymbol{v}i-\boldsymbol{g} \Delta t\right)-\Delta \tilde{\boldsymbol{v}}rΔv=Ri⊤(vj−vi−gΔt)−Δv~
rΔp=Ri⊤(pj−pi−12gΔt2)−Δp~r{\Delta \boldsymbol{p}}=\boldsymbol{R}_i^\top\left(\boldsymbol{p}_j-\boldsymbol{p}_i-\frac{1}{2} \boldsymbol{g} \Delta t^2\right)-\Delta \tilde{\boldsymbol{p}}rΔp=Ri⊤(pj−pi−21gΔt2)−Δp~

其中，g\boldsymbol{g}g为重力向量，R\boldsymbol{R}R和p\boldsymbol{p}p分别为TWI\boldsymbol{T}{WI}TWI的旋转和平移分量。这些残差需通过合适的协方差矩阵加权，该矩阵也可通过递归计算得到：从Σti=0\Sigma{t_i}=\boldsymbol{0}Σti=0开始，进行如下更新：
Σt+1=JfsΣt(Jfs)⊤+JfaΣa(Jfa)⊤+JfgΣg(Jfg)⊤\Sigma_{t+1}=J_f^s \Sigma_t (J_f^s)^\top+J_f^a \Sigma^a (J_f^a)^\top+J_f^g \Sigma^g (J_f^g)^\topΣt+1=JfsΣt(Jfs)⊤+JfaΣa(Jfa)⊤+JfgΣg(Jfg)⊤

其中，Σa\Sigma^aΣa和Σg\Sigma^gΣg为对角矩阵，包含加速度计和陀螺仪的硬件相关噪声参数。关于IMU的底层物理模型和预积分理论的详细信息，可参考相关文献。

4.2.4 优化与部分边缘化

对于每个新帧，最小化由重投影项、IMU项和边缘化先验EmE_mEm组成的非线性能量：
E=∑i∈Pt∈obs(i)rit⊤Σit−1rit+∑(i,j)∈Crij⊤Σij−1rij+EmE=\sum_{\substack{i\in \mathcal{P}\\ t\in obs(i)}}r_{it}^\top \Sigma_{it}^{-1}r_{it}+\sum_{(i,j)\in \mathcal{C}}r_{ij}^\top \Sigma_{ij}^{-1}r_{ij}+E_mE=i∈Pt∈obs(i)∑rit⊤Σit−1rit+(i,j)∈C∑rij⊤Σij−1rij+Em

重投影误差对所有点集P\mathcal{P}P求和，且对每个点iii，对其所有观测帧集obs(i)obs(i)obs(i)（包括主帧）求和；集合C\mathcal{C}C包含由IMU因子连接的帧对。

采用高斯-牛顿算法优化能量EEE，为限制问题规模，在每次迭代中固定优化的关键帧位姿和连续状态的数量。添加新帧后，sk\boldsymbol{s}_ksk中包含nnn个仅含位姿的关键帧，sf\boldsymbol{s}_fsf中包含mmm个最新帧（包括新增帧）；优化完成后，对状态进行部分边缘化，防止问题规模持续扩大。

边缘化的两种可能场景见图5：第一种是边缘化最旧的非关键帧，此时舍弃以该帧为目标帧的路标因子，保持问题的稀疏性；第二种是出现新关键帧，此时对该帧的速度、偏置，以及一个旧关键帧及其对应的路标进行边缘化。

图4 第四节提出的视觉-惯性里程计子系统。左侧为路标投影（逆距离采用颜色编码），用于估计当前帧的位置；右侧为局部视觉-惯性光束平差的结果。蓝色为关键帧位姿及关联的路标，红色为当前状态及估计的轨迹。如第五节所述，利用一组非线性因子近似局部窗口中关键帧位姿的信息，并将其重用于全局建图。

两种场景下，均在待移除变量的线性化马尔可夫毯上进行边缘化（马尔可夫毯为与这些变量相邻的状态集合），线性化后的H\boldsymbol{H}H和b\boldsymbol{b}b表示增量ξ\boldsymbol{\xi}ξ向量空间中估计状态的分布。

将增量ξ=[ξα⊤,ξβ⊤]⊤\boldsymbol{\xi}=[\boldsymbol{\xi}\alpha^\top,\boldsymbol{\xi}\beta^\top]^\topξ=[ξα⊤,ξβ⊤]⊤分为保留在系统中的变量ξα\boldsymbol{\xi}\alphaξα和待边缘化的变量ξβ\boldsymbol{\xi}\betaξβ，利用舒尔补计算新分布的参数：
Hααm=Hαα−HαβHββ−1Hβα\boldsymbol{H}{\alpha \alpha}^m=\boldsymbol{H}{\alpha \alpha}-\boldsymbol{H}{\alpha \beta} \boldsymbol{H}{\beta \beta}^{-1} \boldsymbol{H}{\beta \alpha}Hααm=Hαα−HαβHββ−1Hβα
bαm=bα−HαβHββ−1bβ\boldsymbol{b}{\alpha}^m=\boldsymbol{b}{\alpha}-\boldsymbol{H}{\alpha \beta} \boldsymbol{H}{\beta \beta}^{-1} \boldsymbol{b}{\beta}bαm=bα−HαβHββ−1bβ

其中，原始的H\boldsymbol{H}H和b\boldsymbol{b}b按如下方式分块：
H=[HααHαβHβαHββ],b=[bαbβ]\boldsymbol{H}=\begin{bmatrix}\boldsymbol{H}{\alpha \alpha} & \boldsymbol{H}{\alpha \beta} \\ \boldsymbol{H}{\beta \alpha} & \boldsymbol{H}{\beta \beta}\end{bmatrix}, \quad \boldsymbol{b}=\begin{bmatrix}\boldsymbol{b}{\alpha} \\ \boldsymbol{b}{\beta}\end{bmatrix}H=[HααHβαHαβHββ],b=[bαbβ]

此时，Hααm\boldsymbol{H}{\alpha \alpha}^mHααm和bαm\boldsymbol{b}{\alpha}^mbαm定义了仅依赖于ξα\boldsymbol{\xi}_\alphaξα的能量项，可添加至下一次迭代的总能量中。

采用一阶估计雅可比矩阵维持线性化边缘化先验的零空间特性：一旦变量被纳入边缘化先验，其线性化点便固定，计算H\boldsymbol{H}H和b\boldsymbol{b}b所用的雅可比矩阵在该线性化点处求解，而残差在当前状态估计值处求解。当ξα\boldsymbol{\xi}\alphaξα与计算bαm\boldsymbol{b}{\alpha}^mbαm中残差所用的状态偏差为δα\delta_\alphaδα时，边缘化项中的残差需进行线性近似，因此添加到高斯-牛顿优化中的项为bαm+Hααmδα\boldsymbol{b}{\alpha}^m+\boldsymbol{H}{\alpha \alpha}^m \delta_\alphabαm+Hααmδα，而非直接使用bαm\boldsymbol{b}_{\alpha}^mbαm。

图5 因子图。(a) 边缘化一帧后，系统包含nnn个旧关键帧K1⋯KnK_1\cdots K_nK1⋯Kn和m−1m-1m−1个最新帧F1F_1F1、F2F_2F2（这些帧也可能承载路标，因此为关键帧）。添加新帧后，对最旧的速度vvv和最旧的偏置bbb进行边缘化。(b) 若速度和偏置不属于关键帧，则对整个帧（包括其位姿TTT）进行边缘化。© 若速度和偏置属于关键帧，则选择另一个关键帧进行边缘化，包括其承载的路标和位姿。两种场景下，均舍弃以被边缘化帧为目标帧的重投影因子；后一种场景下，舍弃从被边缘化帧到F2F_2F2的重投影因子，以允许重新线性化。注：图中未展示重投影因子所有可能的主帧-目标帧组合。

五、视觉-惯性建图

第四节提出的视觉-惯性里程计固定滞后平滑方法，因优化窗口外的线性化点固定，估计结果会累积漂移。消除该漂移的典型方法是检测回环，并将回环检测约束融入优化。本文提出分层方法：底层运行视觉-惯性里程计，视觉-惯性建图层运行光束平差法，并额外利用浓缩了里程计层关键帧位姿信息的非线性因子。光束平差法优化关键帧的相机位姿和关键点的位置，通过关键点匹配隐式检测回环，实现全局一致的建图。

5.1 全局地图优化

为获取统计独立的观测值，在全局地图优化中检测并匹配关键帧间的ORB特征（与VIO层的特征不同），由此可使用公式(8)定义的重投影误差函数。将该重投影误差与恢复的非线性因子的误差项结合，得到目标函数：
EG(s)=∑i∈Pt∈obs(i)rit⊤Σit−1rit+Enfr(s)E^G(\boldsymbol{s})=\sum_{\substack{i\in \mathcal{P}\\ t\in obs(i)}}r_{it}^\top \Sigma_{it}^{-1}r_{it}+E_{nfr}(\boldsymbol{s})EG(s)=i∈Pt∈obs(i)∑rit⊤Σit−1rit+Enfr(s)

其中，Enfr(s)E_{nfr}(\boldsymbol{s})Enfr(s)为恢复的非线性因子的误差项之和，这些因子及其恢复方法将在下文详细阐述。全局优化层的优化状态s\boldsymbol{s}s包括关键帧位姿和新路标的位置（采用4.2.1节的参数化方式）。

全局地图优化与VIO层在关键帧位姿处交互：当关键帧从VIO层被边缘化时，保存其马尔可夫毯的线性化结果（图5©），并对除关键帧位姿外的所有变量进行边缘化；从该边缘化先验中，恢复一组关于关键帧位姿的非线性因子，以近似其中存储的分布。

图6 非线性因子恢复的可视化。左：从VIO中移除关键帧位姿前，由边缘化得到的稠密连接因子；右：提取的非线性因子，用于近似原始因子中存储的分布。

5.2 非线性因子恢复

非线性因子恢复（NFR）利用一组新的非线性因子，近似原始因子图中线性化马尔可夫毯所存储的稠密分布，使新因子图具有稀疏的拓扑结构。非线性因子恢复的初始目标是控制SLAM优化的计算复杂度，而本文将其用于将VIO过程中累积的信息传递至全局一致的视觉-惯性地图优化中。

对非线性最小二乘问题的残差函数进行线性化，可得到多元高斯分布p(s)∼N(μo,Ho−1)p(\boldsymbol{s}) \sim \mathcal{N}(\mu_o, \boldsymbol{H}_o^{-1})p(s)∼N(μo,Ho−1)，其中均值μo\mu_oμo等于状态估计值。本文需构建另一个分布pa(s)∼N(μa,Ha−1)p_a(\boldsymbol{s}) \sim \mathcal{N}(\mu_a, \boldsymbol{H}_a^{-1})pa(s)∼N(μa,Ha−1)，以更优的稀疏因子图拓扑结构近似原始分布。

遵循非线性因子恢复的方法，最小化恢复分布与原始分布之间的KL散度，形式化表示为：
DKL(p(s)∣∣pa(s))=12(⟨Ha,Σo⟩−logdet⁡(HaΣo)+∥Ha12(μa−μo)∥2−d)\begin{aligned} D_{KL}(p(\boldsymbol{s})||p_a(\boldsymbol{s}))=&\frac{1}{2}\left( \langle \boldsymbol{H}_a,\Sigma_o \rangle -log \det(\boldsymbol{H}_a\Sigma_o)+\right.\\ &\left.\left\| \boldsymbol{H}_a^{\frac{1}{2}}(\mu_a-\mu_o)\right\|^2-d\right) \end{aligned}DKL(p(s)∣∣pa(s))=21(⟨Ha,Σo⟩−logdet(HaΣo)+ Ha21(μa−μo) 2−d)

其中，Σo=Ho−1\Sigma_o=\boldsymbol{H}_o^{-1}Σo=Ho−1，ddd为常数。

对于待恢复的第iii个非线性因子，需定义残差函数满足ri(s,zi)=ϵr_i(\boldsymbol{s}, z_i)=\epsilonri(s,zi)=ϵ（其中ϵ∼N(0,Hi−1)\epsilon \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{H}_i^{-1})ϵ∼N(0,Hi−1)）。非线性因子恢复会估计因子的伪测量值ziz_izi和信息矩阵Hi\boldsymbol{H}_iHi。选择ziz_izi使得ri(μo,zi)=0r_i(\mu_o, z_i)=0ri(μo,zi)=0，可推导出μa=μo\mu_a=\mu_oμa=μo，从而使公式中的第三项消去。为估计Hi\boldsymbol{H}_iHi，定义：
Jr=[⋮Ji⋮],Hr=[⋱0Hi0⋱]J_r=\begin{bmatrix}\vdots \\ J_i \\ \vdots\end{bmatrix}, \quad \boldsymbol{H}_r=\begin{bmatrix}\ddots & & 0 \\ & \boldsymbol{H}_i & \\ 0 & & \ddots\end{bmatrix}Jr= ⋮Ji⋮ ,Hr= ⋱0Hi0⋱

其中，JrJ_rJr为各定义残差函数关于状态的雅可比矩阵的堆叠，Hr\boldsymbol{H}_rHr为块对角矩阵，由各残差函数对应的Hi\boldsymbol{H}_iHi组成。由此可将Ha\boldsymbol{H}_aHa表示为Ha=Jr⊤HrJr\boldsymbol{H}_a=J_r^\top \boldsymbol{H}_r J_rHa=Jr⊤HrJr，进而通过最小化以下式子恢复信息矩阵Hi\boldsymbol{H}iHi：
DKL(Hr)=⟨Jr⊤HrJr,Σo⟩−logdet⁡(Jr⊤HrJr)D{KL}\left(\boldsymbol{H}_r\right)=\langle J_r^\top \boldsymbol{H}_r J_r, \Sigma_o \rangle-log \det\left(J_r^\top \boldsymbol{H}_r J_r\right)DKL(Hr)=⟨Jr⊤HrJr,Σo⟩−logdet(Jr⊤HrJr)

相关研究表明，当JrJ_rJr为满秩可逆矩阵时，存在如下闭式解：
Hi=({JrΣoJr⊤}i)−1\boldsymbol{H}_i=\left(\left\{J_r \Sigma_o J_r^\top\right\}_i\right)^{-1}Hi=({JrΣoJr⊤}i)−1

其中，{⋅}i\{\cdot\}_i{⋅}i表示对应的对角块。

5.3 用于分布近似的非线性因子

当需要对关键帧进行边缘化时（图5©），保存当前的线性化结果，并对除关键帧位姿外的所有变量进行边缘化，得到一个将优化窗口中所有关键帧位姿稠密连接的因子。本文利用该因子恢复被边缘化关键帧与其他所有关键帧之间的非线性因子（图6），定义如下残差函数：
rrel(s,zrel)=log(zrelTj−1Ti)r_{rel }\left(\boldsymbol{s}, z_{rel }\right)=\text{log} \left(z_{rel } \boldsymbol{T}j^{-1} \boldsymbol{T}i\right)rrel(s,zrel)=log(zrelTj−1Ti)
rrp(s,zrp)=⌊zrpRi−1(0,0,−1)⊤⌋xyr{rp}\left(\boldsymbol{s}, z{rp}\right)=\left\lfloor z_{rp} \boldsymbol{R}i^{-1}(0,0,-1)^\top\right\rfloor{xy}rrp(s,zrp)=⌊zrpRi−1(0,0,−1)⊤⌋xy
rpos(s,zpos)=zpos−pir_{pos}(\boldsymbol{s},z_{pos})=z_{pos}-\boldsymbol{p}irpos(s,zpos)=zpos−pi
ryaw(s,zyaw)=⌊Rizyaw⌋yr{yaw}(\boldsymbol{s},z_{yaw})=\left\lfloor \boldsymbol{R}i z{yaw}\right\rfloor _{y}ryaw(s,zyaw)=⌊Rizyaw⌋y

其中，⌊⋅⌋xy\lfloor \cdot \rfloor_{xy}⌊⋅⌋xy表示取向量的x、y分量，zzz为线性化时刻由估计状态得到的恢复测量值。本文中，zrel=Ti−1Tj∈SE(3)z_{rel}=\boldsymbol{T}_i^{-1} \boldsymbol{T}j \in SE(3)zrel=Ti−1Tj∈SE(3)，zrp=Ri∈SO(3)z{rp}=\boldsymbol{R}i \in SO(3)zrp=Ri∈SO(3)，zpos=pi∈R3z{pos}=\boldsymbol{p}i \in \mathbb{R}^3zpos=pi∈R3，zyaw=Ri−1(1,0,0)⊤∈R3z{yaw }=\boldsymbol{R}_i^{-1}(1,0,0)^\top \in \mathbb{R}^3zyaw=Ri−1(1,0,0)⊤∈R3。

恢复待移除关键帧与VIO层当前所有关键帧之间的成对相对位姿因子，同时为该关键帧恢复横滚-俯仰因子、绝对位置因子和偏航因子（图6）。这使得雅可比矩阵JrJ_rJr为满秩可逆矩阵，因此可使用公式(30)恢复各因子的信息矩阵。

由于偏航角和绝对位置是VIO的4个不可观测状态，其相关信息仅来自初始位姿的先验。因全局地图无需该信息，故舍弃偏航因子和绝对位置因子，仅将相对位姿因子和横滚-俯仰因子用于地图优化。此时，能量项EnfrGE_{nfr}^GEnfrG为：
EnfrG(s)=∑(i,j)∈Rrij⊤Hijrij+∑i∈Pri⊤HiriE_{nfr}^G(\boldsymbol{s})=\sum_{(i, j) \in \mathcal{R}} r_{ij}^\top \boldsymbol{H}{ij} r{ij}+\sum_{i \in \mathcal{P}} r_i^\top \boldsymbol{H}_i r_iEnfrG(s)=(i,j)∈R∑rij⊤Hijrij+i∈P∑ri⊤Hiri

其中，R\mathcal{R}R为所有相对位姿因子的集合，P\mathcal{P}P为所有横滚-俯仰因子的集合。

六、实验评估

在EuRoC数据集上对本文方法进行评估，并与当前主流系统对比，分别验证VIO子系统和完整的视觉-惯性建图方法的性能。本文VIO子系统在局部帧窗口中进行优化，为每个跟踪帧提供位姿估计；建图系统对VIO选择的关键帧进行全局地图优化。采用绝对轨迹误差（ATE）的均方根（RMS）作为评价指标，评估前将估计结果与真实值进行对齐。

6.1 系统参数

在KLT跟踪阶段，将图像划分为网格大小为50像素的规则网格；对于每个未从先前帧跟踪到点的网格单元，提取FAST响应最优的特征点（若响应超过阈值）。在EuRoC数据集的分辨率下，系统在任意时刻均跟踪80~120个特征点。

在VIO层，使用包含7个旧关键帧（仅位姿）和3个最新时间状态（位姿、速度、偏置）的窗口；若与局部地图中当前跟踪点连接的KLT特征占比低于70%，则将最新的时间状态选为关键帧。

6.2 精度

评估结果汇总于表1。在视觉-惯性里程计方法的对比中，本文系统在10个序列中的8个序列上表现最优，最接近的竞品在5个序列上表现最优。

为评估建图部分的性能，将本文方法与视觉-惯性版本的ORB-SLAM对比，其视觉子系统与本文建图层所采用的方法非常相似（均使用ORB关键点），核心区别在于惯性信息的处理：ORB-SLAM使用关键帧间的预积分测量值，而本文使用浓缩了VIO层IMU和视觉跟踪信息的恢复非线性因子。

在"机器大厅"序列中，本文系统的性能显著优于ORB-SLAM，原因是该场景的环境尺度较大，导致关键帧的时间间隔也较大；在"Vicon房间"序列中，二者的性能差异较小，因为搭载相机的微型飞行器在小房间内的快速运动，使得关键帧的数量较多、时间间隔较小。

地图重建的定性结果见图1，本文系统能够重建与重力对齐的全局一致地图，即使在ORB特征检测无法匹配的片段中，也能恢复关键帧的位姿。

6.3 因子加权

为评估提取因子的重要性及其在最终建图结果中的合理加权方式，设计两种对比实现方案：第一种不使用任何因子，仅依赖基于ORB特征的光束平差法；第二种提取因子，但对所有因子使用单位权重（即公式(35)中Hij=Hi=I\boldsymbol{H}_{ij}=\boldsymbol{H}_i=\boldsymbol{I}Hij=Hi=I），这是位姿图优化中常用的方法。表1的评估结果表明，与上述两种方案相比，采用第五节方法恢复因子权重的系统，具有更高的精度和鲁棒性。

6.4 耗时分析

建图阶段耗时降低的主要原因是，与直接的IMU融合相比，全局优化的状态维度降低了2.5倍（无需估计速度和偏置）。在英特尔E5-1620 CPU（4核8线程）上对系统进行测试，实现了高度并行化，充分利用了所有可用的CPU资源。

在EuRoC序列上，VIO子系统处理单帧的平均耗时为7.83毫秒（MH 02序列耗时最长，为9.4毫秒；V1 03序列耗时最短，为5.5毫秒）；平均仅有11.5%的帧被选为关键帧并进入建图层处理。

建图层的耗时见表2，以MH 05序列为例（图1，2273帧立体图像，时长114秒），VIO子系统的总处理时间为19.2秒，建图层为9.7秒，处理速度约为实时播放的4倍。

表1 EuRoC数据集上估计轨迹的均方根绝对轨迹误差（单位：米）。上半部分为在局部窗口中优化、为每个相机帧估计位姿的VIO方法结果；下半部分为对所有关键帧操作、进行全局地图优化的建图方法结果。两种评估中，本文系统在多数序列上的误差均为最低，性能优于竞品。注：V2 03序列因其中一个相机存在400多帧缺失，未纳入对比。

表2 EuRoC序列上建图子系统的平均处理时间（单位：毫秒），按地图中的关键帧数量归一化。

七、结论

本文提出一种全新的视觉-惯性建图方法，将高精度的视觉-惯性里程计与全局一致的基于关键帧的光束平差法相融合。该方法采用分层框架，从VIO估计结果中逐次恢复非线性因子，以浓缩关键帧间累积的惯性和视觉信息。VIO层被构建为固定滞后平滑问题，在滑动窗口中优化一组活跃的近期帧，并将历史信息保存在边缘化先验中；当关键帧移出窗口并被边缘化时，提取并保留关键帧间累积的VIO信息，用于视觉-惯性建图。

在公共基准数据集上的实验表明，与将关键帧间的IMU预积分测量值用于建图的其他方法相比，本文系统的轨迹估计结果更优。该方法通过降低状态空间的维度，有望减少优化的计算成本，实现大规模的视觉-惯性建图。融合其他传感器模态的信息、将系统扩展至多相机场景，是未来的重要研究方向。

参考文献

（原文参考文献部分保持不变，此处略）