# MAVIS：基于$SE_{2}(3)$的精确IMU预积分的多摄像头增强视觉-惯性SLAM系统

王一夫*，吴永翰*，萨英基，帕拉·阿尔瓦罗，罗德里格斯-奥帕索·克里斯蒂安，林涛军†，李宏东†

摘要—我们提出了一种专为多个部分重叠的摄像头系统设计的新型基于优化的视觉-惯性SLAM系统，命名为MAVIS。该框架充分利用了多摄像头系统提供的宽视场优势，以及惯性测量单元（IMU）提供的度量尺度信息。我们提出了一种基于$\mathrm{SE}_2(3)$自同构指数函数的改进版IMU预积分公式，该公式能够有效提升系统在快速旋转运动和长时间积分情况下的跟踪性能。此外，我们将原本为单目或双目配置设计的前端跟踪与后端优化模块扩展至多摄像头系统，并给出了在复杂场景下提升系统性能的具体实现细节。我们在公开数据集上的实验验证了该方法的有效性。我们的MAVIS系统在Hiti SLAM挑战赛2023的所有视觉-惯性赛道（单会话和多会话SLAM）中均斩获第一名，得分是第二名$^{1}$的1.7倍。

## 一、引言

鲁棒且实时的同步定位与地图构建（SLAM）一直是计算机视觉和机器人领域长期以来的难题。纯视觉解决方案往往缺乏激光雷达解决方案所具备的鲁棒性和精度。因此，这类方案通常会搭配额外的传感器来增强性能，比如能够测量角速度和加速度的低成本IMU，尤其是在XR（VR/AR）虚拟和增强现实设备中。虽然现有的单目或双目视觉-惯性解决方案[^1] [^2] [^3] [^4] [^5] [^6] [^7] [^8] [^9]已经证明了其在无纹理环境或快速运动等退化场景中通过融合IMU数据来提升鲁棒性的潜力，但仍然存在一些挑战：比如摄像头视场受限，以及长时间处理特征跟踪失败的能力不足。即便使用了IMU，这些挑战也可能导致系统迅速发散，从而造成定位精度的显著下降。

本文重点研究另一种类型的传感器系统——多摄像头系统。如图1所示，与前向双目摄像头相比，左侧、右侧及上方摄像头的共视区域更为有限，前向双目摄像头能提供更广阔的共视范围。这类系统的优势在于拥有更大的视场，能够对环境进行全方位观测，从而提升运动估计的精度和在纹理贫乏环境中的鲁棒性。

![Image](images/26f232196537e2e3b115119178d84232617429c1ed5285824e60be68999a3a16.jpg)

<div style="text-align: center;">Fig. 1: AlphaSense multi-camera module as an example of multi-camera system analyzed in this paper, with a forward-facing stereo cameras and multiple sideward monocular cameras.</div>

不过，这类系统存在固有的局限性：增加摄像头数量会直接带来计算成本的增加。如何妥善处理所有摄像头的测量数据，以平衡精度、鲁棒性和计算效率，是一项关键挑战。

此外，为了在不牺牲精度的前提下提升基于优化的视觉惯性导航方法的计算效率，[^10]提出了一种IMU预积分方法。该方法通过在选定的关键帧之间对测量值进行预积分，能够将数百个惯性测量值合并为单一的相对运动约束。这种表述方式在提升摄像头前端特征跟踪效果以及整体性能方面发挥着至关重要的作用。然而，现有方法[^10]、[^11]依赖于对位置和速度的粗略积分，且假设IMU在两次测量之间不发生旋转；这种近似处理会对预积分位姿的精度产生负面影响，尤其是在快速旋转运动和长时间积分的情况下。

我们的贡献如下：

- 我们提出了MAVIS，这是一种专为多部分重叠摄像头系统设计的、最先进的基于优化的视觉惯性SLAM框架。

• 我们基于$\mathrm{SE}_{2}(3)$的自同构指数函数，提出了一种新的IMU预积分公式。我们的方法能够实现对IMU数据的高精度积分，从而直接提升SLAM系统的跟踪性能。

- 通过广泛的实验评估，我们证明了MAVIS在鲁棒性和精度方面的显著优势。我们的方法在Hilti SLAM Challenge 2023的纯视觉单会话和多会话赛道中均荣获第一名。

### II. 相关工作

以往的研究框架中已对单目或双目视觉惯性SLAM的优势与挑战进行了深入探讨[^1] [^2] [^3] [^4] [^5] [^6] [^7] [^8] [^9]。如需全面了解相关内容，

---

请参阅[^12]以及最新的研究成果[^13]。本文主要关注多摄像头系统的基于视觉的解决方案。[^14]将ORB-SLAM2 [^15]扩展到了多摄像头场景，支持各种刚性耦合的多摄像头系统。[^16]针对任意多摄像头设置提出了一种自适应的SLAM系统设计，无需针对特定传感器进行调整。一些研究[^17] [^18] [^19] [^20] [^21]侧重于利用环视摄像头系统，这类系统通常由多个无重叠视场的单目摄像头组成，或者专门用于地面车辆的运动估计。虽然这些方法在复杂环境中表现出较强的鲁棒性，但在高度动态场景下性能有限，且在现实世界实验中的精度提升也不明显。

尽管已经提出了许多多摄像头视觉惯性解决方案，但尚无一种能在准确性、鲁棒性和计算效率之间实现完美平衡，尤其是在复杂场景中。VILENS-MC [^22] 是一种基于因子图优化的多摄像头视觉惯性里程计系统，它通过跨摄像头特征选择来提升跟踪效率。不过，该系统缺少局部地图跟踪模块和闭环优化功能，因此在重复访问的位置处，其性能不如ORB-SLAM3 [^9]。BAMF-SLAM [^23] 则提出了一种基于多鱼眼相机的VI-SLAM系统，该系统依靠紧密耦合的半位姿图束调整中的密集像素级对应关系来实现定位；这种方法虽然能提供极高的精度，但需要高端GPU才能接近实时性能。

### A. 惯性预积分

IMU预积分理论最初由 [^24]、[^25] 提出。该理论涉及在局部参考系中对惯性测量动力学进行离散积分，从而能够在每次优化步骤中高效修正状态动力学的偏差。[^26] 提出了一种在 $\mathrm{SO}(3)$ 流形上的无奇异性方向表示方法，并将IMU预积分融入基于优化的VINS中，显著提升了 [^25] 的稳定性。此外，[^1]、[^27] 引入了基于四元数的连续形式预积分方法，以克服离散化效应并提高精度。也有几种通过解析解 [^28]、[^29] 或切换线性系统 [^30]、[^31] 来解决此问题的方法。与我们研究密切相关的一项工作由 [^32] 提出，他们将 [^10] 的流形上预积分扩展到李群 $\mathrm{SE}_{2}(3)$。不过，他们的方法仍局限于对位置和速度采用欧拉积分，且假设IMU测量之间方向保持不变。

### III. 方法论

在这项工作中，我们提出了一种多摄像头VI-SLAM系统，它采用了一种基于 $\mathbf{SE}_2(3)$ 指数的新型精确IMU预积分公式，同时针对多摄像头场景扩展了前端跟踪和后端优化模块。图2展示了该系统的主要组成部分。

![Image](images/cf76a42f18084fc13f0192cb9afd6f5fc56045dfadafd664e116da3d038c6918.jpg)

<div style="text-align: center;">Fig. 2: Overview of our proposed visual-inertial localization and mapping pipeline for multi-camera systems.</div>

### A. IMU固有补偿

