# LIVOX-CAM：面向固态激光雷达的自适应粗精视觉辅助激光雷达里程计
程小龙 $^{ID}$、耿可可 $^{ID}$、刘志超、马天晓、孙烨
摘要—固态激光雷达的应用正不断拓展至各类场景。然而，现有大多数方法都依赖惯性测量单元（IMU）数据融合来实现稳定性能。本文提出LIVOX-CAM——一种基于KISS-ICP的视觉辅助激光雷达里程计，专为小视场角（FoV）固态激光雷达量身定制。该系统采用两阶段架构：前端负责数据预处理，后端则进行粗精迭代位姿优化。通过引入空间自适应模块与视觉辅助功能，该系统能够大幅拓展其应用场景。在公开及私有数据集上的大量实验表明：即便没有IMU输入，所提方法在自动驾驶、环境恶劣场景、非结构化环境以及航空测绘等极具挑战性的场景中，仍能实现鲁棒且精准的性能表现，相较于现有最先进方法具备显著竞争力。
索引词—固态激光雷达、定位、里程计。
## 一、引言
同步定位与建图（SLAM）一直是智能体领域的热门研究课题[^1]。经过数十年的发展，现已涌现出多种基于可见光相机[^2]、[^3]、RGB-D相机[^4]、激光雷达[^5]、[^6]以及多传感器[^7]、[^8]的SLAM系统。由于激光雷达能够生成精确的三维地图，因此它成为移动智能体的核心传感器。不过，目前大多数现有的激光雷达框架[^9]、[^10]、[^5]都是为机械旋转式激光雷达设计的，并不适用于固态或非重复扫描式激光雷达[^11]。
固态激光雷达因其轻量化设计、紧凑的外形、成本效益以及高分辨率性能而受到越来越多的关注，因此有望广泛应用于自动驾驶[^6]、空中机器人[^12]以及3D重建[^8]等领域。尽管有这些优势，但新型扫描机制以及固有的窄视场角（FoV）给激光雷达里程计带来了新的挑战。非重复扫描特性使得大多数现有的点云特征提取算法[^10]、[^13]无法适用。此外，有限的视场角会大幅降低连续扫描之间的重叠度，从而加剧里程计出现漂移和退化的风险。当前的激光雷达SLAM系统[^14]、[^15]往往涉及大量需要手动调整的参数，用户必须根据不同的环境条件进行干预。然而，在长距离、长时间的任务中，周围环境通常会发生显著变化，这给定位稳定性带来了额外挑战。当使用视场角本身就很窄的固态激光雷达时，这一问题会更加突出，因为有限的空间覆盖范围会使其对场景变化和遮挡更加敏感。大多数固态激光雷达里程计[^12]、[^21]、[^22]、[^27]都采用将惯性测量单元（IMU）数据与激光雷达数据融合的紧耦合框架。虽然这种方法有助于解决视场角窄和非重复扫描的问题，但其性能严重依赖于精确的校准和低噪声的IMU数据，而在振动、热漂移或长期使用等现实环境中，这些因素很难得到保证[^16]。据此，本研究旨在开发一种不依赖IMU数据的固态激光雷达里程计方法。固态激光雷达带来的挑战包括：(a)在非重复扫描模式下实现可靠的点云特征提取；(b)窄视场角会增加退化风险；(c)有限的视场角会导致每次扫描的空间范围出现波动。为解决上述问题，我们提出了一种名为“LIVOX-CAM”的视觉辅助激光雷达里程计方法，该方法是在KISS-ICP框架的基础上扩展而来的。该系统充分利用了固态传感器的固有特性，因此非常适合复杂多变的应用场景。
总而言之，本研究的主要贡献可概括如下：1）在KISS-ICP [^19]的基础上，我们提出了LIVOX-CAM——一种专为固态激光雷达设计的新型视觉辅助激光雷达里程计框架，旨在解决视场狭窄和点云退化等难题；2）我们设计了自适应空间模块，能够动态估算每一帧的空间因子，从而使系统能够应对复杂的场景变化；3）我们将视觉特征引入里程计流程，并提出了自适应双重迭代最近点算法（AD-ICP）——这是一种由粗到精的配准策略，能够利用几何与视觉信息，在激光雷达数据质量较差的情况下提升位姿估计精度；4）我们在多个具有挑战性的数据集上开展了大量实验，验证了该系统的实时性能。
---
Fig. 1. Some colored map results constructed by LIVOX-CAM. (a) The underground garage scene. (b) The high-altitude mapping scene. (c) The natural garden scene.
以及LIVOX-CAM的鲁棒性。如图1所示，所得到的细粒度纹理对齐结果体现了该系统所达到的高定位精度。
### II. 相关工作
### A. 机械式激光雷达里程计
早期的激光雷达里程计系统主要是为机械式激光雷达传感器开发的，这类传感器依靠旋转的激光发射器来采集周围环境的密集三维点云。因此，许多现有方法都与机械式激光雷达的扫描特性紧密相关。广为人知的LOAM算法[^17]就包含一种针对机械式激光雷达的点云特征提取方法。该方法通过计算点云中每个点与相邻光束点之间的平滑度，来提取表面点和角点。在此基础上，LeGO-LOAM[^10]通过计算相邻扫描线之间的高度差来实现地面点分割，并引入地面优化以提高建图精度。为了增强在多种环境下的鲁棒性，Pan等人[^15]提出了一种多尺度、多类别的激光雷达里程计系统，该系统利用大量参数和特征类型，能够在室内外环境中稳定运行。为了追求更高的计算效率，F-LOAM[^18]在非线性优化过程中引入了特征加权策略，根据点的平滑度分配权重，从而加快位姿估计的收敛速度。
近年来，激光雷达里程计的研究趋势逐渐从特征提取转向对原始点云的直接处理。与基于特征的方法相比，基于原始点的方法在不同环境和激光雷达模态下表现出更强的适应性。Dellenbach等人[^9]提出了CT-ICP，这是一种结合稀疏体素化表示的连续时间框架，能够实现鲁棒且实时的位姿估计。随后，Vizzo等人[^19]提出了KISS-ICP，进一步探索了激光雷达里程计的应用极限。与其他需要数十个参数的方法相比，KISS-ICP仅使用7个参数。KISS-ICP在无人机、地面机器人和自动驾驶车辆上均表现出较强的泛化能力，也是少数明确支持固态激光雷达的方法之一。不过，其在运行过程中依赖固定的参数设置，这限制了它在动态变化环境中的适应性。这一现状凸显出开发能够实时适应场景变化的里程计系统的必要性，尤其是在固态激光雷达的约束条件下。
### B. 固态激光雷达里程计
大多数特征提取方法都是针对机械式激光雷达的扫描模式设计的，因此不太适用于通常具有窄视野和非重复采样特性的固态激光雷达。此外，手动定义的特征类型越多，激光雷达里程计系统的泛化能力往往越弱，从而限制了其在不同环境中的适用性。为解决这些挑战，Lin等人提出了Loam_livox[^11]，这是为固态激光雷达适配的LOAM变体。该方法会过滤出高质量的点并进行迭代位姿优化。不过，其有效性仅限于低速运动场景，且需要根据环境规模和激光雷达类型手动调整参数。为了克服单激光雷达系统在高速场景下的局限性，Zhang等人提出了Traj-LO[^16]，该方法将激光雷达提取的几何特征与轨迹平滑性的运动学约束紧密结合起来。这种融合使得该系统即便没有惯性传感器，也能达到与激光雷达-惯性里程计相当的性能。
为克服固态激光雷达固有的局限性，如视场角狭窄和采样不均匀等问题，近期研究开始探索传感器融合策略以提升系统鲁棒性。新兴固态激光雷达的独特特性为里程计设计开辟了新方向。Zhu等人[^20]用Livox激光雷达替换了传统视觉SLAM框架中的深度传感器，利用其优势构建了激光雷达-视觉SLAM流程。Xu等人[^21]提出了FAST-LIO2，这是一种基于迭代卡尔曼滤波框架的紧耦合激光雷达-惯性里程计系统。该系统利用高效的增量式k-d树（ikd-Tree）将原始点云直接与全局地图对齐，从而同时实现高精度与实时性能。在此基础上，Zheng等人[^22]、[^8]将视觉-惯性里程计（VIO）系统引入FAST-LIO2框架，以在激光雷达性能下降的条件下进一步提升位姿估计的稳定性。这些紧耦合的多传感器方法虽表现优异，但会增加系统复杂度和计算负担，导致在资源受限的平台上进行实时部署颇具挑战。
借助固态激光雷达的独特特性，我们提出了一种新颖的视觉辅助激光雷达里程计框架，该框架能无缝整合激光雷达与视觉信息以提升整体系统性能。所提出的LIVOX-CAM解决了激光雷达里程计中长期存在的对环境退化敏感及适应性差等问题，同时充分利用了现代固态传感器的紧凑性与精度优势。LIVOX-CAM能够根据环境变化自适应调整系统参数，还能结合视觉特征来克服激光雷达性能退化问题。该方法基于轻量级的KISS-ICP框架$^{[19]}$，既保留了点对点配准的简单性与高效性，又无需依赖来自视觉或惯性系统的辅助子里程计。
---
Fig. 2. The system overview of LIVOX-CAM, which contains the data preprocessing (in blue), the iterative position optimization block (in yellow). The $v_j$ and $\rho_j$ are spatial factors of the current scan. (a) Extracting PL features from a gray image. (b) RGB point cloud. (c) Local map incorporating visual features.
TABLEI
ALL PARAMETERS OF OUR APPROACH
| Parameter | Value |
| Odometry | Expected number of key points $n_{key}$ | 1000 |
| Voxel size of local map $\nu_{{m}}$ | 1 m |
| Expansion factor for subsampling $\alpha$ | 0.3 |
| Maximum distance $r_{{max}}$ | 100 m |
| deviation threshold $\sigma_{{min}}$ | 0.01 m |
| ICP convergence criterion $\gamma$ | $10^{-4}$ |
| Vision | Line feature length L | 30 pixels |
| Line feature width W | 2 pixels |
| ORB feature point count $N_{{orb}}$ | 500 |
| ORB feature point radius R | 2 pixels |
### 三、所提出的系统
### A. 系统概述
图2展示了LIVOX-CAM系统的概览。该框架包含两大核心组件：1）用于数据预处理的前端；2）用于迭代位姿优化的后端。系统首先同步激光雷达与相机数据：对图像进行曝光校正并提取点线（PL）特征，同时利用恒定速度模型对激光雷达点云进行运动补偿。随后计算自适应空间因子以支持动态参数调整。提取出的视觉特征会被投影到点云上，生成彩色融合点云。接着通过两阶段下采样得到地图与关键点。最后采用所提出的自适应双ICP（AD-ICP）算法进行位姿估计，以实现稳健的由粗到细配准。系统参数汇总于表I中，各组件的详细描述将在后续章节给出。
### B. 自适应空间模块
固态激光雷达的有效扫描空间在运行过程中会不断变化，往往导致系统参数与当前场景不匹配。例如，使用固定的下采样尺寸会在狭小空间中造成采样不足，而在大规模环境中则会导致采样过度。为解决这一问题，我们提出了自适应空间模块，能够实时计算空间因子，从而实现关键参数的动态调整。这确保了在不同环境中都能获得充足的关键点密度并维持高效性能。
我们首先给出如下定义：$P_j$ 表示第 j 个帧的点云，其中包含 $n_j$ 个点；$p_i \in P_j$ 表示 $P_j$ 中的第 i 个点。为了确定适合下采样的体素大小，我们引入关键点的预期数量作为指导指标。综合考虑定位精度与计算成本，我们将该数值设定为 1000。为了提取出 $n_{key}$ 个关键点，下采样后需要保留大约 $n_{key}$ 个被占据的体素，每个体素提供一个关键点候选。每个体素中预期的平均点数如下：
$$ \rho_{\mathrm{e x p}}=n_{j}/n_{k e y}. $$
随后，点云会被划分为分辨率为 $v_{m}$ 的体素网格。若某个体素中包含超过三个点，则将其标记为被占据状态。被占据体素的总数为 $N_{j}$，由此可得到每个体素的平均点数 $\rho_{j}$。根据空间体积与关键点数量之比，便可估算出合适的下采样体素大小，其表达式如下：
$$ \frac{v_{j}{}^{3}}{v_{m}{}^{3}}=\frac{\rho_{\mathrm{e x p}}}{\rho_{j}},\quad v_{j}=\sqrt[3]{\frac{\rho_{\mathrm{e x p}}v_{m}{}^{3}}{\rho_{j}}}. $$
我们将 $v_j$ 定义为比例因子，将 $\rho_j$ 定义为分辨率因子。它们被称为空间因子，分别用于描述几何场景的比例以及点的分布情况。当前帧的空间因子会被用来动态调整系统参数。此后，与 KISS-ICP 中的做法一致，采用恒定速度模型来生成去畸变后的点云 $P_j^*$。
### C. 视觉特征提取与融合
在图像数据处理方面，我们首先应用 Drago 色调映射算法，接着进行直方图平均处理，以增强图像的动态范围与视觉质量，最终生成适用于地图可视化的高分辨率图像。为了减少特征提取过程中的计算开销，我们对图像进行下采样，从而从低分辨率图像中提取 ORB [^23] 点特征和 LSD [^24] 线特征。我们选用点-线（PL）特征，是因为它们在包括开阔场地和低纹理场景在内的多种环境中都具有广泛的适用性。视觉特征的提取由表 I 中所列的四个参数控制，这些参数共同决定了特征的数量与检测阈值。如图 2(a) 所示，提取出的灰度特征图像会与去畸变后的激光雷达点云 $P_j^*$ 相融合，从而生成图 2(b) 所示的 RGB 点云 $P_j^c$。点的特征标签存储在 RGB 通道中：线特征对应 R = 255、B = 0；点特征对应 R = 0、B = 255；普通点则对应 R = 0、B = 0。G 通道用于编码反射强度。根据激光雷达点是否由视觉特征投影而来，可将激光雷达点分为两类：视觉特征点和普通点。
### D. 自适应下采样
在数据关联中提取关键点，是里程计系统中广泛采用的策略，因为它能在保持鲁棒位姿估计所需精度的同时，显著降低计算复杂度。KISS-ICP 采用体素网格下采样来提取关键点，每个体素都有预定义的尺寸。
---
其尺寸较小且仅包含一个代表性点，从而确保空间分布均匀。以这种方式获得的关键点无法自适应调整，因此我们提出了采用缩放因子$v_j$的自适应下采样方法。对于每个RGB点云$P_j^e$，我们首先进行一次下采样，得到用于与体素大小$P_j^{map}$合并的局部地图点云$\alpha v_j$；第二次下采样则得到体素大小为$P_j^{key}$的关键点$v_j$。引入扩展因子$\alpha$是为了确保地图分辨率高于关键点的分辨率，进而实现更有效的迭代优化。
为了增强系统在几何稀疏场景中的鲁棒性，我们将视觉特征投影到点云上。对于RGB点云中的每个点$p_i^c \in P_j^c$，我们根据其RGB像素值来判断它是否为PL特征点。为了从有限的视觉信息中提取更多视觉特征点，我们采用较小的体素大小以保留更详细的特征。PL特征点的下采样体素大小为$v_j^{pl}$，而其他点则称为普通点，其下采样体素大小为$v_j^{reg}$。下采样体素大小可表示为：
$$ v_{j}^{p l}=\alpha v_{j}^{r e g}=\left\{\begin{matrix}{\alpha v_{j},\alpha\in(0.0,1.0]}&{1^{s t}}\\ {v_{j}}&{2^{n d},}\\ \end{matrix}\right. $$
其中$\alpha$表示下采样的扩展因子，$v_j^{reg}$代表普通点的下采样体素大小，$v_j^{pl}$则表示PL特征点的下采样体素大小。通过这种方式选取的关键点能够有效捕捉场景的视觉特性，如图2(b)所示。通过将视觉特征融入自适应下采样策略，提取出的关键点具有更强的区分度，从而实现更稳健的数据关联。这种视觉引导有助于缓解点云退化问题，尤其是在低纹理或几何特征不明显的环境中。
### E. 局部地图管理
扫描到地图的配准比扫描到扫描的对齐更具鲁棒性[^25]。因此，构建能在匹配精度与计算效率之间取得平衡的局部地图，对于确保稳定且实时的位姿估计至关重要。我们使用哈希表来构建局部地图$M_{local}$，以便高效存储和管理体素块，每个体素块作为存储点云数据的容器。为了实现自适应的空间表示，我们引入了五个动态参数来控制体素块的分辨率与组织方式，从而使局部地图能够根据环境复杂度和系统需求实时调整。
$$ V_{{b l o c k}}^{i}\left\{i,v_{m},n_{j}^{\operatorname*{m a x}},F,R_{\operatorname*{m a x}}\right\}\qquad V_{{b l o c k}}^{i}\in M_{{l o c a l}}, $$
其中i为体素块的编号，$v_{map}$表示局部地图的网格体素大小为$v_m \times v_m \times v_m$，$n_j^{\max}$代表第j帧中每个体素允许存储的点数，F是用于区分特征点的体素块特征标签，$R_{\max}$则表示局部地图的半径。为便于管理，PL特征点$P_{pl} \in P_j^{\max}$和普通点$P_{reg} \in P_j^{\max}$被分别存储在标记为F的体素块中。由于视觉特征对视角和光照变化较为敏感，特征体素块中仅保留距离当前帧最近的10帧数据，以保证特征的一致性并减少内存消耗。普通点则根据距离范围$R_{\max}$被剔除。每个体素内的存储点数$n_j^{\max}$为：
Fig. 3. Correspondence between key points and the local map within a voxel block. In the illustration, green points denote key points, while blue points represent the points within the voxel block. (a) The map resolution is higher than the key points. (b) The map resolution is lower than the key points.
局部地图中的数据块会影响相应估计的精度。空间越小，点云越密集，局部地图中每个体素块所需的点数就越多，如图3所示。因此，每个体素块的最大存储数量$n_{j}^{\max}$会根据当前的点云分辨率$\rho_{i}$动态调整。
这样一来，局部地图$M_{local}$的大小就受到了限制，其中包含了最近10帧中存储的特征体素块以及范围$R_{max}$内的常规体素块；体素块大小$v_m \times v_m \times v_m$保持恒定，而每个体素块的存储数量$n_j^{\max} = 0.5\rho_j$则是动态的。这种设计使我们的方法能够有效适配大规模开放环境和狭窄空间。
### F. 用于数据关联的空间自适应阈值
在扫描到地图的点云搜索过程中，快速在局部地图中找到正确的目标点云至关重要，而最大搜索距离是一种常见的约束条件。KISS-ICP提出了一种通过模型误差$\sigma_{j}$来自适应调整阈值的方法，具体的公式推导可参见文献[^19]。此处仅对修改后的公式进行说明。
当前的模型偏差$\Delta T_j$可由匀速模型的预测位姿和校正后的位姿得出，对应点的可能偏移误差距离$\delta(\Delta T_j)$可通过式(5)计算得出。
$$ \delta(\Delta T_{j})=\delta(\Delta R_{j})+\delta(\Delta t_{j}). $$
其中$\Delta R_j \in SO(3)$和$\Delta t_j \in \mathbb{R}^3$，$\delta(\Delta R_j)$是由式(6)计算得出的旋转偏移误差距离，$\delta(\Delta t_j)$是由式(7)计算得出的平移偏移误差距离。
$$ \delta_{r o t}(\Delta R_{j})=2v_{j}r_{\max}\sin\left(\frac{1}{2}\arccos\left(\frac{t r(\Delta R_{j})-1}{2}\right)\right) $$
$$ \delta_{{t r a n s}}(\Delta t_{j})=\frac{\|\Delta t_{j}\|_{2}}{v_{j}}. $$
为了适应场景尺度的变化，尺度因子$v_j$被纳入偏移误差计算中，从而使系统能够自适应地平衡大规模环境和受限环境中的定位精度。值得注意的是，在开阔的户外区域，0.5米的漂移或许可以容忍，但在狭窄的室内走廊中，同等程度的误差就可能导致严重的定位失败。通过这种方式，即可获得每一帧的偏移误差$\delta(\Delta T_j)$。而模型误差$\sigma_j$则可通过式(8)将所有偏移误差叠加后计算得出。
$$ \sigma_{j}=\sqrt{\frac{1}{N}\sum_{j\in N}\delta(\Delta T_{j})^{2}}, $$
---
其中N表示高于偏差阈值$\sigma_{min}$的模型误差数量。KISS-ICP使用$\tau_{j}=3\sigma_{j}$作为搜索边界，但该方式无法动态适应不同的场景尺度。结果就是，搜索距离要么过大，导致计算开销增加；要么过小，在尺度多变的场景中引发搜索失败。因此，引入尺度因子$v_{j}$，使搜索距离能够随空间尺度进行调整，其表达式如下：
$$ \tau_{j}=\sigma_{j}v_{j}. $$
### G. 自适应双重迭代优化
传统的点对点ICP具有很高的适用性，但它对噪声敏感，在尺度变化的情况下缺乏鲁棒性。为解决这一问题，我们在传统ICP中引入了空间因子和视觉特征。自适应双迭代最近点（AD-ICP）是一种由粗到精的点云配准方法。为了获得鲁棒且精确的位姿$T_j$，我们基于预测的位姿$T_{pre,j}$进行两次校正。我们首先利用视觉特征点$P_j^{pl} \in P_j^{key}$来估计视觉位姿校正量$\Delta T_{v,j}$，然后利用普通点$P_j^{reg} \in P_j^{key}$来估计ICP位姿校正量$\Delta T_{g,j}$。不进行联合迭代优化的原因是不同关键点之间会产生干扰，从而导致迭代时间增加。AD-ICP的步骤如下。
首先根据预测的位姿$P_j^{pl}$将视觉特征点$T_{pre,j}$转换到全局坐标系下。
$$ S_{j}^{p l}=\left\{s_{i}=T_{j-1}T_{p r e,j}p_{i}|p_{i}\in P_{j}^{p l}\right\}, $$
其中$S_j^{pl}$是源特征点，$T_{j-1}$是上一次扫描得到的位姿，$p_i$是$P_j^{pl}$中的一个点。在第一次ICP的第k次迭代中，每个特征源点$s_i \in S_j^{pl}$会在局部地图$q_i \in Q_j^{pl}$中找到其对应的目标特征点$M_{local} = \{q_i | q_i \in \mathbb{R}^6\}$，距离阈值设为$\tau_j$。$s_i$和$q_i$是六维向量，可表示为：
$$ \begin{aligned}&s_{i}=(x_{s},y_{s},z_{s},r_{s},g_{s},b_{s})\\&q_{i}=(x_{q},y_{q},z_{q},r_{q},g_{q},b_{q}).\\ \end{aligned} $$
我们将$s_i^g$、$q_i^g$定义为向量的前三个维度，代表几何信息；将$s_i^c$、$q_i^c$定义为后三个维度，代表颜色信息。随后，我们以特征加权的点对点残差和最小化作为优化目标。
$$ \Delta T_{v,k}^{e s t}=\underset{T}{\arg\min}~\sum w_{i}^{c}(\left\|T s_{i}^{g}-q_{i}^{g}\right\|_{2}), $$
其中$w_{i}^{c}$表示每对点之间的特征相似度，可表示为：
$$ w_{i}^{c}=\frac{\|q_{i}^{c}\|^{2}}{\|s_{i}^{c}-q_{i}^{c}\|^{2}+\|q_{i}^{c}\|^{2}}, $$
每次迭代都会得到位姿校正量$\Delta T_{v,k}^{est}$。迭代收敛后，我们便得到了经视觉校正的位姿$\Delta T_{v,j} = \prod_{k} \Delta T_{v,k}^{est}$。
结合视觉校正后的位姿$\Delta T_{v,j}$，将普通关键点$P_i^{reg}$转换到全局坐标系下，普通源点$S_i^{reg}$可表示为：
$$ S_{j}^{r e g}=\left\{s_{i}=\Delta T_{v,j}T_{j-1}T_{p r e,j}p_{i}|p_{i}\in P_{j}^{r e g}\right\}. $$
接着，与第一次迭代类似，每个点$s_i \in S_j^{reg}$会与其对应的点$q_i \in Q_j^{reg}$匹配。我们以几何加权的点对点残差和最小化作为优化目标。
$$ \Delta T_{g,k}^{e s t}=\underset{T}{\operatorname{a r g}\operatorname*{m i n}}\sum w_{i}^{g}(\left\|T s_{i}^{g}-q_{i}^{g}\right\|_{2}), $$
其中$w_{i}^{g}$表示几何置信度，$\Delta T_{g,k}^{est}$代表每次迭代的位姿校正量。为了更好地适应多尺度场景，我们引入了空间因子，几何置信度可表示为：
$$ w_{g}=\left(\frac{\left\|s_{i}^{g}-q_{i}^{g}\right\|^{2}}{\alpha\tau_{j}+\left\|s_{i}^{g}-q_{i}^{g}\right\|^{2}}\right)^{2}, $$
迭代过程收敛后，我们便得到了第二次迭代优化中的几何校正误差$\Delta T_{g,j} = \prod_k \Delta T_{g,k}^{est}$。最终，第j帧的位姿偏差$\Delta T_j$可表示为：
$$ \Delta T_{j}=\left(\Delta T_{v,j}T_{j-1}T_{p r e,j}\right)^{-1}\Delta T_{g,j}\Delta T_{v,j}T_{j-1}T_{p r e,j}. $$
迭代过程中，当校正量小于最小变化阈值$\gamma$时，即视为收敛条件。对于第一次迭代优化，最小变化阈值为$10\gamma$，该方法在应用视觉特征校正时几乎不消耗计算资源。在视觉特征提取不足的情况下，比如光照较差时，系统会自动跳过粗略的视觉匹配阶段。这种设计确保了算法具有鲁棒性，且对光照变化不敏感。
### 四、实验评估
在本节中，我们从以下几个方面对所提出的方法进行全面评估。第IV-A节介绍了数据集和实验设置。第IV-B节通过消融实验来验证每个所提模块的有效性。在第IV-C节中，我们将所提方法与几种最先进的算法进行比较，包括基于激光雷达的方法（KISS-ICP $^{[19]}$、Traj-LO $^{[16]}$、Loam-livox $^{[11]}$、Livox-mapping $^{[1]}$），激光雷达-惯性方法（FAST-LIO2 $^{[21]}$），以及激光雷达-惯性-视觉方法（FAST-LIVO $^{[22]}$）。在第IV-D节中，进一步在特殊的高视角下对所提方法进行评估，以验证其对非常规且大尺度观测视角的适应性。最后，第IV-E节评估了所提算法的实时性能。
### A. 数据集与设置
我们的实验涵盖了五种数据集，包括自动驾驶、机器人、手持设备以及无人机测绘场景，旨在全面评估所提方法在不同平台和环境下的适用性与局限性。
Botanic Garden [^26] is a challenging robot navigation dataset collected in a 48000 m $^{2}$ natural botanic garden, featuring complex unstructured environments such as dense vegetation, narrow trails, riversides, and grasslands. The dataset is collected with stereo cameras, 3D LiDARs, and IMUs.
---
TABLE II
THE ATE/RTE OF OUR METHOD ON ABLATION EXPERIMENT
| Method | SEU 04 | M3DGR Corrid01 | M3DGR Dark01 | GEODE Tunnel5 |
| ATE | RTE | ATE | RTE | ATE | RTE | ATE | RTE |
| Baseline | 4.531 | 0.089 | 20.819 | 29.443 | 0.185 | 0.110 | 24.605 | 0.226 |
| +V | 3.962 | 0.098 | 17.795 | 25.166 | 0.189 | 0.113 | 31.816 | 0.227 |
| +A | 0.879 | 0.043 | 11.043 | 15.61 | 0.169 | 0.110 | 4.455 | 0.047 |
| +AV | 0.380 | 0.042 | 1.119 | 1.582 | 0.172 | 0.108 | 0.373 | 0.046 |
SEU数据集是一组包含八个序列的私有数据集，涵盖了各种具有挑战性的场景，包括闭环路线、恶劣环境、尺度变化以及长距离导航。数据是通过AVIA固态激光雷达、海康威视彩色相机以及用于获取真实轨迹的GNSS模块采集的。其中SEU_04和SEU_08代表尺度变化的场景，SEU_02、SEU_05和SEU_06包含闭环轨迹，其余序列则反映正常的驾驶条件。
GEODE数据集 [^27] 是一个多激光雷达数据集，专注于越野、内河航道和地铁隧道等现实中的退化场景，旨在评估SLAM算法在特征稀少且模糊条件下的性能。
MARS-LVIG数据集 [^28] 是一个激光雷达-视觉-惯性-GNSS数据集，包含了在80米至130米高度拍摄的空中视角图像。该数据集涵盖了多种具有挑战性的环境，包括机场、岛屿、乡村城镇和山谷。
M3DGR数据集 [^29] 是一个传感器丰富的基准测试集，专门设计用于评估SLAM系统在各种场景下的表现，包括视觉挑战、激光雷达退化、车轮打滑以及GNSS信号缺失等情况。数据采集平台配备了RGB-D相机、3D激光雷达、GNSS、惯性测量单元以及轮式里程计传感器，为鲁棒的SLAM评估提供了丰富的多模态数据。
在植物园、SEU和GEODE数据集中，所有方法均采用相同的参数设置进行评估，以考察它们对不同场景的鲁棒性和敏感性。我们系统的参数设置总结在表I中。在定量分析方面，我们使用evo工具包 [^30] 来计算绝对轨迹误差（ATE）和相对轨迹误差（RTE）的均方根误差，窗口大小设为1米。
### B. 实验1：消融研究
为了验证视觉辅助模块和自适应空间模块的有效性，我们开展了消融实验。基线方法采用固定的下采样尺寸，且不引入视觉输入。随后，我们分别加入自适应空间模块（A）和视觉辅助模块（V），以评估它们各自的贡献。为确保实验结果的可信度，我们在具有不同场景特征的三个数据集上对所提方法进行了评估。定量结果汇总于表II中，相应的运动轨迹则如图4所示。
SEU 04（地下车库）、M3DGR Corridor01（室内走廊）以及GEODE Tunnel5（地铁隧道）属于点云退化程度各异的环境，这种退化主要由受限结构和较低的几何多样性导致。当激光雷达靠近平面墙壁时，系统很容易出现故障，表现为明显的轨迹漂移现象。这主要是由于采用了固定的下采样体素尺寸，导致生成的点云过于稀疏，所含结构信息有限，
Fig. 4. (a) Trajectory comparison on SEU_04 sequence. The bold black solid lines indicate walls, and the red arrows indicate the moving direction. (b) The point cloud maps on M3DGR Corrid01 sequence. (c) The point cloud maps on the GEODE Tunnel5 sequence. Subfigures (1), (2), (3), and (4) respectively illustrate the results of Baseline, Baseline+V, Baseline+A, and Baseline+AV methods.
从而难以实现精准对齐。相比之下，所提出的自适应下采样策略通过保留更多具有信息量的点来缓解这一问题。即便在点云数量充足的情况下，当激光雷达在几何变化较小的环境中运行时，这种漂移现象在激光雷达里程计中仍较为常见，比如平滑或无特征的表面。仅引入视觉约束时，系统在退化场景中仍可能因点云数量不足而失效。只有将空间自适应模块与视觉辅助模块相结合，才能有效应对这些极具挑战性的场景，如图4所示。为了评估由粗到细的迭代策略对光照变化的敏感度，我们在黑暗环境序列M3DGR Dark01中开展了实验。如表II所示，性能下降幅度极小，说明该方法对低光照条件具有很强的鲁棒性。
消融实验证实，自适应空间模块与视觉辅助模块的融合显著提升了系统的鲁棒性，尤其是在退化或复杂的环境中。此外，该系统受光照变化的影响较小，进一步增强了其在各类场景中的可靠性。
### C. 实验二：定位精度评估
在本节中，我们将所提方法在代表不同环境的3个数据集上与最先进的里程计算法进行对比，这些环境包括室内受限空间、城市室外区域、自然非结构化场景以及退化场景。对比结果汇总于表III中，所提方法在所有评估场景下均取得了具有竞争力的性能。FAST-LIO、FAST-LIVO和Traj-LO的相对轨迹误差（RTE）低于所提方法。这是因为利用惯性测量单元（IMU）和连续时间模型能够对激光雷达点云进行更精准的运动补偿；同时，基于滤波的框架（FAST-LIO、FAST-LIVO）以及连续时间模型（Traj-LO）还进一步纳入了来自历史帧的姿态约束，从而使轨迹更加平滑。Loam-livox和Livox-mapping方法的性能在不同环境下的参数设置下极为敏感，导致其在不同场景中表现出明显的不稳定性。
在Garden Botanic数据集中，PL特征增强了所提方法的鲁棒性，使其能够在非结构化
---
TABLE III
THE COMPARISON OF ABSOLUTE/RELATIVE TRAJECTORY ERRORS (ATE/RTE, METERS). THE – INDICATES METHOD FAIL IN THIS SEQUENCE
| Dataset | Sequence | NO IMU | USE IMU |
| Ours | KISS-ICP [^19] | Traj-LO [^16] | Loam-livox [^11] | Livox-Mapping $^{1}$ | FAST-LIO2 [^21] | FAST-LIVO [^22] |
| RTE | ATE | RTE | ATE | RTE | ATE | RTE | ATE | RTE | ATE | RTE | ATE | ATE |
| Botanic Garden | 1005-00 | 0.047 | 2.26 | 0.058 | 4.70 | 0.022 | 2.41 | 0.061 | 5.47 | 0.185 | 23.65 | 0.018 | 2.305 | 0.024 |
| 1005-01 | 0.045 | 2.15 | 0.066 | 2.82 | 0.018 | 2.04 | 0.067 | 13.61 | 0.144 | 3.81 | 0.018 | 2.470 | 0.026 |
| 1005-07 | 0.051 | 5.88 | 0.098 | 14.52 | 0.026 | 3.39 | 0.074 | 14.02 | 0.162 | 24.67 | 0.023 | 4.438 | 0.027 |
| 1006-01 | 0.050 | 5.77 | 0.073 | 9.04 | 0.028 | 3.46 | 0.093 | 26.71 | 0.071 | 11.75 | 0.065 | 39.733 | 0.028 |
| 1008-03 | 0.059 | 3.31 | 0.089 | 7.41 | 0.025 | 3.41 | 0.095 | 11.83 | 0.095 | 19.77 | 0.024 | 4.019 | 0.042 |
| 1018-00 | 0.046 | 0.54 | 0.058 | 0.88 | 0.021 | 0.47 | 0.056 | 0.71 | 0.062 | 0.73 | 0.027 | 2.154 | 0.043 |
| 1018-13 | 0.042 | 1.01 | 0.065 | 1.31 | 0.018 | 1.11 | 0.073 | 8.11 | 0.074 | 2.37 | 0.025 | 2.390 | 0.028 |
| SEU (Vehicle) | 01 | 0.052 | 1.07 | 0.056 | 1.12 | 0.023 | 4.99 | 7.463 | 43.47 | 0.148 | 77.52 | 0.020 | 4.83 | 0.036 |
| 02 | 0.059 | 0.64 | 0.064 | 0.61 | 0.072 | 1.13 | 8.397 | 27.75 | 0.097 | 18.26 | 0.037 | 0.94 | 0.054 |
| 03 | 0.051 | 1.72 | 0.061 | 2.69 | 0.025 | 3.17 | 0.413 | 8.78 | 0.855 | 34.07 | 0.024 | 5.28 | 0.046 |
| 04 | 0.035 | 0.32 | 0.293 | 24.97 | 0.028 | 0.81 | 0.958 | 4.51 | 0.025 | 0.65 | 0.012 | 0.29 | 0.024 |
| 05 | 0.048 | 1.08 | 0.049 | 1.61 | 0.011 | 0.48 | 0.828 | 11.99 | 0.117 | 40.49 | 0.019 | 3.06 | 0.019 |
| 06 | 0.059 | 1.14 | 0.065 | 1.69 | 0.019 | 0.39 | 6.027 | 40.88 | 13.983 | 102.73 | 0.022 | 2.55 | 0.024 |
| 07 | 0.065 | 2.89 | 0.053 | 4.42 | 0.021 | 3.94 | -- | -- | -- | -- | 0.021 | 7.79 | -- |
| 08 | 0.039 | 0.71 | 0.227 | 53.14 | 0.026 | 1.05 | 0.792 | 4.95 | 0.028 | 2.35 | 0.011 | 0.21 | 0.023 |
| GEODE (Handle) | Tunnel1 | 0.045 | 0.56 | 0.046 | 0.35 | 0.014 | 0.14 | 2.65 | 30.20 | 4.53 | 1.60 | 0.032 | 0.23 | 0.045 |
| Tunnel2 | 0.047 | 1.04 | 0.051 | 1.79 | 0.049 | 1.55 | 2.98 | 23.53 | 2.47 | 1.32 | 0.025 | 1.88 | 0.024 |
| Tunnel3 | 0.057 | 0.56 | 0.054 | 6.12 | 0.078 | 22.0 | 2.91 | 16.98 | 2.44 | 3.57 | 0.035 | 0.51 | 0.066 |
| Tunnel4 | 0.043 | 0.32 | 0.081 | 0.33 | 0.046 | 0.31 | 2.88 | 18.50 | 3.68 | 0.70 | 0.038 | 0.35 | 0.021 |
| Tunnel5 | 0.046 | 0.37 | 0.073 | 5.79 | 0.028 | 0.67 | 2.61 | 7.57 | 2.27 | 2.26 | 0.022 | 0.43 | 0.042 |
| Avg | 0.049 | 1.66 | 0.084 | 7.26 | 0.029 | 2.84 | 2.07 | 16.82 | 1.65 | 19.59 | 0.025 | 4.29 | 0.033 |
Fig. 5. Project point cloud map onto satellite image. The zoom-in images at the left reflect the mapping accuracy and the pictures at the bottom reflect the scene situation. (a) Dynamic object scene. (b) Follow the van scene. (c) Dazzling scene. (d) Low recognition scene.
以及特征稀疏的场景中可靠运行。在1006-01序列中，不平坦路面导致的强烈平台振动会使IMU信号退化，进而造成FAST-LIO和FAST-LIVO等依赖IMU的方法出现显著的Z轴漂移现象。
由于固定体素尺寸的限制，KISS-ICP在SEU_04和SEU_08场景下的表现不佳，这限制了其在尺度变化场景中的适应性，进而降低了精度。SEU_07覆盖4.627公里范围，且存在动态环境和低可见度条件，导致许多方法在此场景下稳定性较差。此类场景中，由于遮挡和移动物体的存在，视觉里程计变得不可靠。因此，FAST-LIVO在包含长时间车辆跟随段的SEU_07场景中也无法正常工作，其失效点如图5(b)所示。为了评估全局一致性，我们将LIVOX-CAM地图与卫星图像在图5中进行对齐。
在GEODE退化数据集上，该数据集具有频繁的激光雷达退化现象和复杂的光照条件，我们的方法在所有序列中均保持了稳定的性能。这种鲁棒性源于视觉特征与几何特征之间的相互补充：在纹理丰富的区域，视觉线索可以弥补激光雷达退化带来的影响；而在视觉数据不可靠的区域，几何信息则能强化对齐效果。由粗到精的配准策略进一步提升了系统在复杂环境中的稳定性。尽管
TABLE IV
THE COMPARISON OF ABSOLUTE TRAJECTORY ERRORS (ATE, METERS) ON MARS-LVIG DATASET. THE – INDICATES THAT THE METHOD FAILED
| Sequence | Ours | KISS-ICP [^19] | Traj-LO [^16] | FAST-LIO2 [^21] |
| HKairport01 | 3.85 | 10.63 | 9.12 | 0.66 |
| HKairport02 | 8.84 | 18.16 | 17.95 | 1.14 |
| HKisland01 | 0.61 | 1.021 | 0.35 | 0.64 |
| HKisland02 | 4.28 | -- | 1.94 | 2.13 |
| AMtown01 | 3.24 | 12.59 | 2.46 | 2.28 |
| AMtown02 | 10.59 | -- | 4.30 | 3.24 |
| AMvalley01 | 11.18 | -- | -- | 4.54 |
| AMvalley02 | 42.86 | -- | -- | 8.12 |
虽然FAST-LIVO采用了紧耦合的激光雷达-惯性-视觉框架，但其性能对光照变化极为敏感。在剧烈的光照变化下，视觉特征会变得不可靠，导致FAST-LIVO的表现不如FAST-LIO2。
### D. 实验三：高海拔场景评估
为了进一步探索系统的极限，我们引入了高海拔测绘场景。MARS-LVIG数据集对激光雷达里程计而言具有独特的挑战性，常常使得许多基于特征的方法无法生效。因此，我们选取了具有代表性的方法进行对比，结果如表四所示。MARS-LVIG数据集中无人机的高速飞行和气流扰动，给恒速运动模型带来了巨大挑战。
结果显示，在不使用惯性测量单元（IMU）数据的情况下，我们的方法是唯一没有在任何序列中失效的方案。Traj-LO将每个激光雷达帧划分为多个段落，并引入轨迹平滑约束，从而在没有退化的序列中取得了良好性能。在AMvalley01和AMvalley02序列中，无人机逐渐靠近山体，导致扫描区域迅速缩小。这引发了点云退化，致使KISS-ICP和Traj-LO等方法失效。在无人机运动剧烈的转弯区域，恒速模型会产生更大的位姿误差；在高海拔地区，由于投影距离较长，这些误差会被进一步放大，从而降低扫描结果与地图的重叠度。虽然在此类场景中我们的方法可能无法超越FAST-LIO2等紧耦合系统，但相比纯激光雷达里程计方法仍有显著提升。
---
TABLE V PERFORMANCE EVALUATION
| SEU | Botanic Garden | GEODE | MARS-LVIG |
| Time (ms) | 69.4 | 70.8 | 57.6 | 93.4 |
| Memory (MB) | 552 | 497 | 256 | 833 |
激光雷达里程计方法。由我们的算法生成的该数据集彩色地图如图1(b)所示。
### E. 运行时间分析
我们的算法能够保持满足10 Hz激光雷达输入频率的实时性能，可在配备英特尔酷睿i7-14700K CPU和64 GB内存的系统上运行，无需依赖GPU加速。不同数据集上的平均处理时间和最大内存占用情况见表五。
### V. 结论
这封信介绍了LIVOX-CAM——一种面向固态激光雷达的视觉辅助激光雷达里程计框架。该系统通过集成自适应空间模块和视觉辅助功能来提升鲁棒性，从而有效减轻在复杂环境中激光雷达性能退化带来的影响。大量实验结果表明，我们的系统在多种挑战性场景下均能保持鲁棒性，包括尺度变化场景、退化场景、低光照场景、非结构化场景以及高海拔测绘场景。不过，在高速且振动剧烈的场景中，由于恒定速度假设的局限性，该系统的表现不如连续时间模型。未来的工作将探索自适应运动模型和视觉回环闭合技术，以提升全局一致性和长期鲁棒性。
## 参考文献
[^1]: P.-Y. Lajoie and G. Beltrame, “Swarm-SLAM: Sparse decentralized collaborative simultaneous localization and mapping framework for multi-robot systems,” IEEE Robot. Automat. Lett., vol. 9, no. 1, pp. 475–482, Jan. 2024.
[^2]: S. Boche, X. Zuo, S. Schaefer, and S. Leutenegger, “Visual-inertial SLAM with tightly-coupled dropout-tolerant GPS fusion,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2022, pp. 7020–7027.
[^3]: C. Campos, R. Elvira, J. J. G. Rodriguez, J. M. M. Montiel, and J. D. Tardos, "ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM," IEEE Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021.
[^4]: J. Liu, X. Li, Y. Liu, and H. Chen, "RGB-D inertial odometry for a resource-restricted robot in dynamic environments," IEEE Robot. Automat. Lett., vol. 7, no. 4, pp. 9573–9580, Oct. 2022.
[^5]: S. Yi, Y. Lyu, L. Hua, Q. Pan, and C. Zhao, "Light-LOAM: A lightweight LiDAR odometry and mapping based on graph-matching," IEEE Robot. Automat. Lett., vol. 9, no. 4, pp. 3219–3226, Apr. 2024.
[^6]: X. Cheng, T. Ma, K. Geng, Z. Liu, Z. Wang, and G. Yin, “SVM-LO: An accurate, robust, real-time LiDAR odometry with segmentation voxel map for autonomous vehicles,” in Proc. IEEE 27th Int. Conf. Intell. Transp. Syst., 2024, pp. 1870–1877.
[^7]: J. Lin and F. Zhang, “R $^{3}$LIVE++: A robust, real-time, radiance reconstruction package with a tightly-coupled LiDAR-inertial-visual state estimator,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 46, no. 12, pp. 11168–11185, Dec. 2024.
[^8]: C. Zheng et al., “FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry,” IEEE Trans. Robot., vol. 41, pp. 326–346, 2025.
[^9]: P. Dellenbach, J.-E. Deschaud, B. Jacquet, and F. Goulette, "CT-ICP: Real-time elastic LiDAR odometry with loop closure," in Proc. IEEE Int. Conf. Robot. Automat., 2022, pp. 5580–5586.
[^10]: T. X. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized LiDAR odometry and mapping on variable terrain,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018, pp. 4758–4765.
[^11]: J. Lin and F. Zhang, “Loam_livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV,” in Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 3126–3131.
[^12]: W. X. Dongjiao He, N. Chen, F. Kong, C. Yuan, and F. Zhang, “Point-LIO: Robust high-bandwidth light detection and ranging inertial odometry,” Adv. Intell. Syst., vol. 5, no. 1, 2023, Art. no. 2200459.
[^13]: K. Honda, K. Koide, M. Yokozuka, S. Oishi, and A. Banno, “Generalized LOAM: LiDAR odometry estimation with trainable local geometric features,” IEEE Robot. Automat. Lett., vol. 7, no. 4, pp. 12459–12466, Oct. 2022.
[^14]: M. Yokozuka, S. Oishi, and A. Banno, “LiTAMIN2: Ultra light LiDAR-based SLAM using geometric approximation applied with KL-divergence,” in Proc. IEEE Int. Conf. Robot. Automat., 2021, pp. 11619–11625.
[^15]: Y. Pan, Y. He, Z. Shao, and Z. Li, “MULLS: Versatile LiDAR SLAM via multi-metric linear least square,” in Proc. IEEE Int. Conf. Robot. Automat., 2021, pp. 11633–11640.
[^16]: X. Zheng and J. Zhu, “Traj-LO: In defense of LiDAR-only odometry using an effective continuous-time trajectory,” IEEE Robot. Automat. Lett., vol. 9, no. 2, pp. 1961–1968, Feb. 2024.
[^17]: J. Zhang and S. Singh, "Low-drift and real-time LiDAR odometry and mapping," Auton. Robots, vol. 41, no. 2, pp. 401–416, 2017.
[^18]: H. Wang, C. Wang, C.-L. Chen, and L. Xie, "F-LOAM: Fast LiDAR odometry and mapping," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2021, pp. 4390–4396.
[^19]: I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, and C. Stachniss, "KISS-ICP: In defense of point-to-point ICP – simple, accurate, and robust registration if done the right way," IEEE Robot. Automat. Lett., vol. 8, no. 2, pp. 1029–1036, Feb. 2023.
[^20]: Y. Zhu, C. Zheng, C. Yuan, X. Huang, and X. Hong, “CamVox: A low-cost and accurate LiDAR-assisted visual SLAM system,” in Proc. IEEE Int. Conf. Robot. Automat., 2021, pp. 5049–5055.
[^21]: 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, Aug. 2022.
[^22]: C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, "FAST-LIVO: Fast and tightly-coupled sparse-direct LiDAR-inertial-visual odometry," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2022, pp. 4003–4009.
[^23]: E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, "ORB: An efficient alternative to SIFT or SURF," in Proc. Int. Conf. Comput. Vis., 2011, pp. 2564–2571.
[^24]: R. G. von Gioi, J. Jakubowicz, J.-M. Morel, and G. Randall, "LSD: A fast line segment detector with a false detection control," IEEE Trans. Pattern Anal. Mach. Intell., vol. 32, no. 4, pp. 722–732, Apr. 2010.
[^25]: J. Behley and C. Stachniss, “Efficient surfel-based SLAM using 3D laser range data in urban environments,” Robot.: Sci. Syst., vol. 2018, p. 59, 2018.
[^26]: Y. Liu et al., “BotanicGarden: A high-quality dataset for robot navigation in unstructured natural environments,” IEEE Robot. Automat. Lett., vol. 9, no. 3, pp. 2798–2805, Mar. 2024.
[^27]: Z. Chen et al., “Heterogeneous LiDAR dataset for benchmarking robust localization in diverse degenerate scenarios,” Int. J. Robot. Res., 2024, Art. no. 02783649251344967.
[^28]: H. Li et al., “MARS-LVIG dataset: A multi-sensor aerial robots SLAM dataset for LiDAR-visual-inertial-GNSS fusion,” Int. J. Robot. Res., vol. 43, no. 8, pp. 1114–1127, 2024.
[^29]: D. Zhang et al., “Towards robust sensor-fusion ground SLAM: A comprehensive benchmark and a resilient framework,” 2025, arXiv:2507.08364.
[^30]: D. Schubert, T. Goll, N. Demmel, V. Usenko, J. Stückler, and D. Cremers, “The TUM VI benchmark for evaluating visual-inertial odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018, pp. 1680–1687.