# 基于不确定性感知的视觉-惯性SLAM与体素占据地图构建

郑在亨¹、西蒙·博切¹、塞巴斯蒂安·巴尔巴斯·莱纳¹、斯特凡·洛伊特内格¹

摘要—我们提出了一种视觉-惯性同时定位与建图方法，该方法将稀疏重投影误差、惯性测量单元预积分项以及相对位姿因子与稠密体素占据地图构建紧密耦合。其中，来自深度神经网络的深度预测结果会以完全概率化的方式被融合进来。具体而言，我们的方法具备严格的不确定性感知能力：首先，我们不仅利用机器人立体相机采集的数据得到深度与不确定性预测结果，还进一步将能提供不同基线范围内深度信息的运动立体视觉数据以概率方式融合，从而大幅提升建图精度。其次，预测并融合后的深度不确定性不仅会作用于占据概率的计算，还会影响生成的稠密子图之间的对齐因子，这些子图会被输入到概率非线性最小二乘估计器中。这种子图表示形式能够在大尺度范围内提供全局一致的几何信息。我们在两个基准数据集上对该方法进行了全面评估，结果显示其定位与建图精度均超越了现有技术水平，同时还能实时输出可直接用于后续机器人规划与控制的体素占据地图。

## I. 引言

状态估计是机器人感知、人机交互以及机器人探索等下游任务的基础模块。凭借互补的特性以及对度量尺度的观测能力，摄像头和惯性测量单元（IMU）已成为解决状态估计问题的典型极简传感器组合。随着OKVIS 2 $^{[1]}$、ORB-SLAM3 $^{[2]}$、OpenVINS $^{[3]}$、VINS-Fusion $^{[4]}$等技术的问世，视觉-惯性同时定位与建图（VI-SLAM）技术已被认为发展成熟。由一组稀疏或（半）稠密路标点构成的重投影误差或光度误差，是VI-SLAM问题中的标准残差项。然而，这种地图表达方式并不适用于需要显式建模密集自由/占据空间的更高层级的机器人任务。

稠密建图会依据传感器位姿，将机载测距或深度测量数据逐步整合到全局模型中，以构建体素地图，这类地图可采用占据网格 [^5] 或截断有向距离场（TSDF） [^6]、[^7] 作为底层地图表示形式。由于观测模型对重建质量起着重要作用，因此必须根据不确定性来决定测量的深度值对每个体素的贡献程度。理想情况下，这种不确定性应具备统计一致性。此外，

![Image](images/0e36fa9ec9e58f5fd5cce99c810a2119576b834e26fe30309207b3cdb1d9302f.jpg)

<div style="text-align: center;">Fig. 1. Our method estimates pose (black line) and volumetric occupancy represented in submaps (visualised here as colored meshes). We fuse stereo and MVS network depths based on their predicted uncertainties giving less noisy meshes, as can be qualitatively observed in the left mesh. Result from the Hilti-Oxford dataset [^10].</div>

在计算机视觉领域，基于有效不确定性或置信度的深度预测一直是热门研究方向[^8]，[^9]。根据相机配置的不同，单目深度网络可从单张图像中估计深度，立体网络能预测左右图像间的视差，而多视图立体（MVS）网络则能在给定多张已标定姿态的图像时，为参考图像帧估计深度。不过，尽管深度神经网络中的不确定性学习技术近期取得了进展，但在稠密建图领域仍广泛采用一种不确定性随深度平方增长的二次模型。该模型实际上假设视差不确定性恒定，虽是一种不错的近似，但普遍适用性有限。例如，对于纹理较少区域中不确定性较高的像素，为确保机器人感知的安全性，应降低对其的信任度。

为解决这一难题，我们提出了具备体积占用映射功能的VI-SLAM系统，该系统完全具备不确定性感知能力：占用信息的整合与视觉-惯性因子图均会根据深度融合的预估不确定性进行合理加权。我们基于静态立体与运动立体中小基线、大基线的互补特性，依据其预估的不确定性，对来自立体网络与MVS网络的深度信息进行最优融合。

---

如图1所示，我们的方法可实时输出机器人位姿，同时逐步构建体积占用地图；其中子地图锚定在局部坐标系下，以实现全局一致的地图表示。总而言之，本文的主要贡献如下。

- 我们引入了基于深度神经网络预估不确定性的深度融合技术，结合深度MVS网络与立体网络，进一步提升建图精度。

- 利用深度神经网络的深度预测结果及其相关不确定性，我们构建了完全概率化的VI-SLAM系统，该系统采用体积占用子映射方式，其中深度整合与因子图优化均会根据预估的不确定性进行加权。

- 我们在基准数据集上全面评估了所提方法在定位与建图方面的精度，结果显示该方法在立体-惯性设备上运行时能达到实时处理水平，且性能处于业界领先水平。

### 二、相关工作

我们回顾了多视图深度深度学习网络与稠密SLAM领域最相关的研究成果，包括概率化建图方法。

立体视觉网络旨在求解校正后的立体图像中的视差。受立体匹配技术的启发[^11]，这类网络通常遵循三个步骤：从立体图像对中提取特征、沿水平方向构建代价体，随后通过细化或上采样计算视差。ACVNet[^12]提出了注意力拼接代价体的概念，以突出代价体中最有价值的信息。Unimatch[^13]则设计了一种用于立体匹配、光流计算及双视图深度预测的统一网络，借助Transformer技术[^14]增强特征表达。类似地，深度多视图立体网络也采用类似的架构，但会基于相机几何关系利用透视变换来用未校正的图像构建代价体。MVSNet[^15]通过方差度量聚合多视图特征以生成代价体，随后再应用三维卷积操作。Simplerecon[^16]通过在代价体中融入相机几何信息，从而规避了三维卷积步骤。Simplemapping[^17]提出输入由稀疏地标生成的先验深度图，以此缩小深度平面的范围。借助深度神经网络在多视图深度预测领域的最新进展，我们针对最先进的方法[^13]、[^17]进行调整，使其能通过拉普拉斯损失学习深度不确定性。预测得到的不确定性会被应用到我们这套完全考虑不确定性的框架中，用于深度融合、占据图绘制以及因子图优化。

对于自主机器人而言，理解稠密几何信息至关重要。KinectFusion[^6]凭借TSDF融合技术与帧到模型配准算法开创了这一领域。ElasticFusion[^18]用稠密的曲面元素来建模环境，这些元素在闭环后会进行非刚性更新。CNN-SLAM[^19]和DeepFusion[^20]则利用卷积神经网络生成的稠密深度图，并融合运动立体技术得到的深度数据

![Image](images/93973b590082e86d21e30a1c9ca71035005e9a82241cac2c0dc603d526d71962.jpg)

<div style="text-align: center;">Fig. 2. An overview block diagram of the proposed method. We fuse depths from stereo and MVS networks from which the current submap is expanded. Fused depth and stereo depth formulate map-to-map and frame-to-map factors in the visual-inertial estimator.</div>