我们首先在IMU预积分阶段之前加入IMU固有补偿。在视觉惯性SLAM系统中使用IMU信息时，通常会将原始的加速度计和陀螺仪测量结果建模为

 $$ \breve{\mathbf{a}}_{t}=\mathbf{a}_{t}+\mathbf{b}_{a_{t}}+\mathbf{n}_{a_{t}} $$

 $$ \breve{\omega}_{t}=\omega_{t}+\mathbf{b}_{\omega_{t}}+\mathbf{n}_{\omega_{t}}, $$

该模型仅考虑了测量噪声密度（即n）和偏差b。受加速度偏差 $b_{a_t}$、陀螺仪偏差 $b_{\omega_t}$ 以及加性噪声 $n_a$ 和 $n_{\omega}$ 的影响，模型(1)虽简单实用，对于出厂时已校准固有参数的设备来说能给出较好的近似结果。但对于成本低廉的消费级惯性传感器，由于存在显著的轴对齐误差和比例因子误差，该模型可能会导致较差的校准结果。受 [^33] 的启发，我们通过引入IMU固有参数建模来扩展IMU模型(1)。

 $$ \mathbf{a}_{t}=\mathbf{S}_{\alpha}\mathbf{M}_{\alpha}\mathbf{a}_{t}+\mathbf{b}_{a_{t}}+\mathbf{n}_{a_{t}} $$

 $$ \breve{\omega}_{t}=\mathbf{S}_{\omega}\mathbf{M}_{\omega}\omega_{t}^{\prime}+\mathbf{A}_{\omega}\mathbf{a}_{t}+\mathbf{b}_{\omega_{t}}+\mathbf{n}_{\omega_{t}}, $$

其中 $\omega'_t = C_\omega \omega_t$ 是加速度计与陀螺仪之间的旋转矩阵。我们将 $S_\alpha$ 和 $S_\omega$ 定义为描述缩放效应的对角矩阵，$M_\alpha$ 和 $M_\omega$ 则是对应未对准小角度的下三角单位矩阵，而令 $A_\omega$ 为一个完全填充的矩阵。上述所有特性都可以通过采用广泛应用的Kalibr [^34] 来获取。

### B. IMU预积分

承接上一节的内容，我们对IMU轴未对准现象和比例因子进行补偿，并用符号 $\bar{\omega}_{t}$ 和 $\bar{a}_{t}$ 来表示存在偏差但无噪声/偏斜且经过比例校正后的IMU测量值。在后续章节中，我们会省略或保留下标t以简化表示方式，或是突出显示变量的时间依赖性。

