# UAF-LIVO：一种能应对跨模态几何不一致性的、基于特征的不确定性感知激光雷达-惯性-视觉里程计 罗志鹏 $^{D}$，IEEE学生会员；胡明楠 $^{D}$；王哲明 $^{D}$，IEEE会员；陈波 $^{D}$，IEEE高级会员。 摘要—本文提出UAF-LIVO，这是一种能应对跨模态几何不一致性的基于特征的序列式激光雷达-惯性-视觉里程计系统。虽然序列融合能够缓解激光雷达与视觉测量之间固有的鲁棒性不平衡问题，但它仍极易受到跨模态错位的影响。这类差异通常源于传感器校准不精确，以及动态或恶劣环境下的视觉特征错位。为解决这一问题，我们提出了一种几何一致的视觉测量构建与更新策略。匹配的特征被分为三类：局部平面支撑型、体素地图支撑型以及仅方位型。针对每一类特征，我们都推导出了完整的传播模型，以明确量化测量不确定性。这一理论基础使得UAF-LIVO能够在IESKF框架内自适应地降低条件不佳的观测值的权重，并剔除异常值。在包含多种传感器配置和场景的公共数据集上进行的实验表明，与现有最先进方法相比，UAF-LIVO在精度上具备竞争力，且在面对几何不一致性时表现出稳定的鲁棒性。 索引词—定位、SLAM、传感器融合。 ## I. 引言 惯性辅助里程计主要与两种互补的传感模式相结合：视觉和激光雷达。视觉-惯性里程计（VIO）将视觉测量数据与惯性测量单元（IMU）的传播结果相融合，以实现具有度量尺度的六自由度估计 [^1]，[^2]；但在纹理较少或光照复杂的环境中，其特征提取的可靠性较差，且需要足够的视差才能保证稳健初始化 [^3]。相比之下，激光雷达-惯性里程计（LIO）能够从稳定的几何观测中得出精确的度量几何信息 [^4]，[^5]；近年来取得的一系列进展，包括高带宽激光雷达里程计 [^6]、基于不变卡尔曼滤波的更新方法 [^7] 以及以平面为中心的特征表示方式 [^8]，进一步提升了其精度和鲁棒性。不过，激光雷达缺乏外观信息，在几何特征稀疏或模糊的场景中依然表现脆弱。 为了充分利用激光雷达与视觉测量的互补优势，人们提出了激光雷达-惯性-视觉里程计（LIVO）框架 [^9]，[^10]，[^11]，[^12]。LIVO将密集的视觉信息与激光雷达精确的几何测量相结合，能够在纹理较少、光照复杂或几何特征复杂的环境中实现稳健的姿态估计。在视觉方面，激光雷达提供了可靠的度量信息，简化了VIO系统的初始化过程；在激光雷达方面，视觉测量则提供了颜色和纹理信息，能够支持物体级场景理解等更高层次的感知任务 [^13]。这些互补的模式使得LIVO系统能够在各种场景下保持精度和鲁棒性，并为自主导航 [^10] 以及基于高斯泼溅技术的高保真环境重建 [^14] 等下游应用提供支持。 从结构上看，LIVO系统通常采用并行或顺序融合策略。并行融合将激光雷达和视觉分支作为两个独立的里程计线程，仅在后端的因子图优化层才将它们耦合在一起 [^12], [^15], [^16]。由于这些子系统在联合优化之前本来就是独立运行的，因此视觉分支容易受到尺度模糊的影响。结果，在复杂场景下视觉跟踪比激光雷达分支更容易出现性能下降，进而导致系统级的几何不一致以及估计结果的发散。LIVER [^15] 通过三阶段初始化来提升鲁棒性，但该流程相对复杂，也凸显了并行融合中保持一致性的固有难度。相比之下，顺序融合利用LIO的状态估计结果来构建视觉测量值，从而避免了视觉子系统的严苛跟踪要求对整体稳定性造成的负面影响 [^10]。 在LIVO框架中，视觉测量大致可分为三类：基于特征的方法 [^12], [^16], [^17]、直接法 [^10], [^11] 以及混合方法 [^9], [^18]。直接法利用光度测量来实现快速对齐，但高度依赖图像梯度和光度一致性，因此在复杂光照场景下很容易出现优化发散现象。 --- ![Image](images/eba69053154abe98e404080329bd7def3bfca5ae3e5861587d37a3741c6fc374.jpg)
Fig. 1. Overview of the proposed UAF-LIVO system. (a) A reference frame is selected through a score-based strategy and is matched to the current visual frame. (b) For each visual feature, its corresponding hash bucket is queried and a local plane is fitted using neighboring LiDAR points. (c) Along the feature's viewing ray, an intersection check with the plane parameterized voxel map is performed; features that fail to intersect any historical plane are classified as TYPE 3 point. (d) A new reference frame is selected through a perspective-difference check to maintain geometric consistency. (e) The corresponding LiDAR scan is projected onto the visual image plane, and the hash buckets are constructed accordingly.
与之相反，基于特征的方法会建立2D-3D几何对应关系，因此很大程度上不依赖于光度假设。这类方法的有效性取决于前端特征匹配器的能力，这使得我们可以针对特定条件选用专门的匹配器，例如用于高效特征匹配的XFeat [^19]，或是用于在运动模糊环境下实现稳健匹配的Motion-Feat [^20]。此外，基于特征的测量还支持显式的异常值剔除，从而能在融合前将动态场景或杂乱场景中常见的无效约束排除掉。 尽管顺序特征型LIVO流程具有诸多优势，但由于跨模态几何不一致性问题，视觉状态的管理依然颇具挑战。我们将跨模态几何不一致性定义为激光雷达先验信息与视觉观测之间的结构差异，其成因包括：(i)物理校准误差、(ii)复杂的环境结构以及(iii)数据关联误差（如动态物体）。传统流程往往假定各模态已完美校准且固定不变，依赖严格的几何投影假设。这种强假设在这些场景下极易导致估计结果发散。为解决这一问题，UAF-LIVO根据对应激光雷达先验的拓扑可靠性，将视觉特征划分为三种空间类别。通过为每一类别推导详尽的不确定性传播模型，该系统能够实现自适应的测量管理：对条件不佳的观测值通过增大协方差来降低其权重，同时明确剔除无效的深度关联，从而确保系统能抵御几何不一致性带来的干扰。 综上所述，我们的贡献主要有三点： - 我们提出了一种基于特征的顺序型LIVO框架，能够应对由物理校准误差和数据关联误差引发的跨模态几何不一致问题，从而实现鲁棒的位姿估计。 - 我们提出了一种几何一致的视觉测量策略，该策略能将不确定性从激光雷达显式地传递到视觉特征中，从而能够自适应地降低不可靠观测值的权重并将其剔除。 - 在多种数据集上的实验表明，我们的系统在精度上具有竞争力，且面对跨模态几何不一致问题时能保持稳定的鲁棒性。 ### 二、方法论 UAF-LIVO的整体架构如图1所示。在本节中，我们首先介绍作为几何先验的LIO子系统，随后详细阐述我们的核心贡献：几何一致的视觉测量构建与更新策略。接着，通过参考帧管理策略对视觉帧进行处理，以确保优化过程的条件良好。 ### A. 符号说明与预备知识 符号说明：标量、向量和矩阵分别用斜体$(s)$、粗体小写字母$(v)$和粗体大写字母$(M)$表示。我们将激光雷达、惯性测量单元、视觉和全局坐标系分别定义为L、I、V和G。左上标表示某个量所表达的参考坐标系$(e.g., ^{G} p)$；为简洁起见，省略的上标默认指G。视觉帧分别用cur、ref和hist表示当前帧、参考帧和历史帧。最后，$[\cdot]_{\times}$表示斜对称矩阵运算。 光线-平面交点：特征点的深度是通过将其背投影的视线和由法向量n及参考点$\mathbf{p}_0$定义的全局平面相交得到的。交点深度$t_{\text{hit}}$的计算公式为： $$ t_{\mathrm{h i t}}(\mathbf{n},\mathbf{p}_{0})=\frac{\mathbf{n}^{\top}(\mathbf{p}_{0}-\mathbf{p}_{\mathrm{r e f}})}{\mathbf{n}^{\top}\mathbf{r}} $$ 其中，相机光心$p_{ref}$和全局光线方向r是根据参考位姿$(\mathbf{R}_{ref}, \mathbf{t}_{ref})$和内参K得出的： $$ \mathbf{p}_{\mathrm{r e f}}=-\mathbf{R}_{\mathrm{r e f}}^{\top}\mathbf{t}_{\mathrm{r e f}},\quad\mathbf{r}=\mathbf{R}_{\mathrm{r e f}}^{\top}\mathbf{\Sigma}^{V}\mathbf{r},\quad^{V}\mathbf{r}=\frac{^{V}\mathbf{r}_{u}}{\left\|^{V}\mathbf{r}_{u}\right\|} $$ --- 此处未归一化的光线为$^V\mathbf{r}_u = \mathbf{K}^{-1}[u, v, 1]^\top$。最终，三维点在全局坐标系中的坐标可表示为$\hat{\mathbf{p}}(t_{\mathrm{hit}}) = \mathbf{p}_{\mathrm{ref}} + t_{\mathrm{hit}}\mathbf{r}$。 ### B. LIO子系统 LIO子系统作为几何骨干，为视觉测量更新提供精确的状态先验。该过程包含三个关键步骤： 惯性测量单元状态传播：给定原始的角速度和线性加速度数据，采用离散时间零阶保持积分法来传播惯性测量单元的状态。这种高频传播能够预测传感器的运动情况，这对于补偿激光雷达扫描中的运动误差至关重要。 激光雷达去畸变：由于激光雷达扫描是连续进行的，原始点云会存在运动畸变。为确保与视觉子系统严格的时间对齐，需根据视觉帧间隔对原始激光雷达数据流重新分段。随后，利用基于惯性测量单元的位姿插值技术，将每个点去畸变到视觉帧对应的时间戳，具体策略参见[^10]、[^21]。 点到平面残差：对去畸变后的点云进行下采样，并与局部地图进行配准以构建点到平面残差[^22]。这些残差会在迭代误差状态卡尔曼滤波器（IESKF）中被最小化，从而更新惯性测量单元机体坐标系下的状态向量，为视觉测量构建提供几何一致的先验。 ### C. 视觉测量构建与更新 在这一部分，我们提出了一种考虑不确定性的视觉测量策略，该策略显式地对噪声进行建模，以确保深度的可靠获取并检测几何不一致性问题。 1) 类型1：基于LiDAR扫描的局部平面支持特性：由于与视觉帧对应的LiDAR点仅包含原始测量不确定性，因此首先利用它们为视觉特征赋予深度信息。一旦选定某个帧作为参考，每个LiDAR点就会被投影到视觉帧V中： $$ \mathbf{u}=\pi(\mathbf{R}_{\mathrm{ref}}\mathbf{p}_{L}+\mathbf{t}_{\mathrm{ref}}), $$ 其中，$\pi(\cdot)$ 表示标准的针孔投影模型。 为了高效判断某个视觉特征在其邻域内是否有足够的投影LiDAR点支持，我们在图像平面上构建了一个哈希桶网格。图像被均匀划分为大小为 $s \times s$ 的桶，每个投影后的LiDAR点 $(u, v)$ 会根据以下规则被分配到相应的桶中： $$ b_{x}=\left\lfloor\frac{u}{s}\right\rfloor,\qquad b_{y}=\left\lfloor\frac{v}{s}\right\rfloor. $$ 随后，该点会被插入到对应的哈希表条目中： $$ \mathcal{H}(b_{x},b_{y})\;\leftarrow\;\mathcal{H}(b_{x},b_{y})\cup\{(u,v)\}. $$ 在我们的实现中，将s设为8像素。 为确保几何一致性并抑制图像平面上突然的方向性深度变化（例如在物体边界处），我们要求查询像素周围的所有 $3 \times 3$ 个相邻桶中至少包含一个投影LiDAR点。 所有投影到该邻域内的LiDAR点会被收集起来，它们的全局3D坐标和协方差分别表示为 $\{G_{P_L}^j, G_{\sum_L^j}\}_{j=1}^N$。接着通过计算质心和协方差来拟合一个局部平面： $$ \bar{\mathbf{p}}=\frac{1}{N}\sum_{j=1}^{N}{}^{G}\mathbf{p}_{L}^{j},\quad\mathbf{C}=\frac{1}{N}\sum_{j=1}^{N}\bigl({}^{G}\mathbf{p}_{L}^{j}-\bar{\mathbf{p}}\bigr)\bigl({}^{G}\mathbf{p}_{L}^{j}-\bar{\mathbf{p}}\bigr)^{\top}. $$ 平面法线n被确定为与C的最小特征值相对应的特征向量。如果满足以下条件，则该邻域被视为局部平滑： $$ r_{j}\;=\;\left|\mathbf{n}^{\top}({}^{G}\mathbf{p}_{L}^{j}-\bar{\mathbf{p}})\right|<\tau_{\mathrm{p l a n e}},\qquad\forall j. $$ 在我们的实现中，将 $\tau_{plane}$ 设为0.05。 对于属于平滑邻域的特征，其深度是通过将背投影射线与拟合平面相交得到的，使用射线-平面相交公式 $t_{\text{hit}}(\mathbf{n}, \mathbf{p}_0)$，从而恢复出对应的3D点 $\hat{\mathbf{p}}(t_{\text{hit}})$。 由于此处使用的点都位于同一拟合平面上，因此估计点 $\hat{\mathbf{p}}(t_{\mathrm{hit}})$ 可以近似为邻近LiDAR点的线性组合。于是，不确定性按如下方式传播： $$ \mathbf{\Sigma}_{\mathrm{g e o}}=\sum_{j=1}^{N}w_{j}^{2}\mathbf{\Sigma}^{G}\mathbf{\Sigma}_{L}^{j}, $$ 其中，权重 $w_{j}$ 是通过求解超定系统得到的： $$ \mathbf{w}=\arg\min_{\mathbf{w}}\left\|{}^{G}\hat{\mathbf{p}}-\sum_{j=1}^{N}w_{j}{}^{G}\mathbf{p}_{L}^{j}\right\|^{2}, $$ 采用最小二乘法求解，而 $\mathbf{w} = [w_1, w_2, \ldots w_N]^\top$ 表示线性组合的系数。 2) 类型2：基于体素图的支持特性：对于因LiDAR点稀疏而缺乏局部支持的特征，我们利用带有累积时间几何不确定性的参数化平面体素图 [^23] 来进一步构建视觉约束。 我们将特征射线进行背投影，并利用3D DDA算法 [^24] 高效地遍历该地图。一旦与最近的有效平面体素 $(n, p_0)$ 相交，便可计算出深度 $t_{\text{hit}}(n, p_0)$，进而得到全局点 $\hat{\mathbf{p}}(t_{\text{hit}})$。 交点处的不确定性来源于四个方面：(i) 相机旋转，(ii) 相机平移，(iii) 平面法线，以及 (iv) 平面上的点，这些均遵循高斯分布： $$ \begin{array}{r l r}{\mathbf{R}_{\mathrm{r e f}}\sim\mathcal{N}(\hat{\mathbf{R}}_{\mathrm{r e f}},\mathbf{\Sigma}_{\mathbf{R}}),}&{\quad}&{\mathbf{t}_{\mathrm{r e f}}\sim\mathcal{N}(\hat{\mathbf{t}}_{\mathrm{r e f}},\mathbf{\Sigma}_{\mathbf{t}}),}\\ {\mathbf{n}\sim\mathcal{N}(\hat{\mathbf{n}},\mathbf{\Sigma}_{\mathbf{n}}),}&{}&{\mathbf{p}_{0}\sim\mathcal{N}(\hat{\mathbf{p}}_{0},\mathbf{\Sigma}_{\mathbf{p}_{0}}).}\end{array} $$ 其中，$\hat{*}$ 表示估计的状态值，而 $\Sigma_{*}$ 则是对应的协方差矩阵。 $t_{hit}$ 的不确定性可以通过一阶不确定性传播来计算： $$ \begin{align*}\sigma_{t}^{2}=&\mathbf{J}_{\mathbf{p}_{0}}\boldsymbol{\Sigma}_{\mathbf{p}_{0}}\mathbf{J}_{\mathbf{p}_{0}}^{\top}+\mathbf{J}_{\mathbf{n}}\boldsymbol{\Sigma}_{\mathbf{n}}\mathbf{J}_{\mathbf{n}}^{\top}+\mathbf{J}_{\mathbf{t}_{\mathrm{ref}}}\boldsymbol{\Sigma}_{\mathbf{t}}\mathbf{J}_{\mathbf{t}_{\mathrm{ref}}}^{\top}\\&+\mathbf{J}_{\mathbf{R}_{\mathrm{ref}}}\boldsymbol{\Sigma}_{\mathbf{R}}\mathbf{J}_{\mathbf{R}_{\mathrm{ref}}}^{\top},\end{align*} $$ --- 其中，$t_{hit}$ 关于 $p_{0}$ 的雅可比矩阵可通过以下方式计算： $$ \mathbf{J}_{\mathbf{p}_{0}}=\frac{\partial t_{\mathrm{h i t}}}{\partial\mathbf{p}_{0}}=\frac{\hat{\mathbf{n}}^{\top}}{\hat{\mathbf{n}}^{\top}\mathbf{r}}, $$ 以及$t_{hit}$关于n的雅可比矩阵可以通过以下方式计算： $$ \begin{aligned}\mathbf{J}_{\mathbf{n}}=\frac{\partial t_{\mathrm{hit}}}{\partial\mathbf{n}}&=\frac{\left(\hat{\mathbf{p}}_{0}-\hat{\mathbf{p}}_{\mathrm{ref}}\right)^{\top}}{\hat{\mathbf{n}}^{\top}\mathbf{r}}\\&\quad-\frac{\left(\hat{\mathbf{n}}^{\top}(\hat{\mathbf{p}}_{0}-\hat{\mathbf{p}}_{\mathrm{ref}})\right)\mathbf{r}^{\top}}{(\hat{\mathbf{n}}^{\top}\mathbf{r})^{2}}.\end{aligned} $$ 其中$\hat{\mathbf{p}}_{\mathrm{ref}} = -\hat{\mathbf{R}}_{\mathrm{ref}}^{\top}\hat{\mathbf{t}}_{\mathrm{ref}}$。 对于运动部分，$t_{hit}$关于状态平移量$t_{ref}$的雅可比矩阵为： $$ \mathbf{J}_{\mathbf{t}_{\mathrm{r e f}}}=\frac{\partial t_{\mathrm{h i t}}}{\partial\mathbf{t}_{\mathrm{r e f}}}=\frac{(\hat{\mathbf{R}}_{\mathrm{r e f}}\hat{\mathbf{n}})^{\top}}{\hat{\mathbf{n}}^{\top}\mathbf{r}}, $$ 而$t_{hit}$关于$\mathbf{R}_{ref}$的雅可比矩阵为： $$ \begin{aligned}\mathbf{J}_{\mathbf{R}_{\mathrm{r e f}}}&=\frac{(\hat{\mathbf{p}}_{\mathrm{r e f}}\times\hat{\mathbf{n}})^{\top}}{\hat{\mathbf{n}}^{\top}\mathbf{r}}\\&\quad+\frac{\hat{\mathbf{n}}^{\top}(\hat{\mathbf{p}}_{0}-\hat{\mathbf{p}}_{\mathrm{r e f}})}{(\hat{\mathbf{n}}^{\top}\mathbf{r})^{2}}(\mathbf{r}\times\hat{\mathbf{n}})^{\top}.\end{aligned} $$ 最后，TYPE 2类型的3D点p的不确定性通过以下方式获得： $$ \mathbf{\Sigma}_{\mathrm{g e o}}=\sigma_{t}^{2}\mathbf{r}\mathbf{r}^{\top}. $$ 3) TYPE 3：仅方位特征：对于无法得到射线与平面交点的特征点（例如激光雷达未观测到的区域），我们采用仅方位约束[^25]来限制旋转，防止估计器因深度关联稀疏而出现条件数过大的问题。 我们将像素平面噪声建模为： $$ \mathbf{\Sigma _{pix}}=\sigma^{2}_{pix} \mathbf{I}_{2},\quad\mathbf{\sigma _{pix}}=\frac{k}{score_{i}}, $$ $$ I_{2} $$ $$ \mathbf{J}_{V_{\mathbf{r}_{u},\mathbf{u}}}=\begin{bmatrix}1/f_{x}&0\\ 0&1/f_{y}\\ 0&0\end{bmatrix}. $$ 经过归一化并转换到全局坐标系后，射线方向的不确定性是通过将$\Sigma_{\mathrm{pix}}$传递通过该映射关系来计算的： $$ \begin{array}{r}{\mathbf{\Sigma}_{\mathrm{g e o}}=\mathbf{J}_{\mathbf{r},\mathbf{u}}\mathbf{\Sigma}_{\mathrm{p i x}}\mathbf{J}_{\mathbf{r},\mathbf{u}}^{\top}+\mathbf{J}_{\mathbf{r},\mathbf{R}_{\mathrm{r e f}}}\mathbf{\Sigma}_{\mathbf{R}}\mathbf{J}_{\mathbf{r},\mathbf{R}_{\mathrm{r e f}}}^{\top},}\end{array} $$ 其中 $$ \mathbf{J}_{\mathbf{r},\mathbf{u}}=\mathbf{R}_{\mathrm{r e f}}^{\top}\left\|\mathbf{\Sigma}^{V}\mathbf{r}_{u}\right\|^{-1}\left(\mathbf{I}_{3}-\mathbf{\Sigma}^{V}\mathbf{r}^{V}\mathbf{r}^{\top}\right)\mathbf{J}_{V_{\mathbf{r}_{u},\mathbf{u}}},\quad\mathbf{J}_{\mathbf{r},\mathbf{R}_{\mathrm{r e f}}}=\left[\mathbf{r}\right]_{\times}. $$ 4) 视觉测量更新：我们采用IESKF来估计误差状态${}^{I}\delta\mathbf{x}$。关键在于，传递后的几何不确定性$\Sigma_{\mathrm{geo}}$会被明确纳入测量协方差$\mathbf{R}_{k}$中： $$ \mathbf{R}_{k}=\mathbf{J}_{\mathrm{g e o}}\mathbf{\Sigma}_{\mathrm{g e o}}\mathbf{J}_{\mathrm{g e o}}^{\top}+\sigma_{\mathrm{p i x}}^{2}\mathbf{I}, $$ 其中$\sigma_{\mathrm{pix}}^{2} \mathbf{I}$是当前帧的像素测量噪声。对于TYPE 1/2类型的特征，其几何基元为全局3D点$\mathbf{p}$。雅可比矩阵$\mathbf{J}_{\mathrm{geo}}$是依据链式法则，相对于视觉坐标系下的坐标$\mathbf{V} \mathbf{p} = [X, Y, Z]^{\top}$推导得出的： $$ \mathbf{J}_{\mathrm{g e o}}^{\mathrm{p}}=\frac{\partial\mathbf{u}}{\partial^{V}\mathbf{p}}\frac{\partial^{V}\mathbf{p}}{\partial\mathbf{p}}=\begin{bmatrix}\frac{f_{x}}{Z}&0&-\frac{f_{x}X}{Z^{2}}\\ 0&\frac{f_{y}}{Z}&-\frac{f_{y}Y}{Z^{2}}\end{bmatrix}\mathbf{R}_{\mathrm{c u r}}. $$ 对于TYPE 3类型的特征，其几何基元为全局方位向量$\mathbf{r}$。雅可比矩阵是相对于视觉坐标系$\mathbf{ray}^{V}\mathbf{r} = [x, y, z]^{\top}$推导得出的： $$ \mathbf{J}_{\mathrm{g e o}}^{r}=\frac{\partial\mathbf{u}}{\partial^{V}\mathbf{r}}\frac{\partial^{V}\mathbf{r}}{\partial\mathbf{r}}=\begin{bmatrix}\frac{f_{x}}{z}&0&-\frac{f_{x}x}{z^{2}}\\ 0&\frac{f_{y}}{z}&-\frac{f_{y}y}{z^{2}}\end{bmatrix}\mathbf{R}_{\mathrm{c u r}}. $$ 因此，不确定性较大的约束会产生较大的$\mathbf{R}_k$，从而在IESKF更新中自动降低它们的影响力： $$ \mathbf{\Sigma}^{I}\delta\mathbf{x}=(\mathbf{H}^{\top}\mathbf{R}_{k}^{-1}\mathbf{H}+\mathbf{\Sigma}_{\mathrm{p r i o r}}^{-1})^{-1}\mathbf{H}^{\top}\mathbf{R}_{k}^{-1}\delta\mathbf{z}, $$ 其中$\Sigma_{prior}$是先验状态协方差，$\delta\mathbf{z}$是对应的视觉残差。测量雅可比矩阵$\mathbf{H} = \frac{\partial \mathbf{u}}{\partial V_p} \frac{\partial^V \mathbf{p}}{\partial t \delta x}$通过相机-惯性测量单元的外参关系，将相机投影模型与惯性测量单元的误差状态显式地串联起来。 5) 不可靠视觉测量的剔除：所有类型的视觉测量都基于马氏距离，利用卡方检验来进行剔除： $$ \begin{array}{r}{r_{M}^{2}=\delta\mathbf{z}^{\top}\mathbf{R}_{k}^{-1}\delta\mathbf{z}>\chi_{\mathrm{t h}}^{2},}\end{array} $$ 其中$R_k$是在公式(20)中推导出的考虑不确定性的协方差。结合公式(24)，视觉测量会根据其几何置信度被评判。在我们的实现中，阈值设定为9。 备注（解耦协方差的物理意义）：由于投影雅可比矩阵$\frac{\partial u}{\partial V_p}$与深度Z成反比，采用固定协方差策略可能会使近场观测在$\Lambda = H^\top R_k^{-1}H$中产生不成比例的大影响，尤其是在跨模态对齐偏差或深度关联不可靠的情况下。UAF-LIVO将2D跟踪噪声与3D结构不确定性$\Sigma_{\text{geo}}$解耦：TYPE 1特征继承有界的局部激光雷达不确定性，而TYPE 2特征则包含依赖于深度、姿态和平面的射线与平面交点不确定性。通过将$\Sigma_{\text{geo}}$投影到图像平面上，$\mathbf{J}_{\text{geo}}\Sigma_{\text{geo}}J_{\text{geo}}^\top$会增加几何不确定性较高或对投影敏感的约束的协方差。这缓解了视觉信息矩阵$\Lambda$中由深度引起的不平衡问题，从而带来更均衡的视觉信息贡献。 ### D. 视觉关键帧管理 为了确保优化过程具有良好的条件，我们采用了一种由历史帧维护和最优参考帧选择组成的帧管理策略。 --- 1) 历史帧维护：我们维护一个固定长度的历史关键帧滑动窗口以支持视觉跟踪。为确保几何多样性并避免悬停运动带来的冗余，只有当新帧相对于最新关键帧表现出足够的运动时，才会将其插入队列中。 历史帧之间的相对旋转 $R_{rel}$ 和全局平移基线 $t_{rel}$ 计算如下： $$ \mathbf{R}_{\mathrm{r e l}}=\mathbf{R}_{\mathrm{c u r}}\mathbf{R}_{\mathrm{h i s t}}^{\top},\qquad\mathbf{t}_{\mathrm{r e l}}=\mathbf{t}_{\mathrm{c u r}}-\mathbf{t}_{\mathrm{h i s t}}. $$ 如果任何相对运动超过预设阈值，该帧就会被接受为新的关键帧： $$ \angle(\mathbf{R}_{\mathrm{r e l}})>\theta_{\mathrm{t h}}\mathrm{o r}\left\|\mathbf{t}_{\mathrm{r e l}}\right\|>t_{\mathrm{t h}}. $$ 在实际应用中，我们将 $\theta_{\mathrm{th}} = 10^{\circ}$ 和 $t_{\mathrm{th}} = 1 \, \mathrm{m}$ 设为相应数值。一旦队列达到最大容量 $N_{\max}$，最旧的帧就会被丢弃。新关键帧被接受后，会立即进行激光雷达投影处理，从而为后续的跟踪建立局部几何支撑。在实现中，我们将 $N_{\max} = 3$ 设为相应数值。 2) 参考帧选择：在每次测量更新步骤中，我们会从历史队列中选择最优的参考帧，以便与当前帧形成约束。选择过程旨在最大化兼顾视差、重叠度和姿态不确定性因素的视角效用得分 S。 给定当前光学中心和候选历史光学中心，我们定义基线向量 $\mathbf{b} = \mathbf{t}_{\text{cur}} - \mathbf{t}_{\text{ref}}$。得分计算公式如下： $$ S=\frac{\|\mathbf{b}_{\perp}\|\exp\left(-\frac{\theta^{2}}{2\sigma^{2}}\right)}{\operatorname{t r}(\mathbf{\Sigma _{R}})+\operatorname{tr}(\mathbf{\Sigma _{t}})}, $$ 其中 $\mathbf{b}_{\perp} = \mathbf{b} - (\mathbf{v}_{\mathrm{ref}}^{\top}\mathbf{b})\mathbf{v}_{\mathrm{ref}}$ 表示垂直于参考观测方向 $\mathbf{v}_{\mathrm{ref}}$ 的基线分量，而 $\theta$ 是相对旋转角度。该评分函数明确倾向于那些具有较大有效视差的帧，以实现良好的优化条件；同时通过指数项惩罚过大的视点差异，并通过分母项考虑高姿态不确定性带来的影响。 ### III. 实验 本节对所提出的 UAF-LIVO 框架进行了全面评估。我们首先验证了该框架中基于不确定性的视觉构建方法的准确性，并通过受控压力测试明确展示了其对抗跨模态几何不一致性的能力。随后，我们在多种具有挑战性的环境中将整个系统与最先进的框架进行对比，最后还进行了详细的实时效率分析。 ### A. 实现细节 我们在 M2DGR [^26]、M3DGR [^27] 和 GEODE [^28] 数据集上评估了我们的方法。M2DGR 提供带有 Velodyne 旋转式激光雷达的 RGB 和热成像数据，而 M3DGR 则提供由 Livox Avia 采集的 RGB 数据与激光雷达测量结果。GEODE 的地铁隧道序列用于评估该框架对抗跨模态几何不一致性的能力。 ![Image](images/db518f9327ffb0ee8e2eb6f533f16fea611fdf13e0abaf2b998b3618264a9042.jpg)
(a)
Absolute Depth Discrepancy Distribution ( $log-x$)
![Image](images/d1990786cd28a050c4ae59d772d387d29f1f9b4d93abc42b540e2adcce74b2e2.jpg)
(b)
Fig. 2. Validation of visual measurement construction. (a) Qualitative global spatial distribution of depth discrepancy. Green: depth discrepancy less than 1%, Yellow: depth discrepancy less than 3% but over 1%, Red: depth discrepancy over 3%. (b) Quantitative depth discrepancy distribution in the occlusion case.
实验在一台配备 RTX 5070 Ti GPU、i7-13700KF CPU 和 32 GB RAM 的 PC 上进行。我们的实现基于 Fast-LIVO2 的 LIO 子系统 [^10]，采用 0.5 米的体素地图、每帧 10 次地图更新迭代以及 100 米的 DDA 射线投射范围。LightGlue 通过 TensorRT FP32 加速，最多处理 128 个关键点。对于轨迹呈近似矩形闭环的 Dark01 数据集，无约束的全轨迹 SE(3) 对齐偶尔会导致配准不一致以及异常大的 ATE 值。因此，我们采用了一种受限的确定性对齐协议，包括初始帧锚定以及无尺度限制的纯旋转对齐。所有对比方法均采用相同的协议。 ### B. 视觉测量构建的验证 为了验证我们视觉测量构建的准确性，我们利用Fast-LIVO2数据集中的建筑序列[^10]来评估跨模态深度差异。该序列包含显著的平面结构，同时也存在固有的几何不一致性。如图2(a)所示，这些差异的空间分布与我们的理论分析一致：虽然墙面上的平面区域能保持较高的深度精度，但复杂纹理和遮挡边界却会出现严重的结构错位现象。 为了量化这种错位程度，我们专门分析了遮挡区域内的误差分布（见图2(b)）。得益于严格的平面有效性校验，TYPE 1特征能达到亚厘米级精度，其平均绝对误差为0.009米，均方根误差为0.014米。相比之下，TYPE 2特征则呈现出明显的长尾型误差分布。尽管大多数原始误差仍被限制在合理范围内， ---
TABLEI
ATE RMSE (M) UNDER EXTRINSIC PERTURBATION. SIGNIFICANT ATE INCREASES ARE MARKED WITH AN ASTERISK (*).
Method$\sigma_{{pix}}$ (px)Dynamic03Dynamic04
$0^{\circ}$$1^{\circ}$$3^{\circ}$$0^{\circ}$$1^{\circ}$$3^{\circ}$
FixCov50.220.230.220.380.400.35
100.230.230.51 $^{{*}}$0.380.400.38
200.230.230.230.390.75 $^{{*}}$0.53 $^{{*}}$
Ours-0.220.220.230.390.400.39
![Image](images/f055e27453f44990da67a21e838177f88d3451935780160bf0de6701d6282482.jpg) ![Image](images/0392f5a6986e8f5d6792b6b8b24e3135366d93a2862b737f7f5238ac9d32f0d2.jpg)
(a)
(b)
Fig. 3. Qualitative comparison of map consistency of Dynamic03 sequence under $3^{\circ}$ perturbation. (a) Map of our uncertainty-aware mechanism. (b) Map of fixed covariance ( $\sigma_{pix} = 10\ px$) strategy.
但其中一小部分异常值严重违背了激光雷达先验，导致TYPE 2的整体平均绝对误差升至1.365米，均方根误差更是达到5.514米。 这种性能差距凸显了我们所提出的不确定性感知公式的重要性——它能防止此类几何异常值干扰估计器。据此，我们的方法会根据估算出的不确定性，自适应地对条件不佳的TYPE 2约束进行降权处理或直接排除。 ### C. 对几何不一致性的鲁棒性分析 为了明确验证算法针对跨模态几何不一致性的鲁棒性，我们对Dynamic03和Dynamic04序列进行了压力测试，人为引入系统性的外部扰动。我们定义了两种离散的扰动等级：一种是1°的旋转误差加上每个轴0.05米的平移偏移，另一种是3°的旋转误差加上每个轴0.10米的偏移。为简洁起见，表中仅用旋转角度来指代这些耦合扰动。为了建立传统的固定协方差基准（记为FixCov），我们将视觉测量的协方差设为$\mathbf{R}_k = \sigma_{\text{pix}}^2 \mathbf{I}$，其中$\sigma_{\text{pix}}$代表统一的像素噪声容忍度。我们在三种不同的容忍度水平下评估该基准：$\sigma_{\text{pix}} \in \{5, 10, 20\}$像素。 如表I所示，采用宽松的像素噪声容忍度（$\sigma_{pix} \geq 10\ px$）会导致不同场景和扰动等级下的全局ATE增大。具体而言，在Dynamic03序列中施加$3^{\circ}$级别的扰动时，固定的$\sigma_{pix} = 10\ px$容忍度会进一步加剧局部轨迹抖动，进而造成可观测的地图畸变，如图3所示。而更严格的容忍度（$\sigma_{pix} = 5\ px$）虽能通过强制全局严格拒绝来取得相近的ATE，但由于视觉权重过高，反而会引起局部抖动，如图4所示。通过直接从LIO结构协方差中推导视觉不确定性，我们的公式缓解了这种敏感性。尽管系统的外部扰动不可避免地会扭曲测量雅可比矩阵，并导致式(23)中的海森矩阵发生偏移， ![Image](images/ccb2800586ad55424a1f9f529df63ad7e4de6b76245b72fe5a3bfb4e4d537490.jpg)
Fig. 4. Visualization of estimated trajectory of Dynamic04 sequence under $3^{\circ}$ perturbation.
但我们的方法能通过不确定性感知的排除机制以及自适应的近场降权处理，有效抑制这些结构偏差，从而减轻局部轨迹抖动现象。 ### D. M2DGR与M3DGR数据集 在本小节中，我们将定量比较所提出的方法与四种代表性框架：Fast-LIO2 [^5]、SR-LIVO [^9]、Fast-LIVO2 [^10]以及LVI-SAM [^16]。Fast-LIO2作为纯粹的LIO基线，用于评估视觉对系统精度的影响。LVI-SAM是一种基于特征的并行LIV-SLAM框架，通过因子图优化实现紧密耦合。在M3DGR数据集上的评估中，我们采用了M3DGR基准测试提供的修改版LVI-SAM实现。SR-LIVO依赖LIO进行位姿估计，并利用其视觉组件进行在线校准。Fast-LIVO2则采用顺序融合策略，并结合直接光度视觉对齐技术。 表二报告了各种场景下的整体性能。对于户外gate_01序列，LVI-SAM的ATE明显更高。这主要是由于激光雷达与视觉模态之间的不平衡：VIO子系统对短基线初始化较为敏感，从而会对联合因子图优化产生负面影响。Fast-LIVO2的密集光度对齐会加剧户外非结构化场景中的几何不一致性，导致其性能不如热成像模态。相比之下，我们所提出的不确定性感知机制能够自然地对这些条件不良的结构性不匹配进行降权处理并予以排除，从而确保RGB和热成像模态下的稳定性。对于具有规则走廊结构的室内hall序列，我们的方法取得了具有竞争力的精度，在hall_05序列中ATE低至1.16米。在小规模房间序列中，LVI-SAM的绝对精度最佳。这在理论上是预期之中的，因为LVI-SAM显式的边缘特征提取在连续平面结构严重破碎的高度杂乱环境中具有先天优势。然而，在所有无激光雷达特征的点云匹配框架中，我们的方法始终能与Fast-LIVO2和SR-LIVO相媲美甚至超越它们。此外，在无纹理的room_dk_01序列中，竞争性的LIVO框架会因视觉跟踪效果恶化而产生优化障碍，而我们的系统仍能保持稳健的性能。 ---
TABLE II
QUANTITATIVE COMPARISON AGAINST LIO AND LIVO FRAMEWORKS. ATE: ABSOLUTE TRAJECTORY ERROR [M]; RTE: RELATIVE TRANSLATION ERROR [M]. BEST RESULTS ARE BOLDED, SECOND BEST ARE $\underline{\text{UNDERLINED}}$.
SequenceFast-LIO2LVI-SAMFast-LIVO2SR-LIVOOurs
ATERTERGBRTERTERGBRTERGBRTERTERGBRTERTERGB
ATERTEATERTEATERTEATERTEATERTEATERTEATERTEATERTERTE
gate_011.530.0052.200.0091.590.0081.900.0101.560.0081.680.0081.970.0081.630.0091.570.0081.68
hall_012.220.1012.180.1002.200.1002.160.1042.240.1022.380.1022.410.1022.300.1022.230.1052.38
hall_041.690.0861.740.0861.730.0861.620.0841.730.0881.570.0851.680.0871.620.0841.730.0891.68
hall_051.280.0661.300.0671.420.0731.220.0681.300.0681.610.0721.580.0701.160.0671.300.0671.61
room_010.440.0070.270.0070.280.0070.460.0080.440.0070.640.0100.660.0100.460.0070.430.0070.64
room_dk_010.590.0080.440.0070.440.0070.690.0090.590.0080.870.0090.530.0090.580.0090.500.0090.87
Dark012.460.9136.552.005--2.430.923--2.561.052--2.420.963--2.56
Dynamic030.300.0803.050.813--0.250.081--1.280.081--0.220.080--1.28
Occlusion030.240.0828.862.866--0.290.083--✗✗--0.330.083--0.33
TABLE III
ABSOLUTE TRAJECTORY ERROR (M) ON GEODE METRO TUNNEL GAMMA SEQUENCES. DRIFT MEANS ATE > 100 M.
MethodTunnel 1Tunnel 2Tunnel 3Tunnel 4Tunnel 5
LVI-SAM [^16]35.42.100.240.350.17
RELEAD [^29]0.372.320.220.150.35
SR-LIVO [^9]✗✗✗✗✗
Fast-LIVO2 [^10]1.51drift6.693.7521.3
Ours (w/o Rej.)2.40driftdriftdriftdrift
Ours (Full)0.611.951.710.850.52
在M3DGR数据集上，Fast-LIVO2与我们提出的方法在所有序列中均能达到相近的亚米级精度。相比之下，其他基线方法则面临特定挑战。值得注意的是，LVI-SAM主要为旋转式激光雷达设计，在非重复扫描条件下边缘特征检测能力不足。这种几何稀疏性，加上无纹理的Dark01序列中视觉约束不可靠，导致其容易出现尺度漂移现象。同时，在Occlusion03场景中，由于长时间遮挡导致视觉信息过早中断，SR-LIVO无法完成轨迹估计。除此之外，LVI-SAM和SR-LIVO在动态或遮挡场景下也会出现轨迹漂移和精度下降问题，这充分体现了顺序融合策略在鲁棒性方面的优势。 ### E. GEODE隧道场景 我们在GEODE数据集上，利用Livox Avia激光雷达采集的五段地铁隧道序列，将所提出的方法与最先进的方法进行对比评估 [^28]。整体定量结果如表三所示。由于支持Avia激光雷达的LVI-SAM和RELEAD官方版本尚未开源，我们直接采用了GEODE基准测试中报告的基线结果。 在隧道场景中，由于视觉组件中的RANSAC PnP算法失效，SR-LIVO无法在这些序列上生成完整的轨迹。值得注意的是，RELEAD在这五个序列中取得了最佳的整体性能，这得益于其通过Hessian矩阵病态性分析和分级非凸优化来检测激光雷达退化情况的能力。LVI-SAM也凭借识别结构性或无纹理退化区域的能力而取得了具有竞争力的性能；它能够选择性地抑制 ![Image](images/bd311643914f076ab3f2090d0a76ecb23409f69d304310bb1275537e7ff8b231.jpg) ![Image](images/17a4875c04ddd3d47dc773905ad7b3ec4650881e898ed5952231db160370a529.jpg) ![Image](images/9de347129b1189b2a5ec91c3ce0761bdbf300aa1cdad0e5c6ac492fa92a63849.jpg)
(c)
![Image](images/74927438ec0a8543d0a2ae94a2b64aa28c782d7b681530882533380976e147a1.jpg)
(d)
Fig. 5. Visualization of estimated trajectory in GEODE Metro Tunneling Sequence. First Row: GEODE Metro Tunnel 2 sequence. Second Row: GEODE Metro Tunnel 4 sequence.
优化窗口中不可靠的更新，具体方法是监测Hessian特征值并追踪数据质量，从而维持状态估计的稳定性。由于我们的LIO子系统本身缺乏显式的激光雷达退化检测与抑制机制，它在这些激光雷达退化的隧道中的整体性能略逊于LVI-SAM和RELEAD。不过，通过严格丢弃不可靠的视觉约束，我们的方法在隧道1和隧道2中仍能达到具有竞争力的精度。 尽管Fast-LIVO2采用了类似的序列更新架构，但它在所评估的五个序列上的跟踪误差更大，主要原因在于它对跨模态不一致性较为敏感。在GEODE地铁隧道场景中，激光雷达模态在狭窄通道内旋转时常常出现退化现象，而极低的光线条件进一步削弱了视觉数据的可靠性。Fast-LIVO2往往会在没有显式拒绝机制的情况下，将这些退化的视觉测量值纳入状态估计中。具体而言，在隧道2序列中，较低的旋转速度延长了激光雷达的退化时间，导致不可靠的视觉约束干扰优化过程并引发全局漂移，如图5(a)所示。在我们的未采用拒绝机制的消融模型中也能观察到类似的故障现象（w/o Rej.），由于错误的基于特征的2D–3D约束，模型在更多序列中出现了漂移情况。 ---
TABLE IV
RONTIME COMPARISON (MS): LiDAR AND VISUAL BRANCHES PERFORMANCE. VLP-32: MECHANICAL; AVIA: NON-REPETITIVE. BEST: BOLDED.
MethodVLP-32Avia
LIOVOLIOVO
LVI-SAM82.7211.8621.8612.57
Fast-LIVO247.777.907.772.92
SR-LIVO14.7811.434.746.64
Ours53.9612.107.908.40
这类约束比光度残差施加了更直接的几何限制。这证实了激光雷达退化会损害基准精度，而未处理的跨模态不一致性会进一步加剧这种退化，甚至可能导致发散。相比之下，我们的框架会显式丢弃病态测量值以维持估计的一致性。此外，隧道4的情况表明，激光雷达与视觉模态之间未处理的空间不一致性会引发明显的轨迹偏移（图5(c)），而我们的方法通过一致性验证避免了这一问题。 ### F. 时间效率分析 表四详细列出了gate_01和Dynamic03序列上的运行时间性能。虽然与我们不确定性感知融合机制相比，SR-LIVO或Fast-LIVO2的开销更小，但我们的系统明显快于LVI-SAM的因子图优化过程。我们的系统保持了很高的实时效率，LIO子系统的耗时仅为53.96/7.90毫秒，视觉组件耗时仅为12.10/8.40毫秒。值得注意的是，该框架主要在CPU上运行，仅将GPU加速用于视觉特征匹配。 ### IV. 结论 这封信介绍了UAF-LIVO，这是一种用于解决跨模态几何不一致问题的序列框架，它通过全面的不确定性传播和显式的异常值剔除来实现这一目标。基于激光雷达的空间关系，我们将视觉特征分为三种与几何相关的类型，进而推导出测量协方差，以便自适应地降低条件不良观测值的权重。实验表明，在严重退化的环境中，该方法具有具有竞争力的精度和稳定的鲁棒性。尽管我们的系统在应对跨模态不匹配问题时表现出显著的韧性，但在激光雷达模态严重退化的情况下，仍可能会出现轨迹漂移现象。未来的工作将致力于减轻对激光雷达几何先验的依赖，从而在激光雷达测量不可靠时仍能保持稳健的估计。 ## 参考文献 [^1]: T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018. [^2]: L. V. Stumberg and D. Cremers, “DM-VIO: Delayed marginalization visual-inertial odometry,” IEEE Robot. Automat. Lett., vol. 7, no. 2, pp. 1408–1415, Apr. 2022. [^3]: J. Kaiser, A. Martinelli, F. Fontana, and D. Scaramuzza, “Simultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation,” IEEE Robot. Automat. Lett., vol. 2, no. 1, pp. 18–25, Jan. 2017. [^4]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIO-SAM: Tightly-coupled LiDAR inertial odometry via smoothing and mapping," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2020, pp. 5135–5142. [^5]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, "FAST-LIO2: Fast direct LiDAR-inertial odometry," IEEE Trans. Robot., vol. 38, no. 4, pp. 2053–2073, 2022. [^6]: D. He et al., “Point-LIO: Robust high-bandwidth light detection and ranging inertial odometry,” Adv. Intell. Syst., vol. 5, no. 7, 2023, Art. no. 2200459. [^7]: C. Zhang, J. Zhang, Q. Liu, Y. Liu, and J. Qin, "SI-LIO: High-precision tightly-coupled LiDAR-inertial odometry via single-iteration invariant extended Kalman filter," IEEE Robot. Automat. Lett., vol. 10, no. 1, pp. 564–571, Jan. 2025. [^8]: L. He, B. Li, and G. Chen, "Place-LIO: Plane-centric LiDAR-inertial odometry," IEEE Robot. Automat. Lett., vol. 10, no. 6, pp. 6231–6238, Jun. 2025. [^9]: Z. Yuan, J. Deng, R. Ming, F. Lang, and X. Yang, “SR-LIVO: LiDAR-inertial-visual odometry and mapping with sweep reconstruction,” IEEE Robot. Automat. Lett., vol. 9, no. 6, pp. 5110–5117, Jun. 2024. [^10]: C. Zheng et al., “Fast-LIVO2: Fast, direct LiDAR-inertial-visual odometry,” IEEE Trans. Robot., vol. 41, pp. 326–346, 2024. [^11]: T. Shi, K. Qian, Y. Fang, Y. Zhang, and H. Yu, “Point-line LIVO using patch-based gradient optimization for degenerate scenes,” IEEE Robot. Automat. Lett., vol. 9, no. 11, pp. 9717–9724, Nov. 2024. [^12]: J. Lin, C. Zheng, W. Xu, and F. Zhang, “R² live: A robust, real-time, lidar-inertial-visual tightly-coupled state estimator and mapping,” IEEE Robot. Automat. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021. [^13]: H. Liu et al., “A real-time LiDAR-visual-inertial object level semantic slam for forest environments,” ISPRS J. Photogrammetry Remote Sens., vol. 219, pp. 71–90, 2025. [^14]: S. Hong et al., “GS-LIVO: Real-time LiDAR, inertial, and visual multi-sensor fused odometry with Gaussian mapping,” IEEE Trans. Robot., vol. 41, pp. 4253–4268, 2025. [^15]: T. Wen, Y. Fang, B. Lu, X. Zhang, and C. Tang, "LIVER: A tightly coupled LiDAR-inertial-visual state estimator with high robustness for underground environments," IEEE Robot. Automat. Lett., vol. 9, no. 3, pp. 2399–2406, Mar. 2024. [^16]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping," in Proc. IEEE Int. Conf. Robot. Automat., pp. 5692–5698. [^17]: S. Zhou et al., “LIR-LIVO: A lightweight, robust LiDAR/vision/inertial odometry with illumination-resilient deep features,” 2025, arXiv:2502.08676. [^18]: J. Lin and F. Zhang, “R3LIVE: A robust, real-time, RGB-colored, LiDAR-inertial-visual tightly-coupled state estimation and mapping package,” in Proc. IEEE Int. Conf. Robot. Automat., 2022, pp. 10672–10678. [^19]: G. Potje, F. Cadar, A. Araujo, R. Martins, and E. R. Nascimento, “XFeat: Accelerated features for lightweight image matching,” in Proc. IEEE/CVF Conf. Comput. Vis. Pattern Recognit., 2024, pp. 1–10. [^20]: Y. Gao, D. Zhang, Y. Yu, J. Zhang, H. Li, and J. Chen, “Motion-feat: Motion blur-aware local feature description for image matching,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2025, pp. 2448–2454. [^21]: Z. Yuan, Q. Wang, K. Cheng, T. Hao, and X. Yang, “SDV-LOAM: Semi-direct visual–LiDAR odometry and mapping,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 45, no. 9, pp. 11203–11220, Sep. 2023. [^22]: J. Zhang and S. Singh, “LOAM: LiDAR odometry and mapping in real-time,” in Proc. Conf. Robot. Sci. Syst., Berkeley, CA, USA, 2014, pp. 1–9. [^23]: C. Yuan, W. Xu, X. Liu, L. Chen, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online LiDAR odometry,” IEEE Robot. Automat. Lett., vol. 7, no. 3, pp. 8518–8525, Jul. 2022. [^24]: J. Amanatides and A. Woo, “A fast voxel traversal algorithm for ray tracing,” Eurographics, vol. 87, no. 3, pp. 3–10, 1987. [^25]: T. Bailey, “Constrained initialisation for bearing-only SLAM,” in Proc. IEEE Int. Conf. Robot. Automat., 2003, pp. 1966–1971. [^26]: J. Yin, A. Li, T. Li, W. Yu, and D. Zo, “M2DGR: A multi-sensor and multi-scenario SLAM dataset for ground robots,” IEEE Robot. Automat. Lett., vol. 7, no. 2, pp. 2266–2273, Apr. 2022. [^27]: D. Zhang et al., “Towards robust sensor-fusion ground SLAM: A comprehensive benchmark and a resilient framework,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Hangzhou, China, 2025, pp. 8894–8901. [^28]: Z. Chen et al., “Heterogeneous LiDAR dataset for benchmarking robust localization in diverse degenerate scenarios,” Int. J. Robot. Res., vol. 45, pp. 6–22, 2026. [^29]: Z. Chen et al., “RELEAD: Resilient localization with enhanced LiDAR odometry in adverse environments,” in Proc. IEEE Int. Conf. Robot. Automat., 2024, pp. 3999–4005.