以进一步提升深度质量。Voxgraph[^21]主张采用子图表示方式来实现可扩展的系统，并提出了基于里程计估计得到的欧几里得距离场的子图对齐方法。关于SLAM中多视图立体网络的相关研究，TANDEM[^22]将深度多视图立体网络集成到视觉里程计中，利用TSDF生成额外的深度数据以实现图像配准。Simplemapping采用了Simplerecon[^16]，将多视图立体网络得到的深度数据与ORB-SLAM3[^2]提供的位姿信息及稀疏地标数据融合为TSDF，但并未引入来自建图模块的反馈机制。SigmaFusion[^23]基于DroidSLAM[^24]提供的信息矩阵预测稠密深度不确定性，通过不确定性整合实现了噪声更低的重建结果。不过，他们的方法对内存的消耗较大，且建图模块与估计器相互分离，属于临时搭建的建图系统，与我们的方法不同。D3VO[^25]提出在直接视觉里程计中利用从深度估计中得到的光度不确定性，但他们的方法并非完整的SLAM系统。最相关的工作是[^26]，该研究引入了占据点到点的因子来实现子图对齐。不过，与精确的激光雷达点云相比，含噪的深度测量数据需要针对下游任务进行合理的加权处理。

与以往的研究不同，我们的方法将视觉惯性估计器与体积占据模型紧密耦合，并通过占据点到点的因子来确保地图的全局一致性。除此之外，我们的核心贡献在于对不确定性的运用，这种不确定性会被传递到所有下游任务中。

### 三、定义与符号说明

本研究旨在基于惯性测量和多个摄像头数据，利用观测不确定性来估计机器人位姿以及子图中表示的体积占据值。子图被定义为具有几何重叠关系的局部地图表示，旨在减少因位姿漂移导致的建图误差；由于每个子图的位姿都可以在估计器更新后得到校正 $^{[21]}$，[^27]。我们的方法基于用于视觉-惯性估计的OKVIS2 $^{[1]}$，以及用于占据子图构建的Supereight2 $^{[5]}$。

世界坐标系 $\overrightarrow{F}_{W}$ 描述的是与重力方向对齐的坐标系，传感器坐标系 $\overrightarrow{F}_{S}$ 与惯性测量单元（IMU）坐标系重合，第i个摄像头坐标系 $\overrightarrow{F}_{C_{i}}$ 则以对应摄像头的光学中心为原点。第j个子图坐标系 $\overrightarrow{F}_{M_{j}}$ 则定义为新子图被创建时的传感器坐标系。

---

机器人的状态由以下形式表示：

 $$ \mathbf{x}=\left[\begin{matrix}{_{W}\mathbf{r}_{S}^{T}}&{\mathbf{q}_{W S}^{T}}&{_{W}\mathbf{v}^{T}}&{\mathbf{b}_{\mathrm{g}}^{T}}&{\mathbf{b}_{\mathrm{a}}^{T}}\\ \end{matrix}\right]^{T}, $$

其中 $_W \mathbf{r}_S$ 表示 $\overrightarrow{F}_S$ 在 $\overrightarrow{F}_W$ 中的位置，$\mathbf{q}_{WS}$ 是 $\overrightarrow{F}_S$ 相对于 $\overrightarrow{F}_W$ 的姿态对应的哈密顿四元数，$_W \mathbf{v}$ 是 $\overrightarrow{F}_S$ 相对于 $\overrightarrow{F}_W$ 的速度，而 $\mathbf{b}_g$、$\mathbf{b}_a$ 分别为陀螺仪和加速度计的偏差。采用齐次表示（斜体字体），从 $\overrightarrow{F}_B$ 到 $\overrightarrow{F}_A$ 的坐标变换可表示为 $_A \mathbf{r}_S = \mathbf{T}_{ABB} \mathbf{r}_S$，其中 $\mathbf{T}_{AB} \in \mathrm{SE}(3)$。

### 四、视觉-惯性估计器

### A. 概述

我们将能够预测深度及深度不确定性的立体视觉网络和多视图立体（MVS）网络引入视觉-惯性状态估计中，从而将深度信息以概率方式整合到占据映射中，并在因子图优化中对占据到点（地图到地图以及帧到地图）的因子进行加权。图2展示了我们的方法：给定多张图像和惯性测量数据后，我们会根据预测的不确定性对深度信息进行最优融合。融合后的深度图及其位姿会被整合到当前正在扩展的子图中。基于共视子图，我们能找到与最新子图重叠度最高的子图。地图到地图的因子由一对子图生成，而来自立体视觉网络的实时深度数据则用于计算帧到地图的因子。这些因子会根据网络预测的不确定性进行加权。我们在后续章节中会详细介绍各个模块。

### B. 占据子图构建

每个子图在其自身的坐标系中维护占据对数几率，我们的子图构建基于Supereight2 [^5]——这是一种基于八叉树的自适应多分辨率映射流程。给定摄像头位姿 $T_{MC}$ 和深度图像D，地图坐标系中某点的对数几率 $M\mathbf{p}$ 定义为：

 $$ l\left(_{M}\mathbf{p}\right)=\log\frac{P_{\mathrm{o c c}}\left(_{M}\mathbf{p}\mid\mathbf{T}_{M C},\mathbf{D}\right)}{1-P_{\mathrm{o c c}}\left(_{M}\mathbf{p}\mid\mathbf{T}_{M C},\mathbf{D}\right)}, $$