现在我们开始介绍我们的核心贡献：一种基于矩阵李群 $\mathbf{SE}_2(\mathbf{3})$ 的自同构指数函数的新颖且精确的IMU预积分公式。我们首先将系统无噪声的运动学模型表述如下：

 $$ \left\{\begin{aligned}\dot{\mathbf{R}}&=\mathbf{R}(\bar{\omega}-\mathbf{b}_{\omega})^{\wedge}\\ \dot{\mathbf{p}}&=\mathbf{v}\\ \dot{\mathbf{v}}&=\mathbf{R}(\bar{\mathbf{a}}-\mathbf{b}_{a})+\mathbf{g}\end{aligned}\right. $$

---

其中 $(\mathbf{R},\dot{\mathbf{p}},\dot{\mathbf{v}})$ 分别表示IMU坐标系下旋转、位置和速度的一阶导数，$\mathbf{g}$ 表示重力矢量，两者均相对于世界固定坐标系来表达。我们还定义了偏差的随机游走过程 $\mathbf{b}_{\omega}=\tau_{\omega}$ 和 $\mathbf{b}_{a}=\tau_{a}$。我们可以用 $\mathbf{SE}_{2}(\mathbf{3})$ 李群中的扩展位姿来表示IMU的姿态，使得状态表示为 $\boldsymbol{\xi}=\begin{bmatrix} \mathbf{R}&\mathbf{p}&\mathbf{v} \\ 0&1&0 \\ 0&0&1 \end{bmatrix}$，而该状态的运动学关系 $\boldsymbol{\xi}$ 可以表示为

 $$ \mathrm{SE}_{2}(3)\mathrm{[}35\mathrm{]} $$

 $$ \dot{\boldsymbol{\xi}}=(\mathbf{G}-\mathbf{D})\boldsymbol{\xi}+\boldsymbol{\xi}(\mathbf{U}-\mathbf{B}+\mathbf{D}) $$

其中

 $$ \mathbf{U}=\left[\begin{matrix}{\omega^{\wedge}}&{0}&{\mathbf{a}}\\ {0}&{0}&{0}\\ {0}&{0}&{0}\\ \end{matrix}\right],\quad\mathbf{G}=\left[\begin{matrix}{0}&{0}&{\mathbf{g}}\\ {0}&{0}&{0}\\ {0}&{0}&{0}\\ \end{matrix}\right], $$

 $$ \mathbf{D}=\begin{bmatrix}0&0&0\\ 0&0&0\\ 0&1&0\end{bmatrix},\quad\mathbf{B}=\begin{bmatrix}(\mathbf{b}_{\omega})^{\wedge}&0&\mathbf{b}_{a}\\ 0&0&0\\ 0&0&0\end{bmatrix} $$

假设 $t_{i-1}$ 为前一图像帧 $\mathcal{F}_{i-1}$ 处的预积分起始时间。我们定义 $t_j$ 为帧 $\mathcal{F}_{i-1}$ 与 $\mathcal{F}_i$ 之间任意IMU测量的时间戳，并令 $t_{j'}$ 为其后续IMU测量的时间戳，这样小的积分时间即为 $\delta = t_{j'}$ - $t_j$。设 $\xi_{t_j}$ 为时刻 $t_j$ 的扩展位姿。根据式(4)进行精确积分，并假设U和B在积分时间 $\delta$ 内为常数，则有

 $$ \xi_{t_{j^{\prime}}}=\exp\left(\delta(\mathbf{G}-\mathbf{D})\right)\xi_{t_{j}}\exp(\delta(\mathbf{U}-\mathbf{B}+\mathbf{D})) $$

假设在时间 $t_{j}$ 到 $t_{i-1}$ 之间有N组IMU测量数据，我们有

 $$ \xi_{t_{j}}=\exp\left((\sum_{s=0}^{N}\delta_{s})(\mathbf{G}-\mathbf{D})\right)\xi_{t_{i-1}}\prod_{s=0}^{N}\exp(\delta_{s}(\mathbf{U}_{s}-\mathbf{B}_{s}+\mathbf{D})) $$

我们定义 $\mathcal{T} = \sum_{s=0}^{N} \delta_s$，然后对式(6)进行重新排列，可得

 $$ \xi_{t_{i-1}}^{-1}\exp\left(-\mathcal{T}(\mathbf{G}-\mathbf{D})\right)\xi_{t_{j}}=\prod_{s=0}^{N}\exp(\delta_{s}(\mathbf{U}_{s}-\mathbf{B}_{s}+\mathbf{D})). $$

左边的指数运算结果为

 $$ \exp\left(-\mathcal{T}(\mathbf{G}-\mathbf{D})\right)=\left[\begin{matrix}{\mathbf{I}}&{\frac{1}{2}\mathcal{T}^{2}\mathbf{g}}&{-\mathcal{T}\mathbf{g}}\\ {0}&{1}&{0}\\ {0}&{\mathcal{T}}&{1}\\ \end{matrix}\right]. $$

将式(8)代入式(7)，式(7)左侧分量与 [^10] 中式(33)的中间部分完全相同，右侧则是我们新推导出的预积分项，其中每个指数项均可展开为

 $$ \begin{array}{r l}&{\mathrm{p}(\delta_{s}(\mathbf{U}_{s}-\mathbf{B}_{s}+\mathbf{D}))=}\\ &{\quad\left[\begin{matrix}{\exp(\delta_{s}(\omega_{s}-\mathbf{b}_{\omega_{s}})^{\wedge})}&{\mathbf{J}_{2}(\mathbf{a}_{s}-\mathbf{b}_{a_{s}})}&{\mathbf{J}_{1}(\mathbf{a}_{s}-\mathbf{b}_{a_{s}})}\\ {0}&{1}&{0}\\ {0}&{\delta_{s}}&{1}\end{matrix}\right],}\end{array} $$

其中

 $$ \theta=\|\tilde{\omega}\| $$

 $$ \mathbf{J}_{1}=\delta_{s}\mathbf{I}+\frac{1}{\theta^{2}}(1-\cos(\delta_{s}\theta))\tilde{\omega}^{\wedge}+\frac{1}{\theta^{3}}(\delta_{s}\theta-\sin(\delta_{s}\theta))(\tilde{\omega}^{\wedge})^{2} $$

 $$ \mathbf{J}_{2}=\frac{1}{2}{\delta_{s}}^{2}\mathbf{I}+\frac{1}{\theta^{3}}(\delta_{s}\theta-\sin(\delta_{s}\theta))\tilde{\omega}^{\wedge}+\frac{1}{\theta^{4}}(\frac{1}{2}{\delta_{s}}^{2}\theta^{2}+\cos(\delta_{s}\theta)-1)(\tilde{\omega}^{\wedge})^{2} $$

此处我们使用 $\tilde{\omega} = \omega_s - b_{\omega_s}$。最终的迭代预积分表达式如下：

 $$ \left\{\begin{aligned}\Delta\mathbf{R}_{t_{j^{\prime}}}^{t_{i}}&=\Delta\mathbf{R}_{t_{j}}^{t_{i}}\exp(\delta_{j}(\omega_{j}-\mathbf{b}_{\omega_{j}})^{\wedge})\\ \Delta\mathbf{p}_{t_{j^{\prime}}}^{t_{i}}&=\Delta\mathbf{p}_{t_{j}}^{t_{i}}+\delta_{j}\Delta\mathbf{v}_{t_{j}}^{t_{i}}+\Delta\mathbf{R}_{t_{j}}^{t_{i}}\mathbf{J}_{2}(\mathbf{a}_{j}-\mathbf{b}_{a_{j}})\\ \Delta\mathbf{v}_{t_{j^{\prime}}}^{t_{i}}&=\Delta\mathbf{v}_{t_{j}}^{t_{i}}+\Delta\mathbf{R}_{t_{j}}^{t_{i}}\mathbf{J}_{1}(\mathbf{a}_{j}-\mathbf{b}_{a_{j}})\end{aligned}\right. $$

需要注意的是，在欧拉积分方案下，雅可比项 $\mathbf{J}_1$ 和 $\mathbf{J}_2$ 可简化为 $\mathbf{J}_1 = \delta_s\mathbf{I}$ 和 $\mathbf{J}_2 = \frac{1}{2}\delta_s^2\mathbf{I}$。

在实际实现中，无噪声项并不可得，因此用带有 $\hat{(\cdot)}$ 标记的估计值来替代它们。随后我们需考虑基于先前估计值的不确定性以及测量噪声的协方差传播问题。我们定义误差项如下：

 $$ e_{i j^{\prime}}=[(e_{i j^{\prime}}^{\Delta\mathbf{R}})^{\top},(e_{i j^{\prime}}^{\Delta\mathbf{p}})^{\top},(e_{i j^{\prime}}^{\Delta\mathbf{v}})^{\top},(e_{j^{\prime}}^{\mathbf{b}_{\omega}})^{\top},(e_{j^{\prime}}^{\mathbf{b}_{a}})^{\top}]^{\top} $$

 $$ \begin{array}{r}{e_{i j^{\prime}}^{\Delta\mathbf{R}}=\log(\Delta\mathbf{R}_{t_{j^{\prime}}}^{t_{i}}{}^{\top}\Delta\hat{\mathbf{R}}_{t_{j^{\prime}}}^{t_{i}})^{{\vee}}\sim\mathcal{N}(0,\Sigma_{i j^{\prime}}^{\mathbf{R}})\in\mathbb{R}^{3}}\end{array} $$

 $$ e_{i j^{\prime}}^{\Delta\mathbf{p}}=\Delta\hat{\mathbf{p}}_{t_{j^{\prime}}}^{t_{i}}-\Delta\mathbf{p}_{t_{j^{\prime}}}^{t_{i}}\sim\mathcal{N}(0,\Sigma_{i j^{\prime}}^{\mathbf{p}})\in\mathbb{R}^{3} $$

 $$ e_{i j^{\prime}}^{\Delta\mathbf{v}}=\Delta\hat{\mathbf{v}}_{t_{j^{\prime}}}^{t_{i}}-\Delta\mathbf{v}_{t_{j^{\prime}}}^{t_{i}}\sim\mathcal{N}(0,\Sigma_{i j^{\prime}}^{\mathbf{v}})\in\mathbb{R}^{3} $$

 $$ e_{j^{\prime}}^{\mathbf{b}_{\omega}}=\hat{\mathbf{b}}_{\omega_{j}}-\mathbf{b}_{\omega_{j}}\sim\mathcal{N}(0,\Sigma_{i j^{\prime}}^{\mathbf{b}_{\omega}})\in\mathbb{R}^{3} $$

 $$ e_{j^{\prime}}^{\mathbf{b}_{a}}=\hat{\mathbf{b}}_{a_{j}}-\mathbf{b}_{a_{j}}\sim\mathcal{N}(0,\Sigma_{i j^{\prime}}^{\mathbf{b}_{a}})\in\mathbb{R}^{3} $$

我们推导出预积分项演化的矩阵表示形式

 $$ \boldsymbol{e}_{i j^{\prime}}=\mathbf{A}_{j^{\prime}}\boldsymbol{e}_{i j}+\mathbf{B}_{j^{\prime}}\mathbf{n}_{j^{\prime}} $$

 $$ \mathbf{A}=\left[\begin{matrix}{\operatorname{e x p}(-\delta_{j^{\prime}}(\hat{\omega}_{j^{\prime}}-\hat{\mathbf{b}}_{\omega_{j^{\prime}}})^{\wedge})}&{0}&{0}&{-\delta_{j^{\prime}}\mathbf{I}}&{0}\\ {-\Delta\hat{\mathbf{R}}_{t_{j}}^{t_{i}}(\hat{\mathbf{J}}_{2}(\hat{\mathbf{a}}_{j^{\prime}}-\hat{\mathbf{b}}_{a_{j^{\prime}}}))^{\wedge}}&{\mathbf{I}}&{\delta_{j^{\prime}}\mathbf{I}}&{\mathbf{D}_{1}}&{-\Delta\hat{\mathbf{R}}_{t_{j}}^{t_{i}}\hat{\mathbf{J}}_{2}}\\ {-\Delta\hat{\mathbf{R}}_{t_{j}}^{t_{i}}(\hat{\mathbf{J}}_{1}(\hat{\mathbf{a}}_{j^{\prime}}-\hat{\mathbf{b}}_{a_{j^{\prime}}}))^{\wedge}}&{0}&{\mathbf{I}}&{-\mathbf{D}_{2}}&{-\Delta\hat{\mathbf{R}}_{t_{j}}^{t_{i}}\hat{\mathbf{J}}_{1}}\\ {0}&{0}&{0}&{\mathbf{I}}&{0}\\ {0}&{0}&{0}&{0}&{\mathbf{I}}\\ \end{matrix}\right] $$

 $$ \mathbf{B}=\left[\begin{matrix}{\delta_{j^{\prime}}\mathbf{I}}&{0}&{0}&{0}\\ {-\mathbf{D}_{1}}&{\Delta\hat{\mathbf{R}}_{t_{j}}^{i_{i}}\hat{\mathbf{J}}_{2}}&{0}&{0}\\ {\mathbf{D}_{2}}&{\Delta\hat{\mathbf{R}}_{t_{j}}^{i_{i}}\hat{\mathbf{J}}_{1}}&{0}&{0}\\ {0}&{0}&{-\delta_{j^{\prime}}\mathbf{I}}&{0}\\ {0}&{0}&{0}&{-\delta_{j^{\prime}}\mathbf{I}}\\ \end{matrix}\right],\mathbf{n}_{j^{\prime}}=\left[\begin{matrix}{\mathbf{n}_{\omega_{j^{\prime}}}}\\ {\mathbf{n}_{\mathbf{a}_{j^{\prime}}}}\\ {\tau_{\omega_{j^{\prime}}}}\\ {\tau_{a_{j^{\prime}}}}\\ \end{matrix}\right] $$

以及

 $$ \begin{aligned}\mathbf{D}_{1}&=\Delta\hat{\mathbf{R}}_{t_{j}}^{t_{i}}\left(\frac{\delta_{j^{\prime}}{}^{2}}{\|\hat{\boldsymbol{\omega}}_{j^{\prime}}-\hat{\mathbf{B}}_{\omega_{j^{\prime}}}\|^{2}}\Big(\left((\hat{\boldsymbol{\omega}}_{j^{\prime}}-\hat{\mathbf{b}}_{\omega_{j^{\prime}}})\wedge(\hat{\mathbf{a}}_{j^{\prime}}-\hat{\mathbf{b}}_{a_{j^{\prime}}})\right)^\wedge\right.\\&\left.+(\hat{\boldsymbol{\omega}}_{j^{\prime}}-\hat{\mathbf{b}}_{\omega_{j^{\prime}}})\wedge(\hat{\mathbf{a}}_{j^{\prime}}-\hat{\mathbf{b}}_{a_{j^{\prime}}})\wedge\right)\\&\left.\quad+\frac{2\delta_{j^{\prime}}{}^{2}}{\|\hat{\boldsymbol{\omega}}_{j^{\prime}}-\hat{\mathbf{b}}_{\omega_{j^{\prime}}}\|^{4}}\Big(\left((\hat{\boldsymbol{\omega}}_{j^{\prime}}-\hat{\mathbf{b}}_{\omega_{j^{\prime}}})\wedge\right){}^{2}(\hat{\mathbf{a}}-\hat{\mathbf{b}}_{a_{j^{\prime}}})(\hat{\boldsymbol{\omega}}_{j^{\prime}}-\hat{\mathbf{b}}_{\omega_{j^{\prime}}})^\top\Big)\right)\ $ \mathbf{D}_{2}&=\Delta\hat{\mathbf{R}}_{t_{j^{\prime}}}^{t_{i}}\|\hat{\boldsymbol{\omega}}_{j^{\prime}}-\hat{\mathbf{b}}_{\omega_{j^{\prime}}}\|^{4}\delta_{j^{\prime}}{}^{2}(\hat{\boldsymbol{\omega}}_{j^{\prime}}-\hat{\mathbf{b}}_{\omega_{j^{\prime}}})\wedge(\hat{\mathbf{a}}_{j^{\prime}}-\hat{\mathbf{b}}_{a_{j^{\prime}}})(\hat{\boldsymbol{\omega}}_{j^{\prime}}-\hat{\mathbf{b}}_{\omega_{j^{\prime}}})\top\\&\quad(19)\end{aligned} $$

其中，$\hat{\omega}$ 和 $\hat{a}$ 分别是经过偏差校正、含噪声且经过内在补偿后的IMU测量结果。

### C. 前端跟踪

我们的目标是为配备多台部分重叠摄像头的传感器套件设计SLAM解决方案（参见图1所示）。受 [^22] 和 [^9] 的启发，我们采用了基于局部地图的定位策略，利用提取的二维特征点和局部三维地图点来实现精确的位姿估计。这种局部地图匹配机制能够

---

![Image](images/2235046f06c3c468945c55994349a2a62ea6941c57e4b63d2ff7695a99fa106a.jpg)

<div style="text-align: center;">Fig. 3: Geometric relationships of local feature matching across multiple cameras.</div>

 $$ (u_{c_{k}}^{n},v_{c_{k}}^{n})=\pi_{c_{k}}(\mathbf{T}_{b c_{k}}^{-1}\mathbf{T}_{\mathcal{I}_{i}^{i-1}}^{-1}\mathbf{T}_{i-1}^{-1}\mathbf{p}_{n}), $$

在摄像头重新访问先前位置时显著提升跟踪性能。我们将机体坐标系定义为与IMU坐标系一致。首先，利用从IMU预积分得到的预测相对位姿，将所有局部地图点投影到当前时刻的多摄像头图像上。如图3所示，为了增强共视关系，我们会在摄像头内部以及摄像头之间进行特征匹配。由于多摄像头系统已经过精确标定，任意摄像头上的投影地标可表示为：

其中，$\mathbf{p}_n$ 表示世界坐标系中地标n的位置，$\pi_{c_k}$ 是投影函数，它利用摄像头 $\mathbf{p}_n$ 的内参将 $(u_{c_k}^n, v_{c_k}^n)$ 转换为像素位置 $c_k$。我们将 $\mathbf{T}_{bc_k}$ 定义为摄像头 $c_k$ 相对于IMU坐标的外参。设 $\mathbf{T}_{i-1}$ 为参考坐标系 $\mathcal{F}_{i-1}$ 在世界坐标系中的绝对位姿，$\mathbf{T}_{\mathcal{T}_{i-1}^1}$ 为利用IMU预积分得到的当前帧 $\mathcal{F}_i$ 与参考坐标系之间的估计相对位姿。对于那些没有与地标关联的二维特征点，我们会在当前帧和关键帧之间进行特征匹配，并通过三角测量创建新的局部地图点。随后，MAVIS后端优化会利用所有这些关系来增强共视边并提升定位精度。此外，我们利用描述符之间的距离进行进一步验证，并在后端优化中采用鲁棒的成本函数来剔除所有错误匹配的特征点。

### D. 后端优化

与许多其他基于优化方案的视觉-惯性SLAM/VIO系统 [^2]、[^9]、[^22] 类似，我们的MAVIS通过滑动窗口束调整方案，最小化基于所有摄像头观测的视觉重投影误差以及预积分IMU测量的误差，从而更新所有状态向量。我们将状态向量 $\mathcal{X}$ 定义为运动状态 $\{\mathbf{x}\} = [\mathbf{x}_1 \cdots \mathbf{x}_v]$ 和地标状态 $\{\mathbf{p}\} = [\mathbf{p}_0 \cdots \mathbf{p}_l]$ 的组合，而 $\mathbf{x}_i$ 包含6自由度机体位姿 $\mathbf{R}_i$ 和 $\mathbf{t}_i$、线速度 $\mathbf{v}_i$，以及IMU偏差 $\mathbf{b}_{\omega_i}$ 和 $\mathbf{b}_{a_i}$。$\mathbf{p}_n$ 表示地标n的位置。视觉-惯性束调整可表示为：

 $$ \min_{\mathcal{X}}\sum_{i=1}^{v}\left(\|\mathbf{r}_{\mathbf{x}_{i},\mathcal{I}_{i}^{i-1}}\|_{\sum_{\mathbf{x}_{i},\mathcal{I}_{i}^{i-1}}}^{2}+\sum_{k=0}^{m}\sum_{n\in\mathcal{L}_{i}}\rho\|\mathbf{r}_{\mathbf{c}_{k},\mathbf{x}_{i},\mathbf{p}_{n}}\|_{\sum_{\mathbf{c}_{k},\mathbf{x}_{i},\mathbf{p}_{n}}}\right) $$

其中

$\mathcal{L}_{i}$ 是在关键帧 $\mathcal{F}_{i}$ 中观测到的地标集合

- $\mathbf{r}_{x_i, \mathcal{I}_i^{i-1}}$ 是IMU的残差，$\mathbf{r}_{c_k, x_i, p_n}$ 是摄像头 $c_k$ 的视觉测量残差。

$\rho(\cdot)$ 是用于剔除异常值的鲁棒核函数。

### IV. 多摄像头视觉SLAM应用

图2展示了我们系统架构的概览。在第三节介绍了IMU预积分、前端跟踪以及后端优化模块之后，本节将详细介绍具体的实现细节，这些细节直接决定了我们SLAM系统的整体精度和鲁棒性。

### A. 视觉测量预处理

为了应对现实应用场景以及来自[^36]的数据集中存在的挑战，比如黑暗场景、帧丢失和数据不连续等问题，我们在特征提取步骤之前先进行数据预处理。具体来说，我们采用直方图均衡化技术来补偿暗帧，该技术能显著提升提取出的特征点的数量和分布。此外，由于多摄像头设备传输图像数据时需要更大的带宽，因此更容易出现帧丢失和同步问题。为此，我们利用特征跟踪中剩余的图像，避免长时间整合IMU数据。在相机-IMU外参标定过程中，我们获取了各相机与IMU之间的时间延迟，并加入了中曝光时间补偿参数，以进一步降低同步误差的影响。通过采用这些方法，我们的SLAM解决方案性能得到了明显提升。

### B. 相机-IMU初始化

受[^9]的启发，我们在MAVIS中采用了一种稳健且精准的多摄像头IMU初始化方法。首先，我们利用第一帧的立体信息生成初始地图；一旦在第一关键帧中确定了特征的深度，便利用传播的运动模型来投影相邻关键帧中的所有路标。值得注意的是，后续阶段的投影操作同时涵盖了同摄像头内和不同摄像头间的场景。有关前端跟踪的详细信息，请参阅第三节C部分。随后，我们在2秒内执行纯视觉的MAP估计束调整，同时计算相邻关键帧间的IMU预积分和协方差。在纯IMU优化方法之后，我们通过多视觉-惯性束调整来联合优化地图点位置、IMU参数以及相机位姿。为了避免实时输出中出现轨迹跳变，我们在优化过程中固定最后一帧的位姿。虽然也可以采用更先进的算法，如[^39]，但这些算法并未集成到MAVIS中。

---

![Image](images/cbb40d0a7f42cb73616f940ab68b76e1eb39ce97f22cee5ed170b3101bbb855a.jpg)

<div style="text-align: center;">Fig. 4: Multiple detected closed loops from site1\_handheld\_4 sequence in Hilti SLAM Challenge 2023. Accumulated drifts are significantly reduced, ensuring enhanced accuracy and robustness.</div>

### C. 回环检测

回环检测模块利用DBoW2库[^40]来识别候选帧。为了充分利用多摄像头系统宽广的视场角，我们从同摄像头内和不同摄像头间检测潜在的回环，这使得我们的MAVIS系统能够准确检测出普通单目或立体VIO系统无法识别的回环——比如掉头运动（参见图4）。与[^1]、[^7]、[^9]类似，我们会进行几何验证以剔除异常的回环检测结果。一旦检测到正确的回环，我们便执行全局束调整，利用所有可用信息优化整个轨迹，从而大幅减少大多数序列中的漂移现象。

### D. 多智能体SLAM

为了参与 Hiltí SLAM Challenges 2023 [^41] 的多会话（multi-session）挑战，我们采用类似的方法，通过回环闭合修正模块（参见图 5）来合并地图。我们使用词袋（Bag-of-Words, BoW）模型 [^40] 来识别潜在的重叠关键帧，并利用局部地图辅助几何对齐。通过融合多个子地图并进行成功的验证检查，我们最终生成全局一致的地图。在多会话挑战的背景下，我们指定一个单会话序列作为全局地图的基础。当一个序列的 SLAM 处理完成后，地图会在本地保存，并在处理后续数据序列之前预加载。通过系统处理所有序列和地图融合，我们可以将所有子地图集成到一张全局地图中。

### 五、实验

我们在多种数据集上评估了该方法的表现。首先，我们使用Eu-RoC数据集，将其与立体惯性配置下的多数最先进视觉-惯性算法进行对比[^38]。此外，我们还在Hiti SLAM挑战赛2023数据集上针对多摄像头配置对系统进行了评估[^41]，该数据集包含了来自手持设备和地面机器人的具有挑战性的序列。定性和定量结果均凸显了我们系统的有效性。我们的框架采用C++实现，并在搭载AMD Ryzen 9 5950X 16核处理器的Ubuntu 20.04台式机上进行了评估。

### A. 在EuRoC数据集上的性能表现

我们最初的实验采用了广泛使用的EuRoC数据集[^38]，这些数据集包含由无人机在室内飞行时拍摄的序列，该无人机配备了同步工作的双目相机和惯性测量单元（IMU）。我们将MAVIS与多种最先进的方法进行对比测试，包括VINS-MONO [^1]、OKVIS [^2]、SVOGTSAM [^3]、EqVIO [^4]、OpenVINS [^6]、VINS-FUSION [^5]、Kimera [^7]、BASALT [^8]以及ORB-SLAM3 [^9]。为了公平比较，我们采用了EuRoC数据集提供的校准结果，并且没有对IMU进行内部补偿处理。我们使用EVO [^42]工具来定量评估绝对轨迹误差（以米为单位的均方根误差），并将结果汇总在表I中。其中最优结果以粗体显示，而“-”则表示该方法未能完成对应序列的计算。

如上表所示，我们的方法在大多数序列中均表现出更优的性能。在ATE评估涉及的11个序列中，有6个序列我们取得了最佳结果，仅有ORB-SLAM3 [^9]和BASALT [^8]能接近我们的方法。我们还给出了所有序列的均方根误差标准差（std），用以衡量不同算法在同一数据集上的鲁棒性。我们的方法再次以0.017的标准差误差优于其他所有算法。总而言之，我们的立体视觉方案在精度和鲁棒性方面均达到了最先进的水平。这得益于我们改进的IMU预积分公式，它能够提供更精确的运动建模，并在处理快速旋转和长时间积分时展现出更强的鲁棒性。

### B. 在Hilti SLAM挑战赛2023中的性能表现

为了全面评估我们的多摄像头视觉惯性SLAM系统在复杂条件下的鲁棒性和精度，我们在Hilti SLAM挑战赛上开展了相关实验。

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td colspan="2"></td><td style='text-align: center; word-wrap: break-word;'>MH-01</td><td style='text-align: center; word-wrap: break-word;'>MH-02</td><td style='text-align: center; word-wrap: break-word;'>MH-03</td><td style='text-align: center; word-wrap: break-word;'>MH-04</td><td style='text-align: center; word-wrap: break-word;'>MH-05</td><td style='text-align: center; word-wrap: break-word;'>V-101</td><td style='text-align: center; word-wrap: break-word;'>V-102</td><td style='text-align: center; word-wrap: break-word;'>V-103</td><td style='text-align: center; word-wrap: break-word;'>V-201</td><td style='text-align: center; word-wrap: break-word;'>V-202</td><td style='text-align: center; word-wrap: break-word;'>V-203</td><td style='text-align: center; word-wrap: break-word;'>Std.</td></tr><tr><td rowspan="2">Monocular</td><td style='text-align: center; word-wrap: break-word;'>VINS-MONO [^1]</td><td style='text-align: center; word-wrap: break-word;'>0.070</td><td style='text-align: center; word-wrap: break-word;'>0.050</td><td style='text-align: center; word-wrap: break-word;'>0.080</td><td style='text-align: center; word-wrap: break-word;'>0.120</td><td style='text-align: center; word-wrap: break-word;'>0.090</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.060</td><td style='text-align: center; word-wrap: break-word;'>0.110</td><td style='text-align: center; word-wrap: break-word;'>0.060</td><td style='text-align: center; word-wrap: break-word;'>0.060</td><td style='text-align: center; word-wrap: break-word;'>0.090</td><td style='text-align: center; word-wrap: break-word;'>0.025</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>OKVIS [^2]</td><td style='text-align: center; word-wrap: break-word;'>0.160</td><td style='text-align: center; word-wrap: break-word;'>0.220</td><td style='text-align: center; word-wrap: break-word;'>0.240</td><td style='text-align: center; word-wrap: break-word;'>0.340</td><td style='text-align: center; word-wrap: break-word;'>0.470</td><td style='text-align: center; word-wrap: break-word;'>0.090</td><td style='text-align: center; word-wrap: break-word;'>0.200</td><td style='text-align: center; word-wrap: break-word;'>0.240</td><td style='text-align: center; word-wrap: break-word;'>0.130</td><td style='text-align: center; word-wrap: break-word;'>0.160</td><td style='text-align: center; word-wrap: break-word;'>0.290</td><td style='text-align: center; word-wrap: break-word;'>0.106</td></tr><tr><td rowspan="2">Inertial</td><td style='text-align: center; word-wrap: break-word;'>SVOGTSAM [^3]</td><td style='text-align: center; word-wrap: break-word;'>0.050</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>0.120</td><td style='text-align: center; word-wrap: break-word;'>0.130</td><td style='text-align: center; word-wrap: break-word;'>0.160</td><td style='text-align: center; word-wrap: break-word;'>0.070</td><td style='text-align: center; word-wrap: break-word;'>0.110</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>0.070</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>0.044</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>EqVIO [^4]</td><td style='text-align: center; word-wrap: break-word;'>0.176</td><td style='text-align: center; word-wrap: break-word;'>0.236</td><td style='text-align: center; word-wrap: break-word;'>0.112</td><td style='text-align: center; word-wrap: break-word;'>0.165</td><td style='text-align: center; word-wrap: break-word;'>0.238</td><td style='text-align: center; word-wrap: break-word;'>0.063</td><td style='text-align: center; word-wrap: break-word;'>0.128</td><td style='text-align: center; word-wrap: break-word;'>0.216</td><td style='text-align: center; word-wrap: break-word;'>0.058</td><td style='text-align: center; word-wrap: break-word;'>0.158</td><td style='text-align: center; word-wrap: break-word;'>0.176</td><td style='text-align: center; word-wrap: break-word;'>0.062</td></tr><tr><td rowspan="3">Stereo</td><td style='text-align: center; word-wrap: break-word;'>OpenVINS [^6]</td><td style='text-align: center; word-wrap: break-word;'>0.183</td><td style='text-align: center; word-wrap: break-word;'>0.129</td><td style='text-align: center; word-wrap: break-word;'>0.170</td><td style='text-align: center; word-wrap: break-word;'>0.172</td><td style='text-align: center; word-wrap: break-word;'>0.212</td><td style='text-align: center; word-wrap: break-word;'>0.055</td><td style='text-align: center; word-wrap: break-word;'>0.044</td><td style='text-align: center; word-wrap: break-word;'>0.069</td><td style='text-align: center; word-wrap: break-word;'>0.058</td><td style='text-align: center; word-wrap: break-word;'>0.045</td><td style='text-align: center; word-wrap: break-word;'>0.147</td><td style='text-align: center; word-wrap: break-word;'>0.063</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>VINS-FUSION [^5]</td><td style='text-align: center; word-wrap: break-word;'>0.181</td><td style='text-align: center; word-wrap: break-word;'>0.092</td><td style='text-align: center; word-wrap: break-word;'>0.167</td><td style='text-align: center; word-wrap: break-word;'>0.203</td><td style='text-align: center; word-wrap: break-word;'>0.416</td><td style='text-align: center; word-wrap: break-word;'>0.064</td><td style='text-align: center; word-wrap: break-word;'>0.270</td><td style='text-align: center; word-wrap: break-word;'>0.157</td><td style='text-align: center; word-wrap: break-word;'>0.065</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>0.160</td><td style='text-align: center; word-wrap: break-word;'>0.105</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Kimera [^7]</td><td style='text-align: center; word-wrap: break-word;'>0.080</td><td style='text-align: center; word-wrap: break-word;'>0.090</td><td style='text-align: center; word-wrap: break-word;'>0.110</td><td style='text-align: center; word-wrap: break-word;'>0.150</td><td style='text-align: center; word-wrap: break-word;'>0.240</td><td style='text-align: center; word-wrap: break-word;'>0.050</td><td style='text-align: center; word-wrap: break-word;'>0.110</td><td style='text-align: center; word-wrap: break-word;'>0.120</td><td style='text-align: center; word-wrap: break-word;'>0.070</td><td style='text-align: center; word-wrap: break-word;'>0.100</td><td style='text-align: center; word-wrap: break-word;'>0.190</td><td style='text-align: center; word-wrap: break-word;'>0.055</td></tr><tr><td rowspan="3">Inertial</td><td style='text-align: center; word-wrap: break-word;'>BASALT [^8]</td><td style='text-align: center; word-wrap: break-word;'>0.080</td><td style='text-align: center; word-wrap: break-word;'>0.060</td><td style='text-align: center; word-wrap: break-word;'>0.050</td><td style='text-align: center; word-wrap: break-word;'>0.100</td><td style='text-align: center; word-wrap: break-word;'>0.080</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>0.028</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>ORB-SLAM3 [^37]</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.033</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.051</td><td style='text-align: center; word-wrap: break-word;'>0.082</td><td style='text-align: center; word-wrap: break-word;'>0.038</td><td style='text-align: center; word-wrap: break-word;'>0.014</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.014</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.019</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>MAVIS(Ours)</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.025</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.053</td><td style='text-align: center; word-wrap: break-word;'>0.075</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.016</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>0.017</td></tr></table>

<div style="text-align: center;">TABLE I: Performance comparison on the EuRoC [^38] datasets, RMSE of Absolute trajectory Error (ATE) in meters.</div>

---

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'>Sequence name</td><td style='text-align: center; word-wrap: break-word;'>Difficulties</td><td style='text-align: center; word-wrap: break-word;'>Sequence length</td><td style='text-align: center; word-wrap: break-word;'>Processing time</td><td style='text-align: center; word-wrap: break-word;'>MAVIS(Ours)</td><td style='text-align: center; word-wrap: break-word;'>BAMF-SLAM</td><td style='text-align: center; word-wrap: break-word;'>Maplab2.0</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site1_handheld_1</td><td style='text-align: center; word-wrap: break-word;'>dark around stairs going to Floor 1</td><td style='text-align: center; word-wrap: break-word;'>204.71s</td><td style='text-align: center; word-wrap: break-word;'>224.59s</td><td style='text-align: center; word-wrap: break-word;'>32.5</td><td style='text-align: center; word-wrap: break-word;'>10.0</td><td style='text-align: center; word-wrap: break-word;'>15.0</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site1_handheld_2</td><td style='text-align: center; word-wrap: break-word;'>dark around stairs going to Floor 2</td><td style='text-align: center; word-wrap: break-word;'>167.11s</td><td style='text-align: center; word-wrap: break-word;'>211.98s</td><td style='text-align: center; word-wrap: break-word;'>23.75</td><td style='text-align: center; word-wrap: break-word;'>5.0</td><td style='text-align: center; word-wrap: break-word;'>12.5</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site1_handheld_3</td><td style='text-align: center; word-wrap: break-word;'>insufficient overlap for multi-session</td><td style='text-align: center; word-wrap: break-word;'>170.63s</td><td style='text-align: center; word-wrap: break-word;'>204.09s</td><td style='text-align: center; word-wrap: break-word;'>22.5</td><td style='text-align: center; word-wrap: break-word;'>17.5</td><td style='text-align: center; word-wrap: break-word;'>5.0</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site1_handheld_4</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>295.42s</td><td style='text-align: center; word-wrap: break-word;'>364.41s</td><td style='text-align: center; word-wrap: break-word;'>30.0</td><td style='text-align: center; word-wrap: break-word;'>5.0</td><td style='text-align: center; word-wrap: break-word;'>8.33</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site1_handheld_5</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>159.29s</td><td style='text-align: center; word-wrap: break-word;'>196.86s</td><td style='text-align: center; word-wrap: break-word;'>26.67</td><td style='text-align: center; word-wrap: break-word;'>11.67</td><td style='text-align: center; word-wrap: break-word;'>13.33</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site1_multi_session</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>4.17</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>5.28</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site2_robot_1</td><td style='text-align: center; word-wrap: break-word;'>unsynchronised cameras, long, no loop closure</td><td style='text-align: center; word-wrap: break-word;'>699.31s</td><td style='text-align: center; word-wrap: break-word;'>531.89s</td><td style='text-align: center; word-wrap: break-word;'>15.71</td><td style='text-align: center; word-wrap: break-word;'>6.43</td><td style='text-align: center; word-wrap: break-word;'>7.86</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site2_robot_2</td><td style='text-align: center; word-wrap: break-word;'>unsynchronised cameras</td><td style='text-align: center; word-wrap: break-word;'>305.79s</td><td style='text-align: center; word-wrap: break-word;'>194.40s</td><td style='text-align: center; word-wrap: break-word;'>53.33</td><td style='text-align: center; word-wrap: break-word;'>28.33</td><td style='text-align: center; word-wrap: break-word;'>15.0</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site2_robot_3</td><td style='text-align: center; word-wrap: break-word;'>dark, insufficient overlap for multi-session</td><td style='text-align: center; word-wrap: break-word;'>359.00s</td><td style='text-align: center; word-wrap: break-word;'>187.30s</td><td style='text-align: center; word-wrap: break-word;'>19.0</td><td style='text-align: center; word-wrap: break-word;'>13.0</td><td style='text-align: center; word-wrap: break-word;'>5.0</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site2_multi_session</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>3.33</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>2.33</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site3_handheld_1</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>97.18s</td><td style='text-align: center; word-wrap: break-word;'>124.07s</td><td style='text-align: center; word-wrap: break-word;'>105.0</td><td style='text-align: center; word-wrap: break-word;'>45.0</td><td style='text-align: center; word-wrap: break-word;'>10.0</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site3_handheld_2</td><td style='text-align: center; word-wrap: break-word;'>dropped data</td><td style='text-align: center; word-wrap: break-word;'>148.13s</td><td style='text-align: center; word-wrap: break-word;'>182.31s</td><td style='text-align: center; word-wrap: break-word;'>35.0</td><td style='text-align: center; word-wrap: break-word;'>56.67</td><td style='text-align: center; word-wrap: break-word;'>11.67</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site3_handheld_3</td><td style='text-align: center; word-wrap: break-word;'>dropped data</td><td style='text-align: center; word-wrap: break-word;'>189.60s</td><td style='text-align: center; word-wrap: break-word;'>243.46s</td><td style='text-align: center; word-wrap: break-word;'>23.75</td><td style='text-align: center; word-wrap: break-word;'>31.25</td><td style='text-align: center; word-wrap: break-word;'>7.5</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site3_handheld_4</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>106.88s</td><td style='text-align: center; word-wrap: break-word;'>130.34s</td><td style='text-align: center; word-wrap: break-word;'>65.0</td><td style='text-align: center; word-wrap: break-word;'>37.5</td><td style='text-align: center; word-wrap: break-word;'>10.0</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>site3_multi_session</td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>19.55</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>7.73</td></tr><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>Total score</td><td style='text-align: center; word-wrap: break-word;'>452.21 / 27.0</td><td style='text-align: center; word-wrap: break-word;'>267.35 / -</td><td style='text-align: center; word-wrap: break-word;'>121.19 / 15.3</td></tr></table>

<div style="text-align: center;">TABLE II: Difficulties, timing and score information for test sequences</div>

![Image](images/906e5ba59453e19526ac11a4033d98b5a6d71c8f0aef7ddff5e89ea1540b5d24.jpg)

![Image](images/ac51c59be3eed4865fb458469b5af3691fd0c85fddab76c7523f29fd6052934c.jpg)

![Image](images/b19274994430e7e287ef71c4a25c8a341b3e6dc2037e3053319231928b888467.jpg)

<div style="text-align: center;">Fig. 5: Perspective view of reconstructed scenes. From left to right: site_1, site_2, site_3. Trajectory is coloured in sequence from red, green, blue, magenta.</div>

2023年的数据集[^41]。这些数据集涉及使用Sevensense Robotics AG公司生产的Alphasense多摄像头开发套件采集的手持设备序列数据，该套件可将惯性测量单元（IMU）与四个灰度鱼眼摄像头同步以收集数据。对于机器人序列数据，则使用四个立体OAK-D摄像头以及安装在地面机器人上的高端Xsens MTi-670惯性测量单元。我们在实验中针对每个数据集保持了一致的参数设置。不过，在Hilt挑战赛2023的数据集中，我们发现site2_robot_1序列存在立体摄像头对之间的时间同步问题，因此该序列仅采用了搭配IMU的一对立体摄像头。对于另外两组机器人序列，我们选取了每组中同步效果最好的四个摄像头。这些数据集为ATE评估提供了多个控制点的毫米级精确真实值。我们将我们的方法与BAMF-SLAM[^23]（在单会话视觉/仅IMU赛道中排名第二）以及Maplab2.0[^43]（在多会话赛道中排名第二）进行了对比。相关结果如表II所示，以下几点值得注意：

- 我们在表II中列出了每个序列的难度、用时及得分信息。这些数据集面临着低光照、无纹理环境、摄像头不同步、数据丢失以及缺乏闭环探索场景等挑战。不过所有方法都能成功处理整个数据集，且未出现严重错误。

- 我们的方法性能更优，在单会话和多会话场景下均表现突出。其得分几乎是第二名的两倍。更详细的分析和定量结果可参阅我们的技术报告[^41]。

- 我们系统的运行性能也展现了其实际应用潜力。它仅需CPU就能在标准台式机上实时运行。而BAMF-SLAM[^23]却需要Nvidia GeForce RTX 4090显卡才能处理，且其运行速度仍比我们的系统慢1.6倍。

我们还在Hiltí SLAM挑战赛2022[^36]中测试了我们的方法，取得了130.2分的最佳成绩，这一分数分别是第二名40.9分的3倍。更多详情请参阅[^41]上的实时排行榜。

### 六、结论

在本文中，我们介绍了MAVIS——一种面向多摄像头系统的基于优化的视觉惯性SLAM系统。与其他方法相比，我们提出了基于$\mathbf{SE}_{2}(\mathbf{3})$指数的精确IMU预积分公式，有效提升了跟踪性能，尤其是在快速旋转和长时间积分场景下。我们还针对多摄像头系统扩展了前端跟踪和后端优化模块，并引入了相关实现细节以提升系统在复杂场景下的性能。通过多个数据集上的大量实验验证了该方法卓越的性能。我们认为，这个稳健且通用的SLAM系统对业界具有重要的实用价值。

---

[^1]: T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.

[^2]: 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.

[^3]: J. Delmerico and D. Scaramuzza, “A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots,” in 2018 IEEE international conference on robotics and automation (ICRA). IEEE, 2018, pp. 2502–2509.

[^4]: P. van Goor and R. Mahony, “Eqvio: An equivariant filter for visual-inertial odometry,” IEEE Transactions on Robotics, 2023.

[^5]: T. Qin, J. Pan, S. Cao, and S. Shen, “A general optimization-based framework for local odometry estimation with multiple sensors,” arXiv preprint arXiv:1901.03638, 2019.

[^6]: 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.

[^7]: 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.

[^8]: 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.

[^9]: 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, vol. 37, no. 6, pp. 1874–1890, 2021.

[^10]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration theory for fast and accurate visual-inertial navigation,” IEEE Transactions on Robotics, pp. 1–18, 2015.

[^11]: J.-s. Cui, F.-r. Zhang, D.-z. Feng, C. Li, F. Li, and Q.-c. Tian, "An improved slam based on rk-vif: Vision and inertial information fusion via runge-kutta method," Defence Technology, 2021.

[^13]: G. Huang, “Visual-inertial navigation: A concise review,” in 2019 International Conference on Robotics and Automation (ICRA), 2019, pp. 9572–9582.

[^12]: D. Scaramuzza, “Tutorial on visual odometry,” 2016. [Online]. Available: <http://mrsl.grasp.upenn.edu/loiannog/tutorial_ICRA2016/VO_Tutorial.pdf>

[^14]: S. Urban and S. Hinz, “Multicol-slam-a modular real-time multi-camera slam system,” arXiv preprint arXiv:1610.07336, 2016.

[^15]: 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.

[^16]: J. Kuo, M. Muglikar, Z. Zhang, and D. Scaramuzza, “Redesigning slam for arbitrary multi-camera systems,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), pp. 2116–2122.

[^17]: P. Furgale, U. Schwesinger, M. Rufli, W. Derendarz, H. Grimmett, P. Mühlfellner, S. Wonneberger, J. Timpner, S. Rottmann, B. Li, et al., "Toward automated driving in cities using close-to-market sensors: An overview of the v-charge project," in 2013 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2013, pp. 809–816.

[^18]: Y. Wang and L. Kneip, “On scale initialization in non-overlapping multi-perspective visual odometry,” in International Conference on Computer Vision Systems. Springer, 2017, pp. 144–157.

[^19]: L. Heng, B. Choi, Z. Cui, M. Geppert, S. Hu, B. Kuan, P. Liu, R. Nguyen, Y. C. Yeo, A. Geiger, G. H. Lee, M. Pollefeys, and T. Sattler, "Project AutoVision: Localization and 3D scene perception for an autonomous vehicle with a multi-camera system," arXiv, vol. 1809.05477, 2018.

[^20]: P. Liu, M. Geppert, L. Heng, T. Sattler, A. Geiger, and M. Pollefeys, “Towards robust visual odometry with a multi-camera system,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018, pp. 1154–1161.

[^21]: Y. Wang, K. Huang, X. Peng, H. Li, and L. Kneip, “Reliable frame-to-frame motion estimation for vehicle-mounted surround-view camera systems,” in 2020 IEEE International conference on robotics and automation (ICRA). IEEE, 2020, pp. 1660–1666.

[^22]: L. Zhang, D. Wisth, M. Camurri, and M. Fallon, “Balancing the budget: Feature selection and tracking for multi-camera visual-inertial odometry,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 1182–1189, 2022.

[^23]: W. Zhang, S. Wang, X. Dong, R. Guo, and N. Haala, “Bamf-slam: Bundle adjusted multi-fisheshe visual-inertial slam using recurrent field transforms,” in 2023 IEEE International Conference on Robotics and Automation (ICRA), 2023, pp. 6232–6238.

[^24]: T. Lupton and S. Sukkarieh, “Efficient integration of inertial observations into visual slam without initialization,” in 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2009, pp. 1547–1552.

[^25]: —, “Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions,” IEEE Transactions on Robotics, vol. 28, no. 1, pp. 61–76, 2012.

[^26]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “Imu preintegration on manifold for efficient visual-inertial maximum-a-posterior estimation,” in Robotics: Science and Systems, 2015.

[^27]: S. Shen, N. Michael, and V. Kumar, “Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft mavs,” in 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 5303–5310.

[^28]: K. Eckenhoff, P. Geneva, and G. Huang, “Direct visual-inertial navigation with analytical preintegration,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 1429–1435.

[^29]: —, “Closed-form preintegration methods for graph-based visual-inertial navigation,” The International Journal of Robotics Research, vol. 38, no. 5, pp. 563–586, 2019.

[^30]: J. Henawy, Z. Li, W. Y. Yau, G. Seet, and K. W. Wan, “Accurate imu preintegration using switched linear systems for autonomous systems,” in 2019 IEEE Intelligent Transportation Systems Conference (ITSC), 2019, pp. 3839–3844.

[^31]: J. Henawy, Z. Li, W.-Y. Yau, and G. Seet, “Accurate imu factor using switched linear systems for vio,” IEEE Transactions on Industrial Electronics, vol. 68, no. 8, pp. 7199–7208, 2020.

[^32]: A. Barrau and S. Bonnabel, “A mathematical framework for imu error propagation with applications to preintegration,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 5732–5738.

[^33]: J. Rehder, J. Nikolic, T. Schneider, T. Hinzmann, and R. Siegwart, "Extending kalibr: Calibrating the extrinsics of multiple imus and of individual axes," in 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016, pp. 4304–4311.

[^34]: P. Furgale, J. Rehder, and R. Siegwart, "Unified temporal and spatial calibration for multi-sensor systems," in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2013, pp. 1280–1286.

[^35]: P. van Goor, T. Hamel, and R. Mahony, “Constructive equivariant observer design for inertial navigation,” arXiv preprint arXiv:2308.11124, 2023.

[^36]: L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scaramuzza, and M. Fallon, “Hilti-oxford dataset: A millimeter-accurate benchmark for simultaneous localization and mapping,” IEEE Robotics and Automation Letters, vol. 8, no. 1, pp. 408–415, 2023.

[^37]: C. Campos, J. M. Montiel, and J. D. Tardós, “Inertial-only optimization for visual-inertial initialization,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), pp. 51–57.

[^38]: 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, vol. 35, no. 10, pp. 1157–1163, 2016.

[^39]: Y. He, B. Xu, Z. Ouyang, and H. Li, "A rotation-translation-decoupled solution for robust and efficient visual-inertial initialization," in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), June 2023, pp. 739–748.

[^40]: D. Gálvez-López and J. D. Tardos, “Bags of binary words for fast place recognition in image sequences,” IEEE Transactions on Robotics, vol. 28, no. 5, pp. 1188–1197, 2012.

[^41]: “Hilti slam challenge 2023,” <https://hilti-challenge.com/dataset-2023.html>, accessed: 2023-08-23.

[^42]: M. Grupp, “evo: Python package for the evaluation of odometry and slam.” <https://github.com/MichaelGrupp/evo>, 2017.

[^43]: A. Cramariuc, L. Bernreiter, F. Tschopp, M. Fehr, V. Reijgwart, J. Nieto, R. Siegwart, and C. Cadena, “maplab 2.0—a modular and multi-modal mapping framework,” IEEE Robotics and Automation Letters, vol. 8, no. 2, pp. 520–527, 2022.