# OKVIS2：具备回环闭合功能的实时可扩展视觉-惯性SLAM系统
Stefan Leutenegger $^{1,2}$
摘要—尽管视觉-惯性同步定位与建图（VI-SLAM）技术已逐渐普及，但在机器人、增强现实和虚拟现实（AR/VR）领域，鲁棒且精准的状态估计仍是一项挑战。本文提出了一种完整的VI-SLAM系统，专门应对长路径回环闭合以及重复回环闭合带来的难题。一系列实验表明，该系统的性能达到了甚至部分超越了现有开源系统的水平。该算法的核心在于通过边缘化共同观测数据来创建位姿图边，这些边在回环闭合时又能灵活转化为路标和观测值。该系统采用实时估计器来优化规模受限的因子图，因子图包含观测值、惯性预积分误差项以及位姿图边；当需要时，它还能异步地利用同一因子图来优化更大的回环。
## I. 引言
估计位姿、速度等状态是移动机器人实现自主运行，或是支撑增强现实和虚拟现实（AR/VR）应用的基础能力。视觉约束与空间约束结合惯性测量单元（IMU）提供的时间约束，已被证明能带来极高的鲁棒性和精度，因此这类技术在AR/VR及移动机器人领域的各类商用系统中得到了广泛应用。
如何在保持实时运行的前提下，提升开源系统的鲁棒性与精度，一直是学界研究的重点。过去几年里，陆续涌现出多种性能出色的视觉-惯性里程计（VIO）和同步定位与建图（VI-SLAM）系统。值得注意的是，BASALT [^1]、Kimera [^2]、VINS-Fusion [^3]、ORB-SLAM3 [^4]等先进的VIO和VI-SLAM系统均属于紧耦合系统，即会同时考虑视觉与惯性测量数据及其内部状态；它们还采用了由关键点检测与跟踪、描述符匹配提供的显式稀疏数据关联方式（间接法）。
如何使系统适配任意环境（室内/室外）、传感器套件和运动特性，尤其是实时处理可能频繁出现的回环闭合问题，仍是一大挑战，这也是本文旨在解决的核心问题：具体而言，该方案从共同观测中构建位姿图边，将其作为实时估计器的组成部分，与视觉重投影误差、预积分惯性因子协同工作。这种方式能够扩大待优化状态的窗口范围，从而减轻固定旧状态带来的负面影响。回环闭合时，这个逐步构建的位姿图可与惯性因子、观测值一起进行异步优化；更重要的是，位姿图边能无缝转化为观测值，进而恢复旧的路标和重投影误差。此外，系统会在CPU上异步对关键帧运行轻量级的分割卷积神经网络（CNN），以剔除动态区域内的观测值。在EuRoC [^5]和[^6]等常用数据集上开展的一系列实验表明，OKVIS2的精度具有竞争力，部分指标甚至优于现有先进方法。
综上，本文的主要贡献如下：
- 我正在开发一款名为OKVIS2的具备实时处理能力的多摄像头视觉惯性SLAM系统，该系统支持地点识别与回环闭合功能，我计划在未来几个月内将其作为开源软件发布。
- 该算法能够从边缘化观测数据中构建位姿图因子，这些因子既会被纳入实时跟踪估计器中以扩大优化窗口，也会用于异步运行的回环闭合优化过程。
- 我们设计了一套方案：利用轻量级的语义分割CNN来移除动态物体（行人、云朵等），且该模块可在CPU上运行以实现极高的可移植性。
本文其余部分的结构如下：第二节将回顾最相关的前期工作，第三节简要介绍文中使用的符号，第四节给出系统概述。第五节和第六节分别介绍估计器与前端方面的创新成果，实验结果则在第七节中呈现。
### 二、相关工作
我们简要概述了与实时基于视觉的视觉惯性里程计及SLAM最相关的前期研究。
### A. 视觉里程计与SLAM
早期仅依赖视觉的SLAM系统采用扩展卡尔曼滤波框架下的稀疏观测数据[^7]，但后续系统很快转向使用非线性最小二乘优化方法，因为其计算效率与精度更优。
---
PTAM[^8]，以及后续的ORB-SLAM 1[^9]和2[^10]均采用某种形式的窗口化光束法平差技术，后者还结合了重定位功能以及回环闭合时的位姿图优化。上述系统均采用关键点检测与跟踪和/或描述符匹配方式来确定显式对应关系。
DTAM[^11]提出了一种新范式：它可重建稠密深度帧，并通过逐像素直接光度对齐来实现跟踪，若在GPU上运行还能保持实时处理能力。因此DTAM能提供更丰富的地图表示，不过其跟踪精度略逊于稀疏系统。为了在计算效率与直接光度对齐之间取得平衡，LSD-SLAM[^12]仅对对比度足够的像素进行深度重建，同时它还具备回环闭合检测与优化功能，因此能构成一套高效的基于CPU的全SLAM系统。最后，直接光度对齐的概念也被应用于其他稀疏SLAM系统，如SVO[^13]和DSO[^14]，后者最终还增加了立体视觉[^15]与回环闭合[^16]扩展功能。
### B. 基于深度相机的稠密SLAM
RGB-D（深度）相机的出现推动了密集SLAM系统的发展。KinectFusion [^17] 通过为实时帧中的每个像素应用迭代最近点（ICP）算法，来重建体素化的截断有符号距离场（TSDF）地图，并在跟踪过程中执行几何对齐。随后出现了多种旨在提升空间扩展性的尝试，例如 [^18]、[^19]。人们也探索了其他的地图表示方法，比如表面元素，这使得ElasticFusion能够纳入回环闭合功能 [^20]。所有这些系统的共同点是跟踪（在地图固定的情况下优化位姿）与建图（固定位姿并将新信息融合到地图中）交替进行，而这很可能是大规模场景下轨迹精度较差的原因。不过需要注意的是，密集建图后端可以与其他系统结合使用，比如稀疏视觉-惯性SLAM系统。
### C. 视觉-惯性里程计与SLAM
在将视觉与惯性测量数据紧密耦合的早期尝试中，开创性成果 [^21] 提出了MSCKF，这是一种基于滤波的方法：该方法会保存多个历史状态，并通过边缘化路标的方式利用观测数据更新这些状态。此后衍生的许多方法提升了精度和计算效率，使得该技术非常适合应用于机器人、增强现实和虚拟现实（AR/VR）领域。OpenVINS提供了出色的开源实现 [^22]。ROVIO [^23] 也采用了轻量级的滤波方案，但它使用的是光度更新而非间接的关键点关联方式。不过，对于采用旧状态和路标边缘化策略的方案而言，回环闭合与大规模地图管理的紧密集成始终是一项固有挑战。流行的框架Maplab [^24] 尝试以更松散的方式整合大规模稀疏建图与视觉惯性里程计。
受纯视觉研究成功的启发，第二类视觉惯性里程计与视觉惯性SLAM系统采用非线性最小二乘优化方法，通常同时优化重投影误差和积分后的惯性测量数据，并采用某种窗口化方法来保证计算可行性。OKVIS [^25] 通过边缘化操作，将一组旧关键帧与线性化误差项一同保留来实现这一点。相反，扩展了 [^4] 的ORB-SLAM3 [^10] 则固定旧状态——这种方案更简单，更便于与回环闭合优化功能集成，但本质上并非保守近似，因为它实际上忽略了过去的估计不确定性。为了更严谨地评估固定操作引入的误差，[^26] 分析了残差统计特性，并自适应地选择固定方式，从而生成可优化的收缩/扩张状态窗口。本研究旨在通过将更大的旧关键帧窗口纳入优化问题来缓解固定操作可能带来的弊端，该窗口由相对位姿因子约束，而非仅依赖计算成本高昂的观测数据。
值得注意的是，多传感器里程计与SLAM系统从开源的因子图优化库中获益良多，其中最著名的包括g2o [^27]、Ceres [^28] 和GTSAM [^29]。后者还集成了iSAM 1 [^30] 和 2 [^31]，能够以固有的增量方式高效求解视觉惯性SLAM中常见的全批量因子图。
VINS-Fusion [^3] 的工作方式类似于OKVIS，但它通过位姿图优化来实现闭环检测。在这种优化中，仅由连续关键帧构成的相对位姿边是通过对视觉与惯性测量值进行边缘化得到的。除了多摄像头和IMU外，该系统还支持磁力计和GPS测量数据。相比之下，本文更严格地基于视觉共观测来构建位姿图边，并将其用于实时逐帧跟踪估计。
同样，Kimera [^2] 是一款先进的稀疏VI-SLAM系统，也支持稠密网格映射；它将VIO前端与用于闭环检测的位姿图优化后端相结合，二者均依赖GTSAM框架。
BASALT [^1] 采用了与本文提出的思路类似的方法。从联合观测和惯性测量数据中恢复连接多个关键帧的非线性因子。不过与之不同的是，本文保留了预积分的惯性因子，仅计算两个关键帧共有的观测所带来的相对位姿误差。
### III. 符号与定义
VI-SLAM问题是指利用安装在运动物体上的IMU和N个摄像头，相对于静态世界坐标系$\underline{\mathcal{F}}_{W}$来跟踪该物体的运动。IMU的坐标系记为$\underline{\mathcal{F}}_{S}$，摄像头坐标系记为$\underline{\mathcal{F}}_{C_{i}}, i=1\ldots N$。
左侧下标表示坐标表示方式。齐次位置向量（用斜体表示）可通过$T_{AB}$进行变换，即$A\boldsymbol{r}_{P} = T_{AB\,B}\boldsymbol{r}_{P}$。
采用以下状态表示方式：
$$ \mathbf{x}=[_{W}\mathbf{r}_{S}^{T},\mathbf{q}_{W S}^{T},_{W}\mathbf{v}^{T},\mathbf{b}_{\mathrm{g}}^{T},\mathbf{b}_{\mathrm{a}}^{T}]^{T}, $$
---
Fig. 1: Overview of OKVIS2: the frontend takes a multi-frame and associated IMU messages to initialise the new state, match keypoints, triangulate new landmarks, and to try place recognition, relocalisation, and loop closure. The realtime estimator will then optimise the underlying factor graph and take care of posegraph maintenance and fixation of old states. A full graph optimisation around detected loops is carried out asynchronously and later synchronised.
其中$w_{\mathbf{r}_S}$表示$\underline{F}_S$的原点相对于$\underline{F}_W$的位置，$\mathbf{q}_{WS}$是描述$\underline{F}_S$相对于$\underline{F}_W$姿态的方向的哈密顿四元数，$w^\mathbf{v}$则表示$\underline{F}_S$相对于$\underline{F}_W$的速度。此外，还包括陀螺仪偏差$\mathbf{b}_g$和加速度计偏差$\mathbf{b}_a$。每当我们获取到摄像头的帧数据时，就会在每个时间步k对状态进行估计。
对于四元数，我们采用围绕线性化点$\overline{\mathbf{q}}_{AB}$的乘法扰动来表示。
$$ \mathbf{q}_{A B}=\operatorname{E x p}\left(\delta\alpha_{A B}\right)\otimes\mathbf{\overline{{q}}}_{A B}, $$
其中$\text{Exp}(.)$代表四元数群指数映射，它可将（微小的）旋转向量$\delta\alpha_{AB} \in \mathbb{R}^3$转换为李代数。
### IV. 系统概述
VI-SLAM系统分为前端和实时估计器两部分，每当有新的（多帧）图像到达时，它们便会同步处理图像和IMU消息。为了处理闭环检测问题，系统会异步执行完整的因子图闭环优化。如图1所示，前端负责状态初始化、关键点匹配、立体三角测量（针对连续帧以及同一多帧中的立体图像）、分割CNN运行，还有地点识别；如果地点识别成功，还会进行重定位与闭环初始化。随后，实时估计器会对相应的因子图进行优化，它还负责通过边缘化旧观测数据来创建位姿图边，以及固定旧状态。发生闭环检测后，实时估计器会将位姿图边重新转化为观测数据，进而触发围绕闭环的完整因子图优化，该优化过程为异步执行，完成后会与实时因子图同步。
### V. 视觉-惯性估计器
下文将介绍视觉-惯性估计器的各个组成部分。前端部分以及视觉和惯性误差项大多借鉴自OKVIS [^25]。下文描述的非线性最小二乘代价函数通过谷歌的Ceres求解器 [^32] 进行最小化。
该估计器会最小化视觉、惯性和相对位姿误差（位姿图边），这些误差将在后续分别阐述。
### A. 重投影误差
我们使用第j个路标 $\mathbf{e}_{r}^{i,j,k}$ 在时间步k投影到第i个相机图像上的标准重投影误差 $w^{l}$：
$$ \begin{array}{r}{\mathbf{e}_{\mathrm{r}}^{i,j,k}=\tilde{\mathbf{z}}_{\mathbf{r}}^{i,j,k}-\mathbf{h}(T_{S C_{i}}^{-1}T_{S^{k}W}w l^{j}),}\end{array} $$
其中关键点检测结果为 $\tilde{\mathbf{z}}_{\mathbf{r}}^{i,j,k}$，$\mathbf{h}(\cdot)$ 表示相机投影模型，本研究中采用针孔投影模型，也可选择加入畸变校正（径向-切向畸变或等距畸变）。此处省略雅可比矩阵，因为它们与 [^25] 中的表述类似。
### B. 惯性测量单元误差
我们使用时间实例k和n之间的惯性测量单元误差 $\mathbf{e}_s^{k_0}$：
$$ \mathbf{e}_{\mathbf{s}}^{k}=\hat{\mathbf{x}}^{n}(\mathbf{x}^{k},\tilde{\mathbf{z}}_{\mathbf{s}}^{k,n}))\boxminus\mathbf{x}^{n}\in\mathbb{R}^{15}, $$
其中 $\hat{\mathbf{x}}^{n}(\mathbf{x}^{k})$ 表示基于估计值 $\mathbf{x}^{k}$ 以及两帧之间的惯性测量数据（陀螺仪和加速度计读数）$\tilde{\mathbf{z}}_{s}^{k,n}$ 得到的步骤n处的预测状态。对于四元数以外的所有状态，$\boxminus$ 运算符即为普通的减法运算；对于四元数而言，其定义为：
$$ \mathbf{q}\boxminus\mathbf{q}^{\prime}=\operatorname{L o g}\left(\mathbf{q}\otimes\mathbf{q}^{{^{\prime}-1}}\right), $$
其中Log(.)表示四元数群对数，它实现了从李代数到旋转向量的转换。
我们采用了借鉴自 [^33] 的预积分方案来构建该误差项，从而使得无论使用多少组惯性测量数据，都能高效完成误差计算。
### C. 相对位姿误差
此外，我们还使用时间步r和k之间的相对位姿误差 $\mathbf{e}_{\mathrm{p}}^{k,r}$：
$$ \begin{array}{r}{\mathbf{e}_{\mathrm{p}}^{r,c}=\mathbf{e}_{\mathrm{p},0}^{r,c}+\left[\begin{matrix}{s^{r}\mathbf{r}_{S^{c}}-s^{r}\tilde{\mathbf{r}}_{S^{c}}}\\ {\mathbf{q}_{S^{r}S^{c}}\boxminus\tilde{\mathbf{q}}_{S^{r}S^{c}}}\\ \end{matrix}\right],}\end{array} $$
其中 $\mathbf{e}_0^{r,c}$ 表示一个常数（下文会详细说明），$S^{k}\tilde{\mathbf{r}}_S^r$ 和 $\tilde{\mathbf{q}}_S^{c}S^r$ 分别表示名义上的相对位置和姿态（以参考惯性测量单元坐标系 $\mathbf{\tilde{F}}_S^r$ 为基准表示）。
---
### D. 实时估计问题
至少以相机帧率运行的实时估计器会最小化下述非线性最小二乘代价函数。
$$ \begin{aligned}c(\mathbf{x})&=\frac{1}{2}\sum_{i}\sum_{k\in\mathcal{K}}\sum_{j\in\mathcal{J}(i,k)}\rho\left(\mathbf{e}_{\mathrm{r}}^{i,j,k^{T}}\mathbf{W}_{\mathrm{r}}\mathbf{e}_{\mathrm{r}}^{i,j,k}\right)\\&+\frac{1}{2}\sum_{k\in\mathcal{P}\cup\mathcal{K}\setminus f}\mathbf{e}_{\mathrm{s}}^{k^{T}}\mathbf{W}_{\mathrm{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}_{\mathrm{p}}^{r,c}\mathbf{e}_{\mathrm{p}}^{r,c}.\end{aligned} $$
此处 $\mathbf{W}_r$ 表示视觉权重，即重投影误差的协方差逆；$\mathbf{W}_s^k$ 表示惯性测量单元误差权重。此外，我们对观测值使用了柯西鲁棒函数 $\rho(\cdot)$。集合 $\mathcal{K}$ 包含了所有能观测到相应路标的位姿，这些路标属于集合 $\mathcal{I}(i, k)$。$\mathcal{K}$ 包含最近的T帧以及过去的M个关键帧；帧f表示当前帧。集合 $\mathcal{P}$ 包含位姿图帧，即通过原始共视关系得到的具有相对位姿误差的帧。权重 $\mathbf{W}_p^{r,c}$ 是通过对旧观测值 $\mathcal{K}$ 进行边缘化计算得出的。需要注意的是，$\mathcal{P}$ 所涵盖的时间范围比有观测值的帧要久远得多。集合 $\mathcal{C}(r) \subset \mathcal{P}$ 描述了与帧r通过位姿图边相连的所有位姿图帧。惯性测量单元误差针对所有连续的（关键）帧k、n进行计算。惯性测量单元误差权重 $\mathbf{W}_s^k$ 是根据惯性测量单元误差（预）积分过程中的线性化误差传播结果得出的。
根据与当前活动关键帧的共视性准则，会选取一个新帧作为关键帧，具体细节详见第VI-B节。
如果最近T个帧中最旧的帧不是关键帧，那么相应的状态会被直接移除，而IMU测量值会按照预积分方案被追加到上一帧的IMU误差中，直至下一帧为止。不过，如果最近T个帧中最旧的帧是关键帧，我们就会采用下文第V-D.1节详述的姿态图构建方案。
图2直观展示了随时间推移底层因子图的构建过程，相关细节将在下文进一步说明。
需要注意的是，虽然此处不再赘述，但我们也可以选择将相机外参$T_{SC_i}$设为待优化的变量，也就是进行在线标定。
1) 姿态图构建：为了控制问题的复杂度，每当关键帧数量$|\mathcal{K}|$超过阈值K时，那些与当前帧或当前关键帧共视性最弱的帧就会通过边缘化共同观测数据的方式，从$\mathcal{K}$转移到$\mathcal{P}$，同时插入形式为(6)的相对姿态误差项。当前关键帧是指与当前（最新）帧视觉重叠度最高的帧（参见第VI节）。
有一个重要的例外情况：只要当前帧或当前关键帧与某个最旧的关键帧存在共同观测数据，该最旧关键帧就会被保留下来，以便维持一些长期的共视关系，从而帮助保持准确的方向估计。
(a) In the beginning.
(b) Later: posegraph creation.
Fig. 2: Initially a full batch VI factor graph (Fig. 2a) is created and optimised. Later (Fig. 2b), frames with least overlap with the live frame and current keyframe are turned into posegraph poses by construction of relative pose errors under marginalisation of common observations; also, old poses and speed/bias variables are fixed (dashed lines) to keep the problem realtime capable. When a loop-closure occurs (Fig. 2c), respective observations and landmarks are re-activated.
为了仅生成可处理的姿态图边子集，我们采用了下述启发式方法，图3对此进行了可视化说明。
- 从有观测数据的位姿集合（即$\mathcal{K}$中的位姿）中，挑出已经连接了姿态图边的位姿，将其存入集合$\mathcal{T}$中。
- 用位姿索引r来表示需要转换为姿态图节点的状态，将r插入集合T中。
- 将与r视觉重叠度最高的帧插入集合T中。
- 以两个位姿之间的共观测数量为评判标准，在集合T上计算最大生成树（MST）。
- 现在，按照下述方案创建所有连接到位姿r的MST边，该方案会利用两个帧r和c的所有共同观测数据。
需要注意的是，如果某个路标在多于两个帧（即r和c）中被观测到，这种方法最终会导致观测数据和路标的重复，这本身就是设计上的不一致性。不过有观点认为，这种近似方法仍能产生比直接使用单位权重矩阵来创建相对姿态误差更好的结果，而这正是视觉（惯性）SLAM系统中的事实标准……
具体来说，我们首先将路标转换到参考坐标系$\mathbf{\mathcal{F}}_{S^{r}}$下，也就是索引为r的IMU坐标系下。我们暂时将状态仅视为$\mathbf{p} = [S_{r} \mathbf{r}_{S_{c}}, \mathbf{q}_{S_{r}S_{c}}]$。因此，投影误差会…
---
(a) Keyframe pose r selected for conversion into posegraph node.
(b) Computation of the Maximum Spanning Tree (MST).
Fig. 3: Posegraph edge creation scheme: when keyframe r is selected for conversion into a posegraph node, because of least overlap with the current (key-)frame (Fig. 3a), a Maximum Spanning Tree (MST) is computed 3b) from the poses in the set $\mathcal{T}$ (nodes with posegraph edges and node with most visual overlap), after which all MST edges connected to r are created (Fig. 3c).
分别变为参考帧索引r和另一帧索引c下的投影误差。
$$ \begin{array}{r l r}{{\bf e}_{\mathrm{r}}^{i,j,r}}&{{}=}&{\tilde{\bf z}_{\bf r}^{i,j,r}-{\bf h}({\cal T}_{S C_{i}\setminus S^{r}}^{-1}{\cal l}^{j}),}\end{array} $$
$$ \begin{array}{r l r}{{\bf e}_{\mathrm{r}}^{i,j,c}}&{=}&{\tilde{\bf z}_{\bf r}^{i,j,c}-{\bf h}(T_{S C_{i}}^{-1}T_{S^{c}S^{r}\;S^{r}}l^{j}).}\end{array} $$
采用路标边缘化来处理所有共同观测数据，我们由此得到相应的联合代价。
$$ c_{\mathrm{p}}^{r,c}=\frac{1}{2}\sum_{i=1}^{N}\sum_{\substack{j\in\mathcal{J}(i,r)}}\mathbf{e}_{\mathrm{r}}^{i,j,r^{T}}\mathbf{W}_{\mathrm{r}}\mathbf{e}_{\mathrm{r}}^{i,j,r}+\sum_{\substack{j\in\mathcal{J}(i,c)}}\mathbf{e}_{\mathrm{r}}^{i,j,c^{T}}\mathbf{W}_{\mathrm{r}}\mathbf{e}_{\mathrm{r}}^{i,j,c}, $$
其中，集合$\mathcal{J}(i,r)$和$\mathcal{J}(i,c)$分别表示在时间步r和c时可见、在相应图像i中有测量值且初始重投影误差较小的所有路标。现在我们可以将这一代价近似表示为：
$$ \begin{array}{r l r}{c_{\mathrm{p}}^{r,c}}&{\approx}&{\frac{1}{2}\mathbf{e}_{\mathrm{p}}^{r,c T}\mathbf{W}_{\mathrm{p}}^{r,c}\mathbf{e}_{\mathrm{p}}^{r,c},}\end{array} $$
$$ \begin{array}{r l r}{{\bf e}_{\mathrm{p}}^{r,c}}&{=}&{{\bf e}_{\mathrm{p},0}^{r,c}+\left[\mathbf{\Sigma}_{S}^{r}{\bf r}_{S^{c}}-S^{r}\tilde{\bf r}_{S^{c}}\atop\mathbf{q}_{S^{r}S^{c}}\boxminus\tilde{\bf q}_{S^{r}S^{c}}\right],}\end{array} $$
这里，$s^{r}\tilde{\mathbf{r}}_{S^{c}}$和$\tilde{\mathbf{q}}_{S^{r}S^{c}}$分别是相对位置和姿态，它们是在创建位姿图边时所选取的线性化点。为简洁起见，此处省略雅可比矩阵的推导过程；需要注意的是，当$\mathbf{q}_{S^{r}S^{c}}$趋近于$\tilde{\mathbf{q}}_{S^{r}S^{c}}$时，雅可比矩阵会变成单位矩阵。
式(11)中的其余项是通过对式(10)中所涉及的代价对应的高斯-牛顿系统应用舒尔补运算来计算的，以实现对路标的边缘化处理。首先，我们构建具有如下结构的高斯-牛顿系统：
$$ \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]. $$
请注意变量顺序：先为位姿(p)，随后是所有路标($s^+$P)。虽然此处省略了所需雅可比矩阵的推导过程，但它们可直接由重投影误差项(3)得出；值得注意的是，参考帧中观测到的路标无需进行坐标变换，因为它们本身即已用参考帧表示，因此相应的雅可比矩阵为零。
现在，利用舒尔补运算将路标边缘化：
$$ \begin{array}{r l r}{{\bf H}^{\ast}}&{=}&{{\bf H}_{{\mathrm{p,p}}}-\displaystyle\sum_{j}{\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}^{*}, $$
该系统现在围绕当前位姿p进行线性化，因此：
$$ \mathbf{H}^{*}\delta\mathbf{p}=\mathbf{b}^{*}-\mathbf{H}^{*}(\mathbf{p}\boxminus\tilde{\mathbf{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} = \tilde{\mathbf{p}}$处，此时$\mathbf{E}_{\mathrm{p}}^{r,c} = \mathbf{I}_{6}$）使其与式(17)等效，我们得到：
$$ \mathbf{W}_{\mathrm{p}}^{r,c}=\mathbf{H}^{*}, $$
$$ \mathbf{e}_{\mathrm{p},0}^{r,c}=-\mathbf{H}^{*+}\mathbf{b}^{*}. $$
由于$\mathbf{H}^*, \mathbf{b}^*$和$\tilde{\mathbf{p}}$均为常数，代入$T_{S^r S^c} = T_{WS^r}^{-1} T_{WS^c}$后，相对于时间步r和c处位姿的雅可比矩阵便很容易确定。
2) 旧状态的固定：此外，为了使被估计的位姿数量保持有限，仅保留最近的 A 个状态作为可变状态。A 的值由常数参数 $A_{\min}$ 和 $A_{\Delta T}$ 二者中的较大者确定，其中后者表示从当前帧向前回溯覆盖时间区间 $\Delta T$ 的状态数量。
---
### E. 地点识别、重新定位与闭环检测
每当当前帧在DBoW2 [^34]数据库中查询到匹配结果（比如位姿索引l），且通过3D-2D RANSAC几何验证后，当前活跃的状态和路标窗口就会被变换以与匹配到的位姿l对齐。接着，连接位姿l的位姿图边会被“激活”，重新转化为路标和观测值；同时也会为当前匹配帧创建新的观测值。如果新路标与l中已有的旧路标相匹配，还可能触发路标合并操作。这一流程会生成一组具有观测值的闭环帧$\mathcal{L}$；不过由于这些闭环帧往往位于遥远的过去，它们通常属于实时估计图中被固定的状态。至此，重新定位过程完成。
为了优化环路不一致性问题，采用了以下方案：首先，通过旋转平均将误差分布到环路周围，随后再进行位置不一致性分配，且这些操作均以均等的方式作用于环路周围的（关键帧）姿态。接着，会触发因子图的背景优化过程。在此过程中，会使用实时估计器图的副本，但会对状态进行不同的固定处理（即闭环内的状态仍保持可变）。需要注意的是，所有惯性测量单元误差项也会被纳入此次闭环优化中，从而增添更多约束条件，尤其是关于相对于重力的方向一致性方面的约束。优化完成后，同步进程会将优化后的状态和路标导入实时估计器，并重新对齐在此期间生成的新状态和路标。在优化期间，会暂停重定位尝试。优化结束后，闭环帧的数量 $|\mathcal{L}|$ 会被限制为一个常数 L，同时采用与 V-D.1 相同的位姿图边创建方案。
### VI. 前端概述
下文将对前端的其余组件进行简要介绍。
### A. 匹配与初始化
视觉前端会在多帧图像的每一帧中提取 BRISK 2 [^35]、[^36] 特征点和描述符，并将它们与地图中已有的 3D 路标进行匹配；匹配时会同时考虑描述符距离以及保守估算的投影图像距离。新的 3D 路标既可以通过关键帧内 N 幅图像之间的立体三角测量来初始化，也可以通过实时帧与任意关键帧之间的三角测量来初始化。
### B. 关键帧选择
是否将新帧判定为关键帧，取决于实时帧中匹配路标的比例 $o_i$，以及当前匹配点在任意现有关键帧（索引为 $k \in \mathcal{K}$）中可见的比例 $o_k$。具体而言，这些重叠比例是通过匹配特征点区域的数量除以相应（多图像）帧中所有特征点区域的总量来确定的；其中相应的特征点区域是由以特征点为中心、半径为 $r_{\mathrm{kpt}}$ 的填充圆的并集得到的。如果重叠程度
$$ o=\min(o_1,\max_{k\in\mathcal{K}}o_k), $$
低于某个阈值 $t_{o}$，我们就会将该（多）帧接受为新的关键帧。
### C. 语义分割 CNN
语义分割网络 Fast-SCNN [^37] 会在关键帧的所有图像上运行。该网络已在 Cityscapes 数据集 [^38] 上经过 80 个轮次的微调，微调过程中先将图像转换为灰度图，以便能更好地适配 OKVIS2 输入的灰度图像。为了实现最大的可移植性和灵活性，推理过程会在 CPU 上作为异步后台进程执行，由于网络本身效率较高且仅需处理关键帧，因此这一操作完全可行。推理完成后，那些几乎可以确定为动态变化区域的图像部分中的匹配点会被直接移除（目前仅包括天空，但通过适当的训练也可涵盖人物、动物等）。实验结果表明，该方案在场景内容缓慢移动的情况下能显著提升精度，尤其是云层这类情况，因为此时观测数据不会通过柯西鲁棒化机制自动被丢弃。具体分类示例请参见图 4。
Fig. 4: TUM-VI outdoors1 example sky classifications (red) vs. used keypoints: indoors (top), and outdoors (bottom). Notice how some erroneous sky classifications occur also indoors. Further it can be seen that most of the sky keypoints are identified correctly outdoors, but there are some false negatives (on the rightmost cloud) and false positives (close to the horizon, especially on the leaf-less tree branches).
---
### VII. 实验结果
在视觉惯性里程计/SLAM评估中，针对两个常用的数据集提供了相关结果：ETHZ-ASL EuRoC数据集[^39]和TUM VI基准测试集[^6]。这两个数据集都提供了真实的轨迹数据，但在摄像头、运动模式、（视觉）环境以及轨迹的空间范围方面各具特点。
作者认为有两个指标应当被分开考量。
- 第一个指标是关闭回环检测时的里程计漂移精度。此处采用了与[^25]中相同的方法，即按行驶距离区间统计相对运动误差（位置/姿态）。为完整起见，文中也给出了开启回环检测时的相对位置/姿态误差数据，但需注意此类结果高度依赖于运动模式，也就是回环检测发生的频率。
- 第二个指标是考虑回环闭合情况的绝对轨迹误差（ATE）。我们会将估计轨迹与真实轨迹在位置和偏航角上进行对齐（横滚角和俯仰角属于全局可观测量，因此无需对齐），再统计位置差异的相关数据。需要注意的是，我们既可以评估使用到对应时间戳为止的所有测量数据的估计结果（即因果性评估方式），也可以评估最终完成全部回环闭合、所有测量数据都被纳入考量的轨迹估计结果（即非因果性评估方式）。这两种方法均有效，但必须明确说明所用方式；另外要注意的是，在前一种情况下，里程计评估中已提及的漂移问题可能会主导ATE误差统计数据，因为轨迹在修正回环闭合时会呈现出特征性的“跳跃”现象。
整个过程中使用的估计器参数保持一致：保留最近$T = 3$帧数据，设置$K = 5$个关键帧以及$L = 5$个用于回环闭合的帧。此外，至少会对$A_{\min} = 12$个位姿图帧进行优化，同时所有时间不超过$\Delta T = 2$秒的帧也始终会被纳入优化范围。前端和惯性测量单元的参数因数据集而异，相应的配置文件会与代码一同发布。
### A. ETH-ASL EuRoC数据集
图5展示了EuRoC数据集上MH_05_difficult序列的相对误差统计数据，以此为例说明在VIO模式下，okvis2-vio（即禁用回环闭合功能）相较于原始OKVIS算法okvis的性能提升情况，以及回环闭合如何减少长期漂移现象。[^5] [^25]
同样可以明显看出，最终优化后的轨迹okvis2-slam-final采用非因果性评估方式时，其精度明显高于因果性版本okvis2-slam。
表I列出了绝对轨迹误差（ATE）的结果，其中也与视觉惯性里程计和视觉惯性SLAM领域的前沿算法进行了对比。
Fig. 5: Relative trajectory error statistics for different sub-trajectory lengths (y-axis) of the exemplary EuRoC MH_05_difficult sequence.
| BASALT $^{[1]}$ | Kimera $^{[2]}$ | OKVIS $^{[25]}$ | OKVIS2 VIO (ours) | VINSFusion $^{[3]}$ | OKVIS2 causal (ours) | ORB-SLAM3 $^{[4]}$ | OKVIS2 (ours) |
| Causal evaluation | Non-causal |
| VIO | VI-SLAM |
| MH_01 | 0.080 | 0.080 | 0.079 | 0.057 | 0.166 | 0.044 | 0.036 | 0.027 |
| MH_02 | 0.060 | 0.090 | 0.044 | 0.044 | 0.152 | 0.036 | 0.033 | 0.023 |
| MH_03 | 0.050 | 0.110 | 0.096 | 0.082 | 0.125 | 0.050 | 0.035 | 0.028 |
| MH_04 | 0.100 | 0.150 | 0.197 | 0.189 | 0.280 | 0.089 | 0.051 | 0.066 |
| MH_05 | 0.080 | 0.240 | 0.206 | 0.141 | 0.284 | 0.112 | 0.082 | 0.068 |
| V1_01 | 0.040 | 0.050 | 0.050 | 0.043 | 0.076 | 0.037 | 0.038 | 0.035 |
| V1_02 | 0.020 | 0.110 | 0.066 | 0.037 | 0.069 | 0.021 | 0.014 | 0.013 |
| V1_03 | 0.030 | 0.120 | 0.071 | 0.036 | 0.114 | 0.035 | 0.024 | 0.019 |
| V2_01 | 0.030 | 0.070 | 0.062 | 0.044 | 0.066 | 0.036 | 0.032 | 0.023 |
| V2_02 | 0.020 | 0.100 | 0.077 | 0.044 | 0.091 | 0.024 | 0.014 | 0.015 |
| V2_03 | - | 0.190 | 0.028 | 0.063 | 0.096 | 0.045 | 0.024 | 0.020 |
| Avg | - | 0.119 | 0.089 | 0.071 | 0.138 | 0.048 | 0.035 | 0.031 |
$^{1}$ 结果源自[^4]。
$^{2}$ 采用OKVIS2所用的惯性测量单元和前端参数，重新运行原版OKVIS VIO。
TABLE I: EuRoC Absolue Trajectory Error (ATE) in [m].
可以看出，其性能与ORB-SLAM3[^4]相当，甚至略优。
### B. TUM VI基准测试集
该系统也在TUM VI基准测试集[^6]上进行了测试，相应的ATE结果见表II。
由此可见，在较短的走廊和房间序列中，OKVIS2的性能与ORB-SLAM3相当；不过在较长的序列中，OKVIS2显然能展现出最佳效果。
---
| ROVIO [^23] $^{1}$ | BASALT [^1] $^{1}$ | OKVIS [^25] $^{2}$ | OKVIS2 VIO (ours) | OKVIS2 causal (ours) | ORB-SLAM3 [^4] $^{1}$ | OKVIS2 (ours) | length [m] | loop closures |
| Causal evaluation | Non-causal | | |
| VIO | VI-SLAM | | |
| corridor | 1 | 0.47 | 0.34 | 0.65 | 0.47 | 0.09 | 0.03 | 0.09 | 305 | ✓ |
| 2 | 0.75 | 0.42 | 0.70 | 0.49 | 0.06 | 0.02 | 0.06 | 322 | ✓ |
| 3 | 0.85 | 0.35 | 1.58 | 0.44 | 0.02 | 0.02 | 0.02 | 300 | ✓ |
| 4 | 0.13 | 0.21 | 0.15 | 0.11 | 0.18 | 0.21 | 0.18 | 114 | |
| 5 | 2.09 | 0.37 | 0.41 | 0.47 | 0.09 | 0.01 | 0.09 | 270 | ✓ |
| Avg | 0.86 | 0.34 | 0.70 | 0.40 | 0.09 | 0.06 | 0.09 | 262 | |
| magistrale | 1 | 4.52 | 1.20 | 2.91 | 1.79 | 0.09 | 0.24 | 0.03 | 918 | ✓ |
| 2 | 13.43 | 1.11 | 2.76 | 3.03 | 0.52 | 0.52 | 0.03 | 561 | ✓ |
| 3 | 14.80 | 0.74 | 1.08 | 1.66 | 0.93 | 1.86 | 0.93 | 566 | |
| 4 | 39.73 | 1.58 | 3.03 | 3.43 | 0.76 | 0.16 | 0.04 | 688 | ✓ |
| 5 | 3.47 | 0.60 | 1.18 | 1.36 | 0.17 | 1.13 | 0.01 | 458 | ✓ |
| 6 | - | 3.23 | 2.26 | 2.85 | 0.63 | 0.97 | 0.63 | 771 | (✓) |
| Avg | - | 1.41 | 2.20 | 2.35 | 0.52 | 0.81 | 0.28 | 660 | |
| outdoors | 1 | 101.95 | 255.04 | 109.75 | 22.60 | 24.92 | 32.32 | 24.92 | 2656 | |
| 2 | 21.67 | 64.61 | 20.26 | 12.35 | 8.84 | 10.42 | 0.04 | 1601 | (✓) |
| 3 | 26.10 | 38.26 | 21.37 | 22.97 | 27.14 | 54.77 | 27.14 | 1531 | |
| 4 | - | 17.53 | 14.05 | 9.68 | 7.43 | 11.61 | 7.42 | 928 | |
| 5 | 54.32 | 7.89 | 14.74 | 5.99 | 6.67 | 7.57 | 6.67 | 1168 | ✓ |
| 6 | 149.14 | 65.50 | 32.26 | 20.54 | 26.37 | 10.70 | 26.37 | 2045 | |
| 7 | 49.01 | 4.07 | 13.63 | 2.92 | 0.15 | 4.58 | 0.15 | 1748 | ✓ |
| 8 | 36.03 | 13.53 | 28.63 | 1.20 | 0.43 | 11.02 | 0.05 | 986 | (✓) |
| Avg | - | 58.30 | 31.83 | 12.28 | 11.75 | 17.87 | 11.60 | 1583 | |
| room | 1 | 0.16 | 0.09 | 0.08 | 0.04 | 0.01 | 0.01 | 0.01 | 146 | ✓ |
| 2 | 0.33 | 0.07 | 0.09 | 0.07 | 0.01 | 0.01 | 0.01 | 142 | ✓ |
| 3 | 0.15 | 0.13 | 0.07 | 0.05 | 0.01 | 0.01 | 0.01 | 135 | ✓ |
| 4 | 0.09 | 0.05 | 0.04 | 0.02 | 0.01 | 0.01 | 0.01 | 68 | ✓ |
| 5 | 0.12 | 0.13 | 0.08 | 0.02 | 0.01 | 0.01 | 0.01 | 131 | ✓ |
| 6 | 0.05 | 0.02 | 0.03 | 0.02 | 0.01 | 0.01 | 0.01 | 67 | ✓ |
| Avg | 0.15 | 0.08 | 0.07 | 0.04 | 0.01 | 0.01 | 0.01 | 115 | |
| slides | 1 | 13.73 | 0.32 | 2.55 | 1.16 | 1.47 | 0.41 | 0.10 | 289 | (✓) |
| 2 | 0.81 | 0.32 | 2.01 | 1.37 | 0.16 | 0.49 | 0.16 | 299 | (✓) |
| 3 | 4.68 | 0.89 | 2.44 | 1.62 | 1.35 | 0.47 | 1.35 | 383 | |
| Avg | 6.40 | 0.51 | 2.33 | 1.38 | 0.99 | 0.45 | 0.54 | 324 | |
$^{1}$ 结果取自 [^4]。
$^{2}$ 原始OKVIS VIO是在采用与OKVIS2相同的IMU和前端参数的情况下重新运行的。
$^{3}$ （✓）表示仅由OKVIS2识别出的回环闭合。
TABLE II: TUM-VI Absolute Trajectory Error (ATE) in [m].
尤其是在Magistrale和Slides序列中，以及最值得注意的长距离户外序列里，OKVIS2的表现尤为突出。需要注意的是，尤其在Magistrale和Slides序列中，OKVIS2实现的回环闭合数量比ORB-SLAM3所报告的更多，这些闭合用（✓）标记表示。
在户外场景中，ORB-SLAM3应对缓慢移动云层的方法是移除所有距离超过40米的地标观测数据，而OKVIS2则采用基于CNN的不匹配处理方法。值得注意的是，若针对该数据集严重畸变的鱼眼图像进行微调，并采用更深的网络模型，仍有改进空间。
### C. 耗时分析
在EuRoC MH_05_difficult数据集上测得的每帧耗时详见表III。测试在一台运行Ubuntu 20.04系统的PC上进行，该PC配备第11代英特尔$^{th}$ Gen Intel $^{\circledR}$ Core $^{TM}$ i7-11700K处理器，拥有8个核心，最高主频可达3.60GHz。
| Function | Timings (std. dev.) [ms] |
| Detect & Describe | 7.1 | ( $\pm 1.8$) |
| Match keypoints and triangulate | 26.6 | ( $\pm 12.4$) |
| Attempt loop closures | 17.7 | ( $\pm 24.0$) |
| Optimise realtime graph | 33.2 | ( $\pm 9.2$) |
| Pose graph edge creation & handling | 14.0 | ( $\pm 8.6$) |
TABLE III: Per-frame timings obtained on the EuRoC sequence MH_05_difficult. Note that the loop closure attempts are skipped in VIO mode. The synchronisation of full graph optimisations running in the background is only needed sporadically and amounts to 15.1( $\pm$7.4) ms, when occurring. Furthermore, the detection and description is run while the realtime optimisation is still ongoing. Therefore the numbers cannot simply be summed here to check for realtime capabilities.
需要注意的是，实时估计器在此处总会执行10次优化步骤。系统还实现了实时模式，可根据匹配和回环闭合尝试后剩余的时间预算来调整每帧的优化次数——这对精度的影響极小。
后台执行的回环闭合优化通常只需几十毫秒，而对于TUM-VI户外序列中极长的回环，优化时间才会达到约一秒。
### VIII. 结论
本文提出了一种多摄像头视觉惯性SLAM系统，该系统构建了包含视觉重投影误差、IMU预积分误差项以及从共同观测值边缘化得到的双位姿因子的因子图。实时估计器会在大小受限的最近关键帧和位姿图帧窗口内对这些因子进行最小化处理。一旦发生回环闭合，旧的位姿图边就能无缝地转换回重投影误差和地标。此外，较长的回环可以通过复用同一因子图进行异步优化，从而将回环周围的所有状态保留为优化变量的一部分。一系列实验表明，该方法能与最先进的视觉惯性SLAM系统相媲美。
在未来的工作中，我们可能会致力于解决单目视觉惯性SLAM的鲁棒处理问题，以及纳入绝对位置等额外传感器。此外，我们计划发布OKVIS2的开源实现。
更进一步来说，作为长期目标，我们将探索在此类多传感器融合场景下，密集和语义地图表示能在多大程度上以紧耦合的方式加以应用，从而进一步突破集成化、鲁棒且精准的空间人工智能解决方案的边界，为未来的移动机器人及AR/VR应用提供支持。
---
## 参考文献
[^1]: 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, pp. 422–429, 2019.
[^2]: A. Rosinol, M. Abate, Y. Chang, and L. Carlone, "Kimera: an open-source library for real-time metric-semantic localization and mapping," in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 1689–1696.
[^3]: T. Qin, S. Cao, J. Pan, and S. Shen, "A general optimization-based framework for global pose estimation with multiple sensors," 2019.
[^4]: 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, 2021.
[^5]: 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:
[^6]: D. Schubert, T. Goll, N. Demmel, V. Usenko, J. Stueckler, and D. Cremers, “The tum vi benchmark for evaluating visual-inertial odometry,” in International Conference on Intelligent Robots and Systems (IROS), October 2018. [Online]. Available:
[^7]: A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “Monoslam: Real-time single camera slam,” IEEE transactions on pattern analysis and machine intelligence, vol. 29, no. 6, pp. 1052–1067, 2007.
[^8]: G. Klein and D. Murray, “Parallel tracking and mapping for small ar workspaces,” in 2007 6th IEEE and ACM international symposium on mixed and augmented reality. IEEE, 2007, pp. 225–234.
[^9]: R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “Orb-slam: a versatile and accurate monocular slam system,” IEEE transactions on robotics, vol. 31, no. 5, pp. 1147–1163, 2015.
[^10]: 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, pp. 1255–1262, 2017.
[^11]: R. A. Newcombe, S. J. Lovegrove, and A. J. Davison, “Dtam: Dense tracking and mapping in real-time,” in 2011 international conference on computer vision. IEEE, 2011, pp. 2320–2327.
[^12]: J. Engel, J. Stückler, and D. Cremers, “Large-scale direct slam with stereo cameras,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2015, pp. 1935–1942.
[^13]: C. Forster, M. Pizzoli, and D. Scaramuzza, “Svo: Fast semi-direct monocular visual odometry,” in 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014, pp. 15–22.
[^14]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE transactions on pattern analysis and machine intelligence, vol. 40, no. 3, pp. 611–625, 2017.
[^15]: R. Wang, M. Schworer, and D. Cremers, “Stereo dso: Large-scale direct sparse visual odometry with stereo cameras,” in Proceedings of the IEEE International Conference on Computer Vision, 2017, pp. 3903–3911.
[^16]: X. Gao, R. Wang, N. Demmel, and D. Cremers, “Ldso: Direct sparse odometry with loop closure,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 2198–2204.
[^17]: 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, pp. 127–136.
[^18]: T. Whelan, M. Kaess, M. Fallon, H. Johannsson, J. Leonard, and J. McDonald, “Kintinuous: Spatially extended kinectfusion,” in RSS Workshop on RGB-D: Advanced Reasoning with Depth Cameras, 2012.
[^19]: E. Vespa, N. Funk, P. H. Kelly, and S. Leutenegger, “Adaptive-resolution octree-based volumetric slam,” in 2019 International Conference on 3D Vision (3DV). IEEE, 2019, pp. 654–662.
[^20]: 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, pp. 1697–1716, 2016.
[^21]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman filter for vision-aided inertial navigation,” in Proceedings 2007 IEEE
国际机器人学与自动化会议。IEEE，2007年，第3565–3572页。
[^22]: 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, pp. 4666–4672.
[^23]: M. Bloesch, M. Burri, S. Omari, M. Hutter, and R. Siegwart, “Iterated extended kalman filter based visual-inertial odometry using direct photometric feedback,” The International Journal of Robotics Research, vol. 36, no. 10, pp. 1053–1072, 2017.
[^24]: T. Schneider, M. Dymczyk, M. Fehr, K. Egger, S. Lynen, I. Gilitschenski, and R. Siegwart, “maplab: An open framework for research in visual-inertial mapping and localization,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 1418–1425, 2018.
[^25]: 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, pp. 314–334, 2015.
[^26]: N. Keivan, A. Patron-Perez, and G. Sibley, “Asynchronous adaptive conditioning for visual-inertial slam,” in Experimental Robotics. Springer, 2016, pp. 309–321.
[^27]: G. Grisetti, R. Kümmerle, H. Strasdat, and K. Konolige, “g2o: A general framework for (hyper) graph optimization,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 2011, pp. 9–13.
[^28]: S. Agarwal and K. Mierle, “Ceres solver: Tutorial & reference,” Google Inc, vol. 2, no. 72, p. 8, 2012.
[^29]: F. Dellaert, “Factor graphs and gtsam: A hands-on introduction,” Georgia Institute of Technology, Tech. Rep., 2012.
[^30]: M. Kaess, A. Ranganathan, and F. Dellaert, “isam: Incremental smoothing and mapping,” IEEE Transactions on Robotics, vol. 24, no. 6, pp. 1365–1378, 2008.
[^31]: M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping using the bayes tree,” The International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235, 2012.
[^32]: S. Agarwal, K. Mierle, and Others, "Ceres solver," .
[^33]: 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, pp. 1–21, 2016.
[^34]: D. Gálvez-López and J. D. Tardós, “Bags of binary words for fast place recognition in image sequences,” IEEE Transactions on Robotics, vol. 28, no. 5, pp. 1188–1197, October 2012.
[^35]: S. Leutenegger, M. Chli, and R. Y. Siegwart, “Brisk: Binary robust invariant scalable keypoints,” in 2011 International conference on computer vision. Ieee, 2011, pp. 2548–2555.
[^36]: S. Leutenegger, “Unmanned solar airplanes: Design and algorithms for efficient and robust autonomous operation,” Ph.D. dissertation, ETH Zurich, 2014.
[^37]: R. P. Poudel, S. Liwicki, and R. Cipolla, “Fast-scnn: Fast semantic segmentation network,” arXiv preprint arXiv:1902.04502, 2019.
[^38]: M. Cordts, M. Omran, S. Ramos, T. Rehfeld, M. Enzweiler, R. Benenson, U. Franke, S. Roth, and B. Schiele, “The cityscapes dataset for semantic urban scene understanding,” in Proc. of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2016.
[^39]: M. Burri, M. Bloesch, Z. Taylor, R. Siegwart, and J. Nieto, “A framework for maximum likelihood parameter identification applied on MAVs,” Journal of Field Robotics, vol. 35, no. 1, pp. 5–22, 2018.