其中 $P_{occ}$ 为该点的占据概率。为了考虑深度测量的不确定性，我们采用分段线性逆传感器模型来近似该对数几率。

 $$ l\left(_{M}\mathbf{P}\right)=\left\{\begin{aligned}&l_{\min}&&d_{r}<-3\sigma_{d},\\ &\frac{\left|l_{\min}\right|}{3\sigma_{d}}d_{r}&&-3\sigma_{d}\leq d_{r}<\frac{\tau}{2},\\ &l_{\max}&&\frac{\tau}{2}\leq d_{r}<\tau.\end{aligned}\right. $$

此处，$d_r$ 表示查询点与沿光线方向的表面测量点之间的距离，$l_{\min}$ 是可配置的最小对数几率参数，$\sigma_d$ 是深度的标准差，$\tau$ 代表表面厚度，该厚度假定与深度测量值成正比，而 $l_{\max}$ 可根据连续性确定。为了整合多次测量结果，对数几率均值 $L(\cdot)$ 会按如下方式递归更新：

 $$ \begin{align*}L_{k}\left(_{M}\mathbf{p}\right)&=\frac{L_{k-1}\left(_{M}\mathbf{p}\right)w_{k-1}+l\left(_{M}\mathbf{p}\right)}{w_{k-1}+1},\\w_{k}&=\min\{w_{k-1}+1,\;w_{\max}\},\end{align*} $$

其中 $w_{k}$ 表示观测值的数量，而饱和值则为 $w_{max}$。Supereight2 采用启发式不确定性模型来确定 $\sigma_{d}$，例如线性或二次模型。不过这种近似在一般情况下并不成立：比如无纹理的墙面或物体边缘可能会导致深度预测结果具有高度不确定性。针对这一问题，我们提出利用像素级学习得到的不确定性作为深度神经网络的额外输出，以便将含噪深度信息整合到占据状态中。

### C. 因子图优化

我们的视觉-惯性状态估计器基于 OKVIS2 [^1]，这是一种以因子图形式表述的视觉-惯性 SLAM 算法；此外还有结合激光雷达的扩展版本 [^26]。该估计器会联合最小化下述代价函数。

 $$ \begin{aligned}c(\mathbf{x})&=\frac{1}{2}\sum_{i}\sum_{k\in\mathcal{K}}\sum_{j\in\mathcal{J}(i,k)}\rho_{\mathrm{c}}\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}\\&+\frac{1}{2}\sum_{k\in\mathcal{P}\cup\mathcal{K}}\rho_{\mathrm{t}}\left(\mathbf{e}_{\mathrm{o}2\mathrm{p}}^{k,o(k)^{T}}\mathbf{W}_{\mathrm{o}2\mathrm{p}}^{k,o(k)}\mathbf{e}_{\mathrm{o}2\mathrm{p}}^{k,o(k)}\right)\\&+\frac{1}{2}\sum_{m\in\mathcal{M}}\rho_{\mathrm{t}}\left(\mathbf{e}_{\mathrm{o}2\mathrm{p}}^{m,o(m)^{T}}\mathbf{W}_{\mathrm{o}2\mathrm{p}}^{m,o(m)}\mathbf{e}_{\mathrm{o}2\mathrm{p}}^{m,o(m)}\right).\tag{5}\end{aligned} $$

在该表达式中，带有柯西鲁棒化器 $\mathbf{e}_{\mathrm{r}}^{i,j,k}$ 的重投影误差 $\rho_{c}(\cdot)$ 涉及 $i^{th}$ 相机、包含近期帧和过去关键帧的集合 K，以及这些帧中的可见地物集合 $\mathcal{J}(i,k)$。IMU 预积分误差 $\mathbf{e}_{\mathrm{s}}^{k}$ 则是基于集合 K 中的位姿以及位姿图集合 $\mathcal{P}$（当前帧 f 除外）来定义的。此外，还加入了位于集合 $\mathbf{e}_{\mathrm{p}}^{r,c}$ 中且与 r 有边相连的 $r^{th}$ 位姿图帧与 $c^{th}$ 帧之间的相对位姿误差 $\mathcal{C}(r)$。加权矩阵 $\mathbf{W}_{r}, \mathbf{W}_{s}, \mathbf{W}_{p}$ 分别由重投影协方差矩阵的逆、IMU 不确定性传播以及地物边缘化处理结果得到。采用图基损失函数 $\mathbf{e}_{\mathrm{o}2\mathrm{p}}$ 的占据到点因子 $\rho_{c}(\cdot)$ 体现了“点应位于表面上（L=0）”这一特性，它包含了 N 个深度测量值。

 $$ \begin{align*}\mathbf{e}_{\mathrm{o}2\mathrm{p}}^{a,b}&=\left[e_{\mathrm{o}2\mathrm{p}_{1}}\quad\cdots\quad e_{\mathrm{o}2\mathrm{p}_{N}}\right]^{T},\\e_{\mathrm{o}2\mathrm{p}}&=\frac{L\left(\boldsymbol{T}_{S_{b}W}\boldsymbol{T}_{WS_{a}S_{a}}\boldsymbol{p}\right)}{\left\|\nabla L\left(_{S_{b}}\boldsymbol{p}\right)\right\|}.\end{align*} $$

在该表达式中，p 是通过利用网络输出的深度对像素进行反投影得到的，而 $\nabla L(\cdot)$ 则表示公式 (4) 中定义的对数几率 L 的梯度。根据用于确定点的帧不同，我们会采用帧到地图 $\mathbf{e}_{o2p}^{k,o(k)}$ 或地图到地图 $\mathbf{e}_{o2p}^{m,o(m)}$ 因子 [^26]。具体而言，$\mathbf{e}_{o2p}^{k,o(k)}$ 是由 $k^{th}$ 帧及其重叠最多的子图 $o(k)$ 构成的；而 $\mathbf{e}_{o2p}^{m,o(m)}$ 则是由 $m^{th}$ 子图及其重叠最多的子图 $o(m)$ 定义的。在本文中，帧到地图因子用于约束来自立体相机的当前帧，以便能在优化前构建该因子；而地图到地图因子则利用在 $m^{th}$ 子图中整合后的融合深度来对子图进行对齐。

---

假设表面附近的占据状态呈线性分布，则对应的占据到点加权矩阵定义为：

 $$ \begin{align*}\mathbf{W}_{\mathrm{o}2\mathrm{p}}^{a,b}&=\mathrm{diag}\left(\left[w_{\mathrm{o}2\mathrm{p}_{1}}\quad\cdots\quad w_{\mathrm{o}2\mathrm{p}_{N}}\right]\right),\\w_{\mathrm{o}2\mathrm{p}_{i}}&=\frac{1}{\sigma_{\mathrm{map}_{i}}^{2}+\sigma_{d_{i}}^{2}},\quad\sigma_{\mathrm{map}_{i}}=\frac{l_{\min}}{\left\|3\nabla L\left(S_{b}\mathbf{p}\right)\right\|},\end{align*} $$

其中总不确定性为占据图不确定性 $\sigma_{map}$ 与网络深度不确定性 $\sigma_{d}$ 之和。$l_{min}$ 表示公式 (3) 中定义的最小对数几率值。与高精度的激光雷达测量数据不同，深度不确定性往往包含含噪观测值，因此需将其按像素级分配，才能合理权衡其对优化损失的贡献。下一节中，我们将介绍如何从网络中获取这种不确定性。

 $$ \begin{array}{r}{\mathcal{L}_{\mathrm{s t}}(\boldsymbol{\theta})=\mathcal{L}_{u}(\boldsymbol{\theta})+\mathcal{L}_{\nabla u_{x}}(\boldsymbol{\theta})+\mathcal{L}_{\nabla u_{y}}(\boldsymbol{\theta}),}\end{array} $$

### D. 考虑不确定性的深度融合

获取可靠的深度信息以及相关的不确定性数据对于我们的下游任务而言至关重要。我们的核心思路是融合静态立体视觉与运动立体视觉，这两种方法互为补充：静态立体视觉即便在相机静止时也能提供可靠的小基线数据，而运动立体视觉则能根据相机运动情况产生较大的基线。我们的方法并不依赖于特定的网络，但我们发现Unimatch [^13]和MVS网络 [^17]在真实场景中的表现良好。不过，以往的网络只能预测视差或深度值，却无法给出对应的不确定性信息。因此，我们在基础架构上增加了不确定性解码器，并采用拉普拉斯损失函数来进行（随机性）不确定性学习。具体而言，我们的立体视觉网络损失函数如下：

其中 $\theta$ 表示网络权重，u代表视差。视差损失 $\mathcal{L}_{u}$ 按照 [^8]、[^9] 中的设定，采用拉普拉斯分布进行建模，其定义如下：

 $$ \mathcal{L}_{u}(\boldsymbol{\theta})=\sum_{i\in\mathcal{T}}\frac{\left|u_{i}-u_{\mathrm{g t}_{i}}\right|}{\sigma_{u_{i}}}+\log\sigma_{u_{i}}, $$

这里 $\mathcal{T}$ 表示训练集，包含立体图像对及对应的真实视差数据。为了获得更清晰的不确定性输出，我们额外加入了梯度损失 $\mathcal{L}_{\nabla u}$，其定义方式与 $\mathcal{L}_{u}$ 类似。沿水平和垂直方向的梯度不确定性是由视差不确定性推导而来的，公式如下：

 $$ \begin{align*}\sigma_{\nabla u_{x}}&=\sqrt{\sigma_{u}^{2}(x+1,y)+\sigma_{u}^{2}(x-1,y)},\\\sigma_{\nabla u_{y}}&=\sqrt{\sigma_{u}^{2}(x,y+1)+\sigma_{u}^{2}(x,y-1)}.\end{align*} $$

我们通过线性化方法将不确定性从视差传递到深度。

 $$ \hat{d}_{\mathrm{s t}}=\frac{f_{c}b}{u},\quad\sigma_{\mathrm{s t}}=\frac{f_{c}b}{u^{2}}\sigma_{u}, $$

其中 $f_{c}$ 为校正后的焦距，b表示立体基线。

![Image](images/20b563c45556f1b5ebc41d041667f85acc5cff26facf61f2f964445b9b542afe.jpg)

<div style="text-align: center;">Fig. 3. Predicted inverse (only for visualization purpose) depth and its corresponding standard deviation of (top) stereo network with the 11 cm baseline, (middle) MVS network with the 50 cm maximum baseline among 8 views, and (bottom) depth fusion in the EuRoC dataset.</div>

同样，我们对MVS网络的损失函数也进行了修改 [^17]

 $$ \mathcal{L}_{\mathrm{m v s}}(\phi)=\sum_{i\in\mathcal{T}}\frac{\left|\log d_{i}-\log d_{\mathrm{g t}_{i}}\right|}{\sigma_{l_{i}}}+\log\sigma_{l_{i}}, $$

在该网络中，模型会学习对数深度的不确定性。我们再通过线性化模型将对数深度转换为实际深度。

 $$ \begin{array}{r}{\hat{d}_{\mathrm{m v s}}=\exp\left(\log d_{i}\right),\quad\sigma_{\mathrm{m v s}}=\hat{d}_{\mathrm{m v s}}\sigma_{l}.}\end{array} $$

根据网络输出的逐像素估计值 $(\hat{d}_{\mathrm{st}}, \sigma_{\mathrm{st}})$、$(\hat{d}_{\mathrm{mvs}}, \sigma_{\mathrm{mvs}})$，在假设这两个估计值相互独立的前提下，我们可以最优地融合两种深度估计结果 [^28]，

 $$ \begin{align*}\hat{d}_{\mathrm{fuse}}&=\sigma_{\mathrm{fuse}}^{2}\left(\sigma_{\mathrm{st}}^{-2}\hat{d}_{\mathrm{st}}+\sigma_{\mathrm{mvs}}^{-2}\hat{d}_{\mathrm{mvs}}\right),\\\sigma_{\mathrm{fuse}}^{2}&=\left(\sigma_{\mathrm{st}}^{-2}+\sigma_{\mathrm{mvs}}^{-2}\right)^{-1}.\end{align*} $$

图3展示了一个示例：风扇支架的深度信息在立体视觉网络中更为精细，而较远物体的深度则在MVS网络中更清晰。融合后的深度自然能够基于不确定性感知的融合策略保留最优的深度结果。

### E. 子图管理

为了构建占用网格到点云的转换因子，保持子图均匀分布且相邻子图之间有足够但不过度的重叠区域至关重要。为此，我们会持续监测当前深度视锥体与最新子图之间的重叠体积。一旦该体积低于某个阈值 $\epsilon_{vol}$，就会生成新的子图。我们

---

<div style="text-align: center;">TABLE I</div>

<div style="text-align: center;">ABSOLUTE TRAJECTORY ERROR [M] IN THE EUROC DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>VINS-Fusion $^{[4]}$</td><td style='text-align: center; word-wrap: break-word;'>OKVIS2 $^{[1]}$</td><td style='text-align: center; word-wrap: break-word;'>DVI-SLAM $^{[3]}$\n[^31]</td><td style='text-align: center; word-wrap: break-word;'>Ours</td><td style='text-align: center; word-wrap: break-word;'>ORB-SLAM3 $^{[2]}$</td><td style='text-align: center; word-wrap: break-word;'>OKVIS2 $^{[1]}$</td><td style='text-align: center; word-wrap: break-word;'>Ours</td></tr><tr><td style='text-align: center; word-wrap: break-word;'></td><td colspan="4">Causal</td><td colspan="3">Non-causal</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>MH01</td><td style='text-align: center; word-wrap: break-word;'>0.166</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.042</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.036</td><td style='text-align: center; word-wrap: break-word;'>0.021</td><td style='text-align: center; word-wrap: break-word;'>0.019</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>MH02</td><td style='text-align: center; word-wrap: break-word;'>0.152</td><td style='text-align: center; word-wrap: break-word;'>0.029</td><td style='text-align: center; word-wrap: break-word;'>0.046</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>0.033</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.019</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>MH03</td><td style='text-align: center; word-wrap: break-word;'>0.125</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>0.081</td><td style='text-align: center; word-wrap: break-word;'>0.037</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.029</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>MH04</td><td style='text-align: center; word-wrap: break-word;'>0.280</td><td style='text-align: center; word-wrap: break-word;'>0.065</td><td style='text-align: center; word-wrap: break-word;'>0.072</td><td style='text-align: center; word-wrap: break-word;'>0.072</td><td style='text-align: center; word-wrap: break-word;'>0.051</td><td style='text-align: center; word-wrap: break-word;'>0.059</td><td style='text-align: center; word-wrap: break-word;'>0.071</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>MH05</td><td style='text-align: center; word-wrap: break-word;'>0.284</td><td style='text-align: center; word-wrap: break-word;'>0.087</td><td style='text-align: center; word-wrap: break-word;'>0.069</td><td style='text-align: center; word-wrap: break-word;'>0.081</td><td style='text-align: center; word-wrap: break-word;'>0.082</td><td style='text-align: center; word-wrap: break-word;'>0.061</td><td style='text-align: center; word-wrap: break-word;'>0.066</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V101</td><td style='text-align: center; word-wrap: break-word;'>0.076</td><td style='text-align: center; word-wrap: break-word;'>0.037</td><td style='text-align: center; word-wrap: break-word;'>0.059</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.038</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.035</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V102</td><td style='text-align: center; word-wrap: break-word;'>0.069</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>0.034</td><td style='text-align: center; word-wrap: break-word;'>0.028</td><td style='text-align: center; word-wrap: break-word;'>0.014</td><td style='text-align: center; word-wrap: break-word;'>0.014</td><td style='text-align: center; word-wrap: break-word;'>0.014</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V103</td><td style='text-align: center; word-wrap: break-word;'>0.114</td><td style='text-align: center; word-wrap: break-word;'>0.035</td><td style='text-align: center; word-wrap: break-word;'>0.028</td><td style='text-align: center; word-wrap: break-word;'>0.029</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.020</td><td style='text-align: center; word-wrap: break-word;'>0.021</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V201</td><td style='text-align: center; word-wrap: break-word;'>0.066</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>0.040</td><td style='text-align: center; word-wrap: break-word;'>0.033</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.023</td><td style='text-align: center; word-wrap: break-word;'>0.024</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V202</td><td style='text-align: center; word-wrap: break-word;'>0.091</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>0.039</td><td style='text-align: center; word-wrap: break-word;'>0.028</td><td style='text-align: center; word-wrap: break-word;'>0.014</td><td style='text-align: center; word-wrap: break-word;'>0.019</td><td style='text-align: center; word-wrap: break-word;'>0.016</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V203</td><td style='text-align: center; word-wrap: break-word;'>0.096</td><td style='text-align: center; word-wrap: break-word;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>0.055</td><td style='text-align: center; word-wrap: break-word;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>0.024</td><td style='text-align: center; word-wrap: break-word;'>0.027</td><td style='text-align: center; word-wrap: break-word;'>0.022</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Avg</td><td style='text-align: center; word-wrap: break-word;'>0.138</td><td style='text-align: center; word-wrap: break-word;'>0.043</td><td style='text-align: center; word-wrap: break-word;'>0.051</td><td style='text-align: center; word-wrap: break-word;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>0.035</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></tr></table>

 $^{1}$ 结果取自 [^2]。

 $^{2}$ 为我们自行获得的结果。

 $^{3}$ 结果取自论文。

我们通过采样视锥体内的点来近似计算这一数值，并检验这些点是否曾在之前的子图中被观测到。

我们基于最古老的共视子图来构建占用网格到点云的转换因子，以实现几何闭环检测。我们会记录从最新子图到各共视子图中共同观测到的稀疏路标点。随后，重叠程度最高的子图会被认定为共视子图中包含至少 $M_{min}$ 个点的最古老子图。在优化因子图（5）之后，子图的位姿也会相应更新，从而确保地图的全局一致性。

### V. 实验结果

我们在两个公开数据集EuRoC [^29]和Hilti-Oxford [^10]上评估了该方法在定位与建图精度方面的表现。评估前，会将估计出的轨迹在SE(3)空间中与真实轨迹对齐。对于EuRoC数据集，我们以绝对轨迹误差（ATE）的均方根误差来衡量定位精度；而在Hilti-Oxford数据集中，则采用预定义的评分标准，其中位置误差[^1] [^10]厘米会被换算为[^0] [^100]个点。我们分别统计了因果误差与非因果误差：前者仅考虑截至当前时刻的测量数据，后者则对应最终的闭环轨迹[^1]。为了评估建图精度，我们先利用Marching Cubes算法[^30]重建子图网格，再结合子图位姿将其拼接起来。估计顶点与真实顶点均按1立方厘米的体素大小进行下采样，随后通过点对面ICP算法对两组顶点进行对齐。我们以所有估计顶点到真实顶点的平均距离（无阈值限制，单位为米）来表征精度，而以真实顶点中距离估计顶点0.2米以内的比例来表征完整性。

### A. 实现细节

我们从TartanAir数据集[^13]上的预训练权重出发，对立体网络[^32]进行微调，未采用后处理优化以实现快速推理。

<div style="text-align: center;">TABLE II</div>

<div style="text-align: center;">MESH ACCURACY [M] WITHOUT THRESHOLD AND COMPLETENESS [%] WITH 0.2M THRESHOLD IN THE EUROC DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>Simplemapping $^{[17]}$</td><td style='text-align: center; word-wrap: break-word;'>Ours-Mono</td><td style='text-align: center; word-wrap: break-word;'>Ours</td><td style='text-align: center; word-wrap: break-word;'>Simplemapping $^{[17]}$</td><td style='text-align: center; word-wrap: break-word;'>Ours-Mono</td><td style='text-align: center; word-wrap: break-word;'>Ours</td></tr><tr><td style='text-align: center; word-wrap: break-word;'></td><td colspan="3">Accuracy</td><td colspan="3">Completeness</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V101</td><td style='text-align: center; word-wrap: break-word;'>0.176</td><td style='text-align: center; word-wrap: break-word;'>0.076</td><td style='text-align: center; word-wrap: break-word;'>0.042</td><td style='text-align: center; word-wrap: break-word;'>40.58</td><td style='text-align: center; word-wrap: break-word;'>41.74</td><td style='text-align: center; word-wrap: break-word;'>44.28</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V102</td><td style='text-align: center; word-wrap: break-word;'>0.123</td><td style='text-align: center; word-wrap: break-word;'>0.073</td><td style='text-align: center; word-wrap: break-word;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>55.07</td><td style='text-align: center; word-wrap: break-word;'>54.96</td><td style='text-align: center; word-wrap: break-word;'>58.38</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V103</td><td style='text-align: center; word-wrap: break-word;'>0.161</td><td style='text-align: center; word-wrap: break-word;'>0.093</td><td style='text-align: center; word-wrap: break-word;'>0.048</td><td style='text-align: center; word-wrap: break-word;'>47.74</td><td style='text-align: center; word-wrap: break-word;'>55.92</td><td style='text-align: center; word-wrap: break-word;'>68.48</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V201</td><td style='text-align: center; word-wrap: break-word;'>0.155</td><td style='text-align: center; word-wrap: break-word;'>0.090</td><td style='text-align: center; word-wrap: break-word;'>0.072</td><td style='text-align: center; word-wrap: break-word;'>41.93</td><td style='text-align: center; word-wrap: break-word;'>39.62</td><td style='text-align: center; word-wrap: break-word;'>43.64</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V202</td><td style='text-align: center; word-wrap: break-word;'>0.156</td><td style='text-align: center; word-wrap: break-word;'>0.073</td><td style='text-align: center; word-wrap: break-word;'>0.069</td><td style='text-align: center; word-wrap: break-word;'>56.37</td><td style='text-align: center; word-wrap: break-word;'>54.79</td><td style='text-align: center; word-wrap: break-word;'>57.66</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>V203</td><td style='text-align: center; word-wrap: break-word;'>0.090</td><td style='text-align: center; word-wrap: break-word;'>0.089</td><td style='text-align: center; word-wrap: break-word;'>0.072</td><td style='text-align: center; word-wrap: break-word;'>62.58</td><td style='text-align: center; word-wrap: break-word;'>58.42</td><td style='text-align: center; word-wrap: break-word;'>59.96</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Avg</td><td style='text-align: center; word-wrap: break-word;'>0.144</td><td style='text-align: center; word-wrap: break-word;'>0.082</td><td style='text-align: center; word-wrap: break-word;'>0.057</td><td style='text-align: center; word-wrap: break-word;'>50.71</td><td style='text-align: center; word-wrap: break-word;'>50.91</td><td style='text-align: center; word-wrap: break-word;'>55.40</td></tr></table>

我们自行使用相同指标得出的结果。

![Image](images/e3ec5da79ca8fb689acf0952d2112548785ef5474a56bf6e99effb78b0ddd4da.jpg)

<div style="text-align: center;">Fig. 4. Reconstructed mesh with mesh-to-point error encoding in V102 sequence. (Left) Ours and (Right) Simplemapping [^17].</div>

接着，我们在相关体积层之后加入源自MonoDepth2 [^33]的解码器，用于不确定性学习，同时冻结损失函数中的其他权重（公式8）。同样，我们也为MVS网络[^17]添加了不确定性解码器，其架构与我们基础网络的深度解码器一致。随后，我们基于预训练权重，利用损失函数（公式13）开始训练不确定性模型，同时冻结ScanNet数据集[^34]中的其余权重。训练完成后，我们通过乘以一个恒定增益来对不确定性预测进行校准，以确保验证集中平均马氏距离等于单位距离。

输入图像会被下采样至$512\times384$分辨率，并以10 Hz的频率输入网络。我们将优化因子图后得到的、带有最新位姿的8张图像，以及基线长度为3厘米或10厘米的稀疏路标深度图，一并输入MVS网络。每个子图的体素分辨率设为2.5厘米。我们设定$l_{\min} = -5.015$、$\epsilon_{\mathrm{vol}} = 0.2$、$M_{\min} = 100$，帧到地图因子和地图到地图因子分别最多包含200个和1000个点。若点数量超过阈值，我们会将不确定性最低的点进行下采样。优化过程采用Ceres求解器[^35]，且不同数据集采用的参数保持一致。所有实验均在配备Intel i7-13700 CPU和Nvidia RTX3080 GPU的电脑上运行。

---

<div style="text-align: center;">TABLE III</div>

<div style="text-align: center;">LOCALIZATION SCORES IN THE HILTI-OXFORD DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td colspan="2">OKVIS2 $^{[1]}$ Ours</td><td style='text-align: center; word-wrap: break-word;'>BAMF-SLAM $^{[2][36]}$</td><td style='text-align: center; word-wrap: break-word;'>XR Penguin $^{[37]}$</td><td colspan="2">OKVIS2 $^{[1]}$ Ours</td></tr><tr><td style='text-align: center; word-wrap: break-word;'></td><td colspan="2">Causal</td><td colspan="4">Non-causal</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp01</td><td style='text-align: center; word-wrap: break-word;'>8.46</td><td style='text-align: center; word-wrap: break-word;'>4.62</td><td style='text-align: center; word-wrap: break-word;'>15.38</td><td style='text-align: center; word-wrap: break-word;'>40.00</td><td style='text-align: center; word-wrap: break-word;'>45.38</td><td style='text-align: center; word-wrap: break-word;'>55.38</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp02</td><td style='text-align: center; word-wrap: break-word;'>4.09</td><td style='text-align: center; word-wrap: break-word;'>0.45</td><td style='text-align: center; word-wrap: break-word;'>4.55</td><td style='text-align: center; word-wrap: break-word;'>16.82</td><td style='text-align: center; word-wrap: break-word;'>15.91</td><td style='text-align: center; word-wrap: break-word;'>24.09</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp03</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>8.24</td><td style='text-align: center; word-wrap: break-word;'>16.47</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp07</td><td style='text-align: center; word-wrap: break-word;'>6.67</td><td style='text-align: center; word-wrap: break-word;'>15.00</td><td style='text-align: center; word-wrap: break-word;'>1.67</td><td style='text-align: center; word-wrap: break-word;'>11.67</td><td style='text-align: center; word-wrap: break-word;'>15.00</td><td style='text-align: center; word-wrap: break-word;'>21.67</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp09</td><td style='text-align: center; word-wrap: break-word;'>5.00</td><td style='text-align: center; word-wrap: break-word;'>2.50</td><td style='text-align: center; word-wrap: break-word;'>1.25</td><td style='text-align: center; word-wrap: break-word;'>15.00</td><td style='text-align: center; word-wrap: break-word;'>10.62</td><td style='text-align: center; word-wrap: break-word;'>16.88</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp11</td><td style='text-align: center; word-wrap: break-word;'>4.00</td><td style='text-align: center; word-wrap: break-word;'>2.00</td><td style='text-align: center; word-wrap: break-word;'>2.00</td><td style='text-align: center; word-wrap: break-word;'>20.00</td><td style='text-align: center; word-wrap: break-word;'>36.00</td><td style='text-align: center; word-wrap: break-word;'>38.00</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp15</td><td style='text-align: center; word-wrap: break-word;'>14.44</td><td style='text-align: center; word-wrap: break-word;'>20.00</td><td style='text-align: center; word-wrap: break-word;'>7.78</td><td style='text-align: center; word-wrap: break-word;'>24.67</td><td style='text-align: center; word-wrap: break-word;'>47.78</td><td style='text-align: center; word-wrap: break-word;'>43.33</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>exp21</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>4.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td><td style='text-align: center; word-wrap: break-word;'>0.00</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Total</td><td style='text-align: center; word-wrap: break-word;'>42.66</td><td style='text-align: center; word-wrap: break-word;'>44.57</td><td style='text-align: center; word-wrap: break-word;'>40.86</td><td style='text-align: center; word-wrap: break-word;'>150.62</td><td style='text-align: center; word-wrap: break-word;'>170.70</td><td style='text-align: center; word-wrap: break-word;'>199.35</td></tr></table>

$^{1}$ 我们自行得出的结果，包含了在线外参校准环节。

$^{2}$ 结果取自排行榜[^38]。

### B. EuRoC数据集

该数据集提供了由无人机拍摄的立体图像以及IMU测量数据，还包括Vicon实验室的地面真实轨迹和点云，精度达到毫米级 [^29]。我们采用立体图像对来训练立体网络，用左图来训练MVS网络。表I总结了ATE指标，所有对比方法均采用立体+惯性配置。我们的方法通过在因果评估中加入占用到点的因子，改进了基线方法OKVIS2 [^1]。在非因果评估中，我们的平均精度与基线方法相当。这表明深度融合未能达到3厘米级的精度，不过这一水平已经相当出色。另一方面，我们的方法以明显优势超越了现有最先进的方法。由于我们的方法将定位与体积建图紧密耦合，我们也在表II中评估了建图精度。为了与单目惯性设置下的Simplemapping [^17]进行公平比较，我们实现了仅使用左图而不用立体网络的Ours-Mono版本。所提出的方法在精度和完整性方面均表现最佳，而Ours-Mono也优于竞争对手。图4对比了建图精度，结果显示我们的方法尤其能提升房间边缘处的网格精度。

<div style="text-align: center;">TABLE IV</div>

### C. Hilt-Oxford数据集

该数据集提供了由手持设备采集的来自5个摄像头的图像及IMU测量数据、稀疏的地面真实位置信息，以及毫米级精度的密集点云 [^10]。我们采用全部5个摄像头的数据来训练视觉惯性估计器，用2个前置摄像头训练立体网络，用左前摄像头训练MVS网络。我们在线校准摄像头与IMU之间的外部参数。表III报告了挑战序列中的定位得分，我们的非因果实现版本还包含了最终的束调整步骤。撰写本文时，我们的方法在已发表的方法中于排行榜上位列第一。就平均位置误差而言，我们的因果得分为13.3厘米，

<div style="text-align: center;">ABLATION STUDY: ATE IN CAUSAL (C), NON-CAUSAL (NC), ACCURACY (ACC.), AND COMPLETENESS (COMP.) IN EUROC DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>S</td><td style='text-align: center; word-wrap: break-word;'>U</td><td style='text-align: center; word-wrap: break-word;'>F</td><td style='text-align: center; word-wrap: break-word;'>ATE-C [m]</td><td style='text-align: center; word-wrap: break-word;'>ATE-NC [m]</td><td style='text-align: center; word-wrap: break-word;'>Acc. [m]</td><td style='text-align: center; word-wrap: break-word;'>Comp. [%]</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>Baseline [^1]</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;'>0.043</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>-</td><td style='text-align: center; word-wrap: break-word;'>-</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>+ Stereo network</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;'>0.045</td><td style='text-align: center; word-wrap: break-word;'>0.032</td><td style='text-align: center; word-wrap: break-word;'>0.080</td><td style='text-align: center; word-wrap: break-word;'>50.38</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>+ Uncertainty</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;'>0.043</td><td style='text-align: center; word-wrap: break-word;'>0.031</td><td style='text-align: center; word-wrap: break-word;'>0.072</td><td style='text-align: center; word-wrap: break-word;'>50.31</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>+ Depth fusion</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;'>0.041</td><td style='text-align: center; word-wrap: break-word;'>0.030</td><td style='text-align: center; word-wrap: break-word;'>0.057</td><td style='text-align: center; word-wrap: break-word;'>55.40</td></tr></table>

<div style="text-align: center;">TABLE V</div>

<div style="text-align: center;">TIMING IN MEAN $\pm\sigma$ [MS] MEASURED IN THE EUROC DATASET</div>

<table border=1 style='margin: auto; word-wrap: break-word;'><tr><td style='text-align: center; word-wrap: break-word;'></td><td style='text-align: center; word-wrap: break-word;'>VI optimization</td><td style='text-align: center; word-wrap: break-word;'>Occupancy update</td><td style='text-align: center; word-wrap: break-word;'>Stereo network</td><td style='text-align: center; word-wrap: break-word;'>MVS network</td></tr><tr><td style='text-align: center; word-wrap: break-word;'>OKVIS2 [^1]</td><td style='text-align: center; word-wrap: break-word;'>24.2 $\pm$ 5.3</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></tr><tr><td style='text-align: center; word-wrap: break-word;'>Ours</td><td style='text-align: center; word-wrap: break-word;'>34.3 $\pm$ 6.6</td><td style='text-align: center; word-wrap: break-word;'>11.6 $\pm$ 3.6</td><td style='text-align: center; word-wrap: break-word;'>56.3 $\pm$ 24.1</td><td style='text-align: center; word-wrap: break-word;'>75.9 $\pm$ 9.7</td></tr></table>

非因果得分为6.1厘米，该得分源自相应序列。图1展示了exp04中带颜色的子图网格中所估计的轨迹，该场景有可用的地面真实点云——我们的方法能达到9.4厘米的精度和62.2%的完整性。

### D. 消融实验

本研究验证了我们所提方法的有效性。表IV汇总了EuRoC数据集中所有序列的平均结果。引入不确定性感知后，定位精度得到了提升。此外，利用网络预测的不确定性替代启发式二次不确定性模型 [^5] 后，建图精度提升了10%；采用深度融合后，建图精度更是提升了29%。图1从定性角度展示了我们的深度融合方法相比采用启发式不确定性模型的立体网络，能生成噪声更少、更完整的网格。

### E. 运行时间与内存占用

我们在EuRoC数据集的MH01序列中测量了计算时间，结果如表V所示。我们的方法在后台运行闭环优化时，实时估计器能达到29 Hz的运行频率，而作为基线的OKVIS2能达到41 Hz。受MVS网络的限制，我们的建图模块运行频率为13 Hz。另外，推理过程中MVS网络和立体网络仅占用3.51 GB的GPU内存。

### VI. 结论

我们提出了一种视觉-惯性SLAM方法，该方法通过不确定性感知的占用到点因子，将定位与体积占用映射紧密耦合。为了进一步提高映射精度，我们利用不确定性学习对立体视觉网络和MVS网络进行微调。随后，基于这些网络预测的不确定性，我们对它们输出的深度信息进行最优融合，并将结果用于子地图构建中的优化与深度整合。通过在基准数据集上的评估，我们的方法在定位和映射精度方面均优于现有最先进方法，同时还能实时生成密集的占用信息（13Hz）。在未来的工作中，我们计划考虑动态物体，引入认知深度不确定性，并将该方法与机器人导航及控制相结合，以兼顾状态不确定性。

---

## 参考文献

[^1]: S. Leutenegger, “OKVIS2: Realtime Scalable Visual-Inertial SLAM with Loop Closure,” arXiv preprint arXiv:2202.09199, 2022.

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

[^3]: 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), 2020, pp. 4666–4672.

[^4]: T. Qin, S. Cao, J. Pan, and S. Shen, “A general optimization-based framework for global pose estimation with multiple sensors,” arXiv preprint arXiv:1901.03642, 2019.

[^5]: N. Funk, J. Tarrio, S. Papatheodorou, M. Popović, P. F. Alcantarilla, and S. Leutenegger, “Multi-resolution 3D mapping with explicit free space representation for fast and accurate mobile robot motion planning,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3553–3560, 2021.

[^6]: 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 IEEE International Symposium on Mixed and Augmented Reality, 2011, pp. 127–136.

[^7]: F. Furrer, T. Novkovic, M. Fehr, A. Gawel, M. Grinvald, T. Sattler, R. Siegwart, and J. Nieto, "Incremental Object Database: Building 3D Models from Multiple Partial Observations," in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018, pp. 6835–6842.

[^8]: A. Kendall and Y. Gal, “What uncertainties do we need in Bayesian deep learning for computer vision?” Advances in Neural Information Processing Systems, vol. 30, 2017.

[^9]: M. Poggi, F. Aleotti, F. Tosi, and S. Mattoccia, “On the uncertainty of self-supervised monocular depth estimation,” in IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 2020, pp. 3227–3237.

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

[^11]: D. Scharstein and R. Szeliski, “A taxonomy and evaluation of dense two-frame stereo correspondence algorithms,” International Journal of Computer Vision, vol. 47, pp. 7–42, 2002.

[^12]: G. Xu, Y. Wang, J. Cheng, J. Tang, and X. Yang, “Accurate and efficient stereo matching via attention concatenation volume,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 46, no. 4, pp. 2461–2474, 2023.

[^13]: H. Xu, J. Zhang, J. Cai, H. Rezatofighi, F. Yu, D. Tao, and A. Geiger, "Unifying flow, stereo and depth estimation," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 45, no. 11, pp. 13941–13958, 2023.

[^14]: A. Dosovitskiy, “An image is worth 16x16 words: Transformers for image recognition at scale,” arXiv preprint arXiv:2010.11929, 2020.

[^15]: Y. Yao, Z. Luo, S. Li, T. Fang, and L. Quan, “MVSNet: Depth inference for unstructured multi-view stereo,” in the European conference on computer vision (ECCV), 2018, pp. 767–783.

[^16]: M. Sayed, J. Gibson, J. Watson, V. Prisacariu, M. Firman, and C. Godard, “SimpleRecon: 3D reconstruction without 3D convolutions,” in the European Conference on Computer Vision (ECCV), 2022, pp. 1–19.

[^17]: Y. Xin, X. Zuo, D. Lu, and S. Leutenegger, “Simplemapping: Real-time visual-inertial dense mapping with deep multi-view stereo,” in IEEE International Symposium on Mixed and Augmented Reality (ISMAR), 2023, pp. 273–282.

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

[^19]: K. Tateno, F. Tombari, I. Laina, and N. Navab, “CNN-SLAM: Real-time dense monocular slam with learned depth prediction,” in the IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2017, pp. 6243–6252.

[^20]: T. Laidlow, J. Czarnowski, and S. Leutenegger, “DeepFusion: Real-time dense 3D reconstruction for monocular SLAM using single-

查看深度与梯度预测，”载于2019年国际机器人与自动化会议（ICRA），第4068–4074页。

[^21]: V. Reijgwart, A. Millane, H. Oleynikova, R. Siegwart, C. Cadena, and J. Nieto, “Voxgraph: Globally consistent, volumetric mapping using signed distance function submaps,” IEEE Robotics and Automation Letters, vol. 5, no. 1, pp. 227–234, 2019.

[^22]: L. Koestler, N. Yang, N. Zeller, and D. Cremers, “TANDEM: Tracking and dense mapping in real-time using deep multi-view stereo,” in Conference on Robot Learning, 2022, pp. 34–45.

[^23]: A. Rosinol, J. J. Leonard, and L. Carlone, “Probabilistic volumetric fusion for dense monocular SLAM,” in Proceedings of the IEEE/CVF Winter Conference on Applications of Computer Vision, 2023, pp. 3097–3105.

[^24]: Z. Teed and J. Deng, “Droid-SLAM: Deep visual slam for monocular, stereo, and RGB-D cameras,” Advances in neural information processing systems, vol. 34, pp. 16558–16569, 2021.

[^25]: N. Yang, L. v. Stumberg, R. Wang, and D. Cremers, “D3vo: Deep depth, deep pose and deep uncertainty for monocular visual odometry,” in Proceedings of the IEEE/CVF conference on computer vision and pattern recognition, 2020, pp. 1281–1292.

[^26]: S. Boche, S. B. Laina, and S. Leutenegger, “Tightly-Coupled LiDAR-Visual-Inertial SLAM and Large-Scale Volumetric Occupancy Mapping,” IEEE International Conference on Robotics and Automation (ICRA), 2024.

[^27]: S. B. Laina, S. Boche, S. Papatheodorou, D. Tzoumanikas, S. Schaefer, H. Chen, and S. Leutenegger, “Scalable Autonomous Drone Flight in the Forest with Visual-Inertial SLAM and Dense Submaps Built without LiDAR,” arXiv preprint arXiv:2403.09596, 2024.

[^28]: N. A. Carlson, “Federated square root filter for decentralized parallel processors,” IEEE Transactions on Aerospace and Electronic Systems, vol. 26, no. 3, pp. 517–525, 1990.

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

[^30]: W. E. Lorenzen and H. E. Cline, “Marching cubes: A high resolution 3d surface construction algorithm,” in Seminal graphics: pioneering efforts that shaped the field, 1998, pp. 347–353.

[^31]: X. Peng, Z. Liu, W. Li, P. Tan, S. Cho, and Q. Wang, “DVI-SLAM: A dual visual inertial SLAM network,” IEEE International Conference on Robotics and Automation (ICRA), pp. 12020–12026, 2024.

[^32]: W. Wang, D. Zhu, X. Wang, Y. Hu, Y. Qiu, C. Wang, Y. Hu, A. Kapoor, and S. Scherer, “Tartanair: A dataset to push the limits of visual slam,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020, pp. 4909–4916.

[^33]: C. Godard, O. Mac Aodha, M. Firman, and G. J. Brostow, “Digging into self-supervised monocular depth estimation,” in the IEEE/CVF International Conference on Computer Vision (CVPR), 2019, pp. 3828–3838.

[^34]: A. Dai, A. X. Chang, M. Savva, M. Halber, T. Funkhouser, and M. Nießner, "ScanNet: Richly-annotated 3D reconstructions of indoor scenes," in the IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2017, pp. 5828–5839.

[^35]: S. Agarwal, K. Mierle, and Others, "Ceres Solver," 8 2024. [Online]. Available: <https://github.com/ceres-solver/ceres-solver>

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

[^37]: Y. Wang, Y. Ng, I. Sa, A. Parra, C. Rodriguez-Opazo, T. Lin, and H. Li, "MAVIS: Multi-Camera Augmented Visual-Inertial SLAM using SE2 (3) Based Exact IMU Pre-integration," in IEEE International Conference on Robotics and Automation (ICRA), 2024, pp. 1694–1700.

[^38] “HILTI SLAM挑战赛2022排行榜”，2025年2月。[在线]。可访问：<https://hilti-challenge.com/leader-board-2022.html>