# 基于块状梯度优化的点线混合LIVO算法用于退化场景 施彤 $^{ID}$、钱坤 $^{ID}$（IEEE会员）、方怡欣 $^{ID}$、张云 $^{ID}$、余海 $^{ID}$ 摘要—基于三维激光雷达的同步定位与建图在缺乏结构信息的环境中容易出现性能退化，进而导致定位精度和建图精度显著下降。本文提出了一种基于FAST-LIVO系统框架的点线混合激光雷达视觉惯性里程计（PL-LIVO），用于在激光雷达退化场景中实现鲁棒的定位。其核心思路是将点和线特征都纳入所提出的直接视觉里程计子系统（PL-DVO）中。通过最小化用于状态优化的块状梯度残差，PL-DVO能够为激光雷达数据提供额外的约束条件。此外，本文还提出了一种激光雷达地图辅助的视觉特征深度提取（LM-VDE）方法，通过将视觉特征映射到激光雷达地图的三维平面上，来恢复这些特征的三维位置。该方法不依赖于单次扫描的密度，且在不同激光雷达传感器上的泛化能力优异。在公开数据集和自建数据集上的大量实验表明，PL-LIVO能够确保鲁棒的定位性能，在激光雷达退化场景中的表现优于其他最先进的系统。 关键词—同步定位与建图、定位、建图、激光雷达-惯性-视觉里程计、块状梯度优化。 ## 一、引言 同步定位与建图（SLAM）是未知环境中实时定位与三维重建的关键技术。基于三维激光雷达的SLAM系统 [^1]、[^2]、[^3] 由于能够提供精确的状态估计和密集的三维地图，已被广泛应用于自动机器人领域。然而，在一些实际应用场景中，比如施工现场的检测机器人 [^4] 以及隧道中行驶的自动驾驶车辆 [^5]，基于激光雷达的SLAM在无约束方向上（比如隧道的延伸方向）容易出现性能退化，从而导致定位精度大幅下降， ![Image](images/d4eb67289a3b59e64759b2105959f71550c6d891d09c496ed493992ef756a150.jpg) 惯性测量单元坐标系下的楼梯运动轨迹中的激光雷达退化环境
Fig. 1. localization and mapping result of PL-LIVO under the stairs_02 sequence. In this sequence, there was a significant exposure change when the operator entered indoors from outside. Moreover, challenging degeneration occurred when the operator was moving upstairs and downstairs, attributed to the limited FoV of the LiDAR scan.
这种定位精度的显著下降使得该系统无法满足实际应用需求。当激光雷达的视场角较小时，这一问题会更加突出 [^6]。 为了提升系统在复杂场景中的性能，许多研究 [^7]、[^8] 融合了惯性测量单元提供的高频运动数据。此外，通过整合图像中的视觉点特征，几种激光雷达-惯性-视觉里程计系统 [^9]、[^10]、[^11] 也展现了其在激光雷达退化场景中的鲁棒位姿估计能力。不过，当前的LIVO系统在处理退化环境时仍面临诸多挑战：（1）以往方法的视觉子系统严重依赖亮度不变性假设，该假设很容易因曝光变化或相机眩光而失效；（2）人造环境中存在大量的线特征，能够提供更有效的约束，但这类特征尚未得到充分利用；（3）现有大多数激光雷达辅助的视觉特征深度提取方法都要求激光雷达扫描具有较高的密度，因此难以适配多线旋转式激光雷达产生的相对稀疏的扫描数据。 为了克服上述局限性，我们提出了PL-LIVO——一种基于FAST-LIVO改进的点线混合视觉里程计[^11]，它采用基于补丁的梯度优化方法来应对退化场景。其核心思想是在直接视觉里程计子系统中整合点和线，以提升姿态估计精度。具体而言， --- 我们采用了一种新颖的基于补丁的梯度残差方法来应对频繁的光照变化；同时提出了一种用于视觉特征的激光雷达辅助深度提取方法，以提高其在不同激光雷达传感器上的泛化能力。具体贡献如下： 1) 我们提出了一种通过最小化基于补丁的梯度残差来整合点与线的直接视觉里程计子系统（PL-DVO）。基于梯度不变性假设，该残差对光照变化的适应性更强。针对线特征，我们设计了与线方向对齐的矩形补丁模式，从而能够搜索到更具信息量的像素，并在不同图像间建立更精确的补丁关联。 2) 我们提出了一种新颖的激光雷达地图辅助视觉特征深度提取（LM-VDE）方法，该方法将二维视觉特征映射到激光雷达地图的三维平面上，而非直接用激光雷达扫描点来拟合三维位置。因此，LM-VDE不受扫描密度的限制，在不同激光雷达传感器上的泛化能力更强。 3) 我们在通用数据集（如NTU VIRAL [^12]数据集）和退化数据集上进行了大量实验（这些退化数据集由R3LIVE [^10]、FAST-LIVO [^11]以及我们的平台在退化场景中采集，如图5所示）。结果表明，我们的系统在普通场景中性能具有竞争力，在激光雷达性能退化的场景中更是优于其他最先进的方法。 ### II. 相关工作 ### A. 激光雷达-视觉-惯性里程计 在激光雷达-视觉-惯性SLAM系统中，多传感器的融合能够缓解单一传感器的性能退化问题。LVI-SAM [^9]利用激光雷达测量得到的深度信息来补充视觉点，并借助视觉-惯性子系统估计出的姿态来初始化激光雷达扫描匹配。R2LIVE [^13]则在基于滤波的框架内整合了激光雷达、IMU和摄像头的数据。这两个系统均采用KLT光流法来跟踪特征点，并通过重投影误差来构建视觉约束。R3LIVE [^10]通过最小化跟踪点与地图之间的光度误差来增强系统的状态估计能力。FAST-LIVO [^11]则重用激光雷达点作为视觉特征，以提升实时性能。 由于线特征对视角变化的敏感度较低[^14]，且能提供更多的几何约束[^15]，许多研究致力于将线特征整合到视觉子系统中以引入额外信息。Huang等人[^16]在激光雷达-视觉里程计中利用线特征，通过在图像上构建点到线的重投影残差。他们提出了一种通过最小化三维点到线距离和二维重投影距离来实现线深度提取的方法。Cao等人[^17]构建了球面坐标系，用以关联视觉与激光雷达子系统中的几何特征，从而实现线段深度的重建。然而，这些深度提取方法依赖于高密度的激光雷达扫描数据。相比之下，我们提出的LM-VDE利用存储在激光雷达地图中的三维平面，仅需少量步骤即可实现更好的适配，尤其适用于相对稀疏的激光雷达传感器（如16线旋转式激光雷达）。 ![Image](images/4aa82c8393779d2e5fe3402a939b56a85f795d994b2f1fa1cf1c785474876e94.jpg)
Fig. 2. System overview of the proposed PL-LIVO. This system comprises 4 basic modules: Proprocessing, Data Association, Feature Generation, and Map Manager. The measurements are fused based on an iterated error-state Kalman filter.
### B. 直接视觉里程计 直接法[^18]、[^19]、[^20]是视觉里程计中用于估计位姿最常用的方法之一。与需要提取显著特征点并生成鲁棒特征描述符的基于特征的方法不同，直接法选择梯度像素并构建光度误差，因此在低纹理场景中表现更为稳健。DVL-SLAM[^21]将激光雷达扫描结果投影到图像上，选取梯度较大的像素来构建图像帧间的光度残差。PL-SVO[^22]将直接法扩展到线特征，沿每条线段均匀分布点，从而在结构化场景中获得额外的约束。Fang等人[^23]利用激光雷达扫描得到的深度测量值来恢复视觉点和视觉线的位置。与这些类视觉系统不同，FAST-LIVO[^11]紧密融合了图像与地图对齐、激光雷达扫描配准以及惯性测量单元数据，从而提升了系统在复杂环境中的鲁棒性。 这些方法都依赖于亮度不变的严格假设，而这一假设会因频繁的曝光变化和相机镜头眩光（如室内外场景）而被打破。一些研究通过在线光度校准[^24]、[^25]来缓解这一问题，但这类方法会增加额外的计算开销。与之不同，我们设计了一种基于补丁的梯度优化方法，该方法建立在较为宽松的梯度不变假设之上，从而增强了方法在不同光照环境下的鲁棒性。此外，我们为视觉线设计了与线方向对齐的矩形补丁，因此帧间对应的补丁可以沿线条进行关联。 ### III. 系统概述 图2展示了我们提出的PL-LIVO系统概览。该系统由核心的激光雷达-惯性里程计（蓝色和灰色部分）以及直接视觉里程计子系统PL-DVO（红色部分）组成。我们提出的LM-VDE（绿色部分）方法用于提取所有视觉特征的深度。图4详细说明了PL-DVO和LM-VDE的具体细节。惯性测量单元、激光雷达和摄像头的数据在观测层面被紧密耦合。 --- ![Image](images/cc755ab5294900883353d7a854b4df47f118acd3e5bb7ddb9d970c9dc00b9b0d.jpg) ![Image](images/06233836366bbcba4324ca15f15b8b014266747eb00c1863668fe639a6722f66.jpg)
Fig. 3. Image-based Data Alignment. As (a) shows, the LiDAR scan is treated as a points stream (green and blue points from different scans). Points within the interval of two consecutive images (the red dotted box) are gathered as a new scan. As (b) shows, an aligned unit is generated with the new scan and the current image.
最初，来自激光雷达和摄像头的输入数据会被送入预处理模块进行数据对齐和特征提取，从而为后续步骤生成对齐后的数据单元。同时，惯性测量单元（IMU）数据也会被采集用于状态传播。随后，该对齐后的数据单元会在数据关联模块中用于构建观测残差。具体而言，会针对每个激光雷达扫描点计算点到平面残差，而对于摄像头视场内的每个视觉特征，则计算基于图像块的梯度残差。值得注意的是，线条的图像块模式被设计成与线条方向对齐的矩形。所有残差会经过不同权重的加权处理，随后用于在误差状态卡尔曼滤波框架中更新系统状态。此外，特征生成模块会通过我们提出的LM-VDE方法，恢复所有提取出的二维视觉特征的三维坐标。同时，当前的激光雷达扫描点也会被转换到世界坐标系下。这些新生成的特征随后会分别被注册到视觉地图和激光雷达地图中，以便进一步进行特征关联和残差计算。为了限制内存占用，旧的特征会被及时移除。 ### 四、方法论 ### A. 预处理 预处理模块会对输入数据进行处理，为后续模块生成对齐后的数据单元。与大多数现有方法[^11]、[^13]（这些方法在接收到激光雷达或摄像头的数据时才会更新状态）不同，我们采用基于图像的数据对齐方式，能够支持同步联合优化。如图3(a)所示，激光雷达扫描结果被视为一系列点的集合（上一次扫描得到的绿色点和当前扫描得到的蓝色点）。两帧连续图像之间的区间内的点（红色虚线框内）会被收集为新的激光雷达扫描数据，随后会被降采样并校正为与当前图像相同的时间点。新的激光雷达扫描数据与当前图像会组合成对齐后的数据单元，如图3(b)所示。此外，图像中的点和线特征会通过[^26]、[^27]进行检测，图像的像素梯度幅值则由索贝尔算子提取，用于后续计算梯度残差。 $$ \mathcal{M}\triangleq S O(3)\times\mathbb{R}^{15} $$ $$ \begin{aligned}\boldsymbol{x}&\triangleq\left[^{W}\boldsymbol{q}_{b}^{T}\quad^{W}\boldsymbol{v}_{b}^{T}\quad^{W}\boldsymbol{p}_{b}^{T}\quad\boldsymbol{b}_{a}^{T}\quad\boldsymbol{b}_{g}^{T}\quad^{W}\boldsymbol{g}^{T}\right]^{T}\in\mathcal{M}\\\delta\boldsymbol{x}&\triangleq\left[^{W}\delta\boldsymbol{\theta}_{b}^{T}\quad^{W}\delta\boldsymbol{v}_{b}^{T}\quad^{W}\delta\boldsymbol{p}_{b}^{T}\quad\delta\boldsymbol{b}_{a}^{T}\quad\delta\boldsymbol{b}_{g}^{T}\quad^{W}\delta\boldsymbol{g}^{T}\right]^{T}\in\mathbb{R}^{18}\\\boldsymbol{u}&\triangleq\left[\boldsymbol{a}_{m}^{T}\quad\boldsymbol{\omega}_{m}^{T}\right]^{T}\\\boldsymbol{w}&\triangleq\left[\boldsymbol{n}_{a}^{T}\quad\boldsymbol{n}_{a}^{T}\quad\boldsymbol{n}_{ba}^{T}\quad\boldsymbol{n}_{b,v}^{T}\right]^{T}\tag{1}\end{aligned} $$ 与对齐后的数据单元相对应的IMU数据（如图3(a)中红色虚线框内所示）会被采集，用于计算状态$\pmb{x}$、误差状态$\delta\pmb{x}$、IMU输入$\pmb{u}$以及噪声$\pmb{w}$，如公式(1)所示。$\pmb{W}$和$\pmb{b}$分别对应世界坐标系和IMU（机体）坐标系。$\pmb{w}_{q_b}^{'},\pmb{w}_{b_b}^{'},\pmb{w}_{b_b}^{'}$和$\pmb{w}_{p_b}^{'}$表示机体的旋转、速度和位置，而$\pmb{b}_a$和$\pmb{b}_g$则代表IMU偏差。$\pmb{w}_{g}$表示世界坐标系下的重力。$\delta\pmb{x}$中的各项是相对于$\pmb{x}$中的状态的误差状态。$\pmb{w}$由IMU输入$\pmb{u}$的测量噪声以及IMU偏差的随机游走噪声组成。 以IMU测量值为输入，利用离散时间运动模型可推算出名义状态$\hat{x}$、误差状态$\delta x$以及对应的协方差矩阵$\hat{P}$，如公式(2)所示。$f(\hat{x}_k, u_k)$、$F_k$、$C_k$和$Q_k$的详细内容见[^28]。这种状态传播会给出$x_k$的先验正态分布，如公式(3)所示。 $$ \begin{aligned}\widehat{\boldsymbol{x}}_{k+1}&=f\left(\widehat{\boldsymbol{x}}_{k},\boldsymbol{u}_{k}\right)\\\delta\boldsymbol{x}_{k+1}&=\left(I+\boldsymbol{F}_{k}\Delta t\right)\delta\boldsymbol{x}_{k}+\left(\boldsymbol{C}_{k}\Delta t\right)\boldsymbol{w}\\\widehat{\boldsymbol{P}}_{k+1}&=\left(I+\boldsymbol{F}_{k}\Delta t\right)\widehat{\boldsymbol{P}}_{k}\left(I+\boldsymbol{F}_{k}\Delta t\right)^{T}\\&\quad+\left(\boldsymbol{C}_{k}\Delta t\right)\boldsymbol{Q}_{k}\left(\boldsymbol{C}_{k}\Delta t\right)^{T}\end{aligned} $$ $$ \boldsymbol{x}_{k}-\widehat{\boldsymbol{x}}_{k}=\delta\boldsymbol{x}_{k}\sim\mathcal{N}\left(\mathbf{0},\widehat{\boldsymbol{P}}_{k}\right) $$ ### B. 面向视觉特征的基于图像块的梯度残差 对于图像视场内的每个视觉特征，其三维位置会从视觉地图投影到当前图像上，从而建立与参考图像中对应特征的二维特征关联。随后，通过在当前图像中提取补丁P，并计算参考图像中对齐的补丁Q，来实现围绕该二维特征的补丁关联。相关补丁之间的像素级差异被定义为视觉残差。与传统方法在灰度图上收集像素并计算光度残差不同，我们如图4(b)所示，在梯度图上收集像素；这意味着PL-DVO是基于梯度不变性假设进行运算的。这样选择是因为曝光变化会改变整个图像的亮度，但对相邻像素间的相对亮度影响甚微。因此，当光照发生变化时，像素的梯度更为稳定。 图4(b)展示了视觉点和线的图像补丁模式。点补丁是常见的$2a \times 2a$正方形，而线补丁呈$a \times 4a$矩形，其4a边与线条方向对齐。这种线补丁能够选取具有更多有效梯度的像素，有助于在不同帧间关联对应的像素。 给定真实状态$x_k$、视觉地图$\mathcal{M}_V$以及当前梯度图像$I_k$，便可按照算法1的计算步骤，得出视觉特征的基于补丁的梯度残差。 对于$\mathcal{P}_i$视场内的每个视觉点$\mathbf{I}_k$（第2–8行），我们可以获取其在$\mathbf{I}_k$上的像素坐标，并提取其补丁$\mathbf{P}_i$（第4行）。如图4(a)所示，通过仿射变换$\mathbf{Q}_i$计算参考图像上与之关联的补丁$\mathbf{A}_i$，使其与$\mathbf{P}_i$对齐（第5行）。对齐后的补丁间的像素级差异被定义为视觉点残差$\mathbf{r}_P$，该值应为零，其计算公式如式(4)所示： $$ \mathbf{r}_{\mathcal{P}}\left(\boldsymbol{x}_{k},\mathcal{P}_{i}\right)=\mathbf{P}_{i}-\mathbf{A}_{i}\mathbf{Q}_{i}=\mathbf{D}^{\mathbf{p}}\left(\boldsymbol{\pi}\left({}^{C}\boldsymbol{T}_{W}\mathcal{P}_{i}\right)\right)-\mathbf{A}_{i}\mathbf{Q}_{i} $$ --- ![Image](images/ec00d8803fc1c7d38d8458c9d876e75fd54d99fe48332f01b78555bfe6abd237.jpg)
New Detected Visual Points/Lines on Current Image Projected Visual Points/Lines on Current Image from Visual Map
Fig. 4. Overview of the proposed PL-DVO. The proposed patch-based gradient residual for points and lines is constructed as (a). The utilized patch pattern is designed as (b). (c-f) depict the proposed LM-VDE method for extracting the depth of visual features.
```pseudo // Algorithm 1: Patch-Based Gradient Residuals. ``` 数据：视觉局部地图$\mathcal{M}_{V}$、梯度图像$\mathbf{I}_{k}$、真实状态$x_{k}$ 结果：视觉点残差$\{\mathbf{r}_{P}\}$、视觉线残差$\{\mathbf{r}_{L}\}$ 1 $\longrightarrow$ 视觉点残差$\longrightarrow$； 2 遍历$\mathcal{P}_{i}$中的每个点特征$\mathcal{M}_{V}$ 3 $\left|\begin{array}{c} if \mathcal{P}_{i} \text{ within the } \text{FoV of } \mathbf{I}_{k} \text{ then } \\ \text{compute square gradient patch } \mathbf{P}_{i} ; \\ \text{calculate associated reference patch } \mathbf{Q}_{i} \text{ by } \text{affine transformation } \mathbf{A}_{i}, \text{ and compute point } \\ \text{patch residual } \mathbf{r}_{P} (4); \end{array}\right.$ 4 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$ 5 $\left|\begin{array}{c} \text{end} \\ \text{for each line feature } \mathcal{L}_{i} \text{ in } \mathcal{M}_{V} \text{ do } \\ \text{if } \mathcal{L}_{i} \text{ within the } \text{FoV of } \mathbf{I}_{k} \text{ then } \\ \text{for each sample point } l_{m} \text{ on } \mathcal{L} \text{ do } \\ \text{compute rectangle gradient patch } \mathbf{P}_{m} \\ \text{aligned with line direction } (5); \\ \text{calculate associated reference patch } \mathbf{Q}_{m} \text{ by } \text{line direction and scale, and compute point-on-line patch residual } \mathbf{r}_{l} (5); \end{array}\right.$ 6 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$ 7 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$ 8 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$ 9 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{for each line feature } \mathcal{L}_{i} \text{ in } \mathcal{M}_{V} \text{ do } \\ \text{if } \mathcal{L}_{i} \text{ within the } \text{FoV of } \mathbf{I}_{k} \text{ then } \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 10 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7) \end{array}\right.$ 11 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{for each sample point } l_{m} \text{ on } \mathcal{L} \text{ do } \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 12 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{for each sample point } l_{m} \text{ on } \mathcal{L} \text{ do } \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 13 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 14 $\left|\begin{array}{c} \text{visual line residual } \longrightarrow; \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 15 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 16 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 17 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 18 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 19 $\left|\begin{array}{c} \text{end} \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{add } \mathbf{r}_{P} \text{ to } \{\mathbf{r}_{P_{i}}\} (7); \\ \text{end} \end{array}\right.$ 其中$\pi(\bullet)$为相机投影模型，$\mathbf{D}^{\mathbf{P}}$表示在梯度图像上提取$2a \times 2a$正方形内邻近像素的操作。${}^{C}T_{W}$代表从世界坐标系到相机坐标系的变换，该变换由外部参数${}^{C}T_{b}$以及旋转和位置状态计算得出。 对于摄像机视场内的每一行视觉线 $\mathcal{L}_i$（第10-19行），在提取 $\{l_m\}$ 时会对其上 n 个点 $\mathcal{L}_i$ 进行采样（详见第IV-D节）。我们在梯度图像上以 $l_m$ 大小的矩形区域提取每个 $a \times 4a$ 的相邻像素（此操作记为 $\mathbf{D}^1$），从而形成线条块 $\mathbf{P}_m$（第13行）。如图4(b)所示，该矩形的长边会与线条方向对齐，以便覆盖更多具有不同梯度的像素。如图4(a)所示，这种方式还能让我们无需进行仿射变换，即可沿线条方向确定对应的参考块 $\mathbf{Q}_m$（第14行）。为了进一步提升块对齐效果，需采用缩放因子 $s_m$ 来描述当前帧与参考帧之间 $l_m$ 的深度比，进而对 $\mathbf{Q}_m$ 进行缩放。我们将每对 $\mathbf{P}_m$ 和 $\mathbf{Q}_m$ 之间的逐像素差异定义为 $\mathbf{r}_1$，并将其累加得到线条块残差 $\mathbf{r}_L$（第16行）。$\mathbf{r}_1$ 和 $\mathbf{r}_L$ 应当为零，其计算公式如式(5)、(6)所示： $$ 0=\mathbf{r}_{1}\left(\boldsymbol{x}_{k},\boldsymbol{l}_{m}\right)=\mathbf{D}^{1}\left(\boldsymbol{\pi}^{\left(C\right)}T_{W}\boldsymbol{l}_{m}\right))-\boldsymbol{s}_{m}\mathbf{Q}_{m} $$ $$ 0=\mathbf{r}_{\mathcal{L}}\left(\boldsymbol{x}_{k},L_{i}\right)=\sum_{m=1}^{n}\mathbf{r}_{1}\left(\boldsymbol{x}_{k},l_{m}\right) $$ 这两个残差会在三层梯度图像金字塔上被优化，从而更新状态，具体细节见第IV-C节。 ### C. 激光雷达观测与状态更新 我们的激光雷达地图采用了VoxelMap [^29]，用于在体素中存储一系列平面特征。给定真实状态 $\boldsymbol{x}_k$、激光雷达局部地图 $\mathcal{M}_L$ 以及下采样的激光雷达扫描数据 $\{\mathbf{p}_i\}$，我们按照 $\mathbf{r}_L(\boldsymbol{x}_k, \mathbf{p}_i)$ 中的方法搜索有效的点对面匹配关系，并计算激光雷达观测残差 [^29]。$\mathbf{r}_L$ 服从正态分布 $\mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_L)$。 将先验状态分布（式3）与激光雷达点和视觉特征的观测分布（式4、6）相结合，可得出 $\mathbf{x}_k$ 的最大后验估计，如式(7)所示： $$ \begin{aligned}\min_{\boldsymbol{x}_{k}\in\mathcal{M}}&\left(\left\|\boldsymbol{x}_{k}-\widehat{\boldsymbol{x}}_{k}\right\|_{\bar{P}_{k}}^{2}+\sum_{i=1}^{m_{L}}\left\|\mathbf{r}_{\mathbf{L}}\left(\boldsymbol{x}_{k},\mathbf{p}_{i}\right)\right\|_{\boldsymbol{\Sigma}_{\mathbf{L}}}^{2}\right.\\&\left.+\sum_{i=1}^{m_{p}}\left\|\mathbf{r}_{\mathcal{P}}\left(\boldsymbol{x}_{k},\mathcal{P}_{i}\right)\right\|_{\boldsymbol{\Sigma}_{\mathcal{P}}}^{2}+\sum_{i=1}^{m_{l}}\left\|\mathbf{r}_{\mathcal{L}}\left(\boldsymbol{x}_{k},\mathcal{L}_{i}\right)\right\|_{\boldsymbol{\Sigma}_{\mathcal{L}}}^{2}\right)\end{aligned} $$ 其中 $\|\boldsymbol{x}\|_{\hat{\boldsymbol{\Sigma}}}^{2} = \boldsymbol{x}^{T} \boldsymbol{\Sigma}^{-1} \boldsymbol{x}$。$\boldsymbol{\Sigma}_{\mathcal{P}}$ 和 $\boldsymbol{\Sigma}_{\mathcal{L}}$ 是两个预定义的权重因子，用于在优化过程中衡量 $\boldsymbol{r}_{\mathcal{P}}$ 和 $\boldsymbol{r}_{\mathcal{L}}$ 的重要性。$m_{L}$ 激光雷达 --- ```pseudo // Algorithm 2: LM-VDE: LiDAR Map Assisted Visual Features Depth Extraction. ``` 数据：激光雷达局部地图 $\mathcal{M}_L$、下采样的激光雷达扫描数据 $\mathbf{L}_k$、检测到的点 $\{p_i\}$ 和线条 $\{(e_1e_2)_i\}$ 结果：三维点 $\{\mathcal{P}_i\}$ 和三维线条 $\{\mathcal{L}_i\}$ 1 设 $e_1$、$e_2$ 为 $e_1e_2$ 的起点和终点； 2 将 $\mathbf{L}_k$ 投影到图像上； 3 —— 点深度提取 ——； 4 对 $\{p_i\}$ 中的每个点 p 执行以下操作： 5 在图像上搜索 $\mathbf{p}_L$ 中最接近的激光雷达扫描点 $\mathbf{L}_k$； 6 利用 $\mathbf{p}_L$ 从 $\pi_{\mathcal{P}}$ 中索引出对应的平面特征 $\mathcal{M}_L$； 7 在相机坐标系中：使投影线与 $\pi_{\mathcal{P}}$ 相交，从而求出深度 $\mathbf{z}$（式9）； 8 在相机坐标系中：利用 $\mathcal{P}^C$ 沿投影线恢复三维坐标 $\mathbf{z}$（式8）； 9 将 $\mathcal{P}^C$ 转换到世界坐标系下，得到 $\mathcal{P}$； 10 把 $\mathcal{P}$ 添加到 $\{\mathcal{P}_i\}$ 中； 11 结束循环 12 —— 线深度提取 ——； 13 对于$e_1e_2$中的每个$\{e_1e_{2_i}\}$执行如下操作 14 在$\{e_m\}$上采样n个点为$e_1e_2$； 15 对每个$e_m$执行如下操作 16 重复5到6次，索引为$\pi_{\mathcal{L}_m}$； 17 将$\pi_{\mathcal{L}_m}$添加到$\{\pi_{\mathcal{L}_m}\}$中； 18 结束 19 对$\{\pi_{\mathcal{L}_m}\}$执行RANSAC算法并拟合出$\pi_{\mathcal{L}}$； 20 对$e_1$、$e_1$重复执行7到9步，从而恢复它们在世界坐标系下的3D坐标$\mathbf{E}_1$、$\mathbf{E}_2$； 21 利用端点$\mathcal{L}$、$\mathbf{E}_1$对$\mathbf{E}_2$进行参数化； 22 将$\mathcal{L}$添加到$\{\mathcal{L}_i\}$中； 23 结束 点、视觉点$m_p$以及视觉线$m_l$被融合起来以优化状态$\mathbf{x}_k$。 这种优化通过迭代误差状态卡尔曼滤波器来求解，求解过程从分辨率最低的顶层金字塔开始，逐步向下进行。由于传播后的状态可作为视觉补丁对齐的初始位姿，因此除底层使用高分辨率图像进行两次迭代外，其他各层仅需一次迭代。每次迭代中，都会利用逐步优化的状态来计算激光雷达和视觉观测的残差，进而完成当前的优化。 ### D. LM-VDE与地图更新 局部视觉地图$\mathbf{M}_V$会动态更新：新进入图像的视觉特征会被加入地图，离开图像的特征则会被移除。新检测到的视觉特征的深度由我们提出的LM-VDE方法提取。该方法利用下采样的激光雷达扫描数据$\mathbf{L}_k$来索引局部激光雷达地图$\mathbf{M}_L$中的3D平面，随后将2D视觉特征映射到这些3D平面上。位于3D平面上的视觉点和线条具有稳定的背景，且在不同视角下能保持相对稳定的梯度分布，因此适合用来构建残差。我们在算法2中讨论了LM-VDE，并在后续段落中对其进行了详细说明。 对于每个检测到的点p（第4-11行），如图4(f)所示，可利用公式(8)沿投影线计算出其在相机坐标系下的3D坐标$\mathcal{P}^C$。深度z存在歧义，需要求解。首先，我们将$L_k$投影到图像上并进行最近邻搜索，从而得到最近的扫描点$\mathbf{p}_L$，如图4(c)–(d)所示。接着，利用$\mathbf{p}_L$在激光雷达地图$\mathcal{M}_L$中索引叶体素，如图4(e)所示（第5–6行）。之后，若该叶体素内所有点的最小特征值大于阈值，说明其中存储了符合条件的平面$\pi_P$，此时可依据公式(9)建立$\mathcal{P}$位于$\pi_P$上的关系来求解z（图4(f)及第7行）。最后，利用公式(8)恢复出$\mathcal{P}^C$，再通过$\mathcal{P}$将其转换到世界坐标系下的$\mathbf{x}_k$（第8–10行）。 $$ \mathcal{P}^{\mathrm{C}}=\begin{bmatrix}z\left(p_{x}-c_{x}\right)/f_{x}\\ z\left(p_{y}-c_{y}\right)/f_{y}\\ z\end{bmatrix} $$ $$ \left(^{W}\boldsymbol{T}_{C}\mathcal{P}^{\mathrm{C}}-\mathbf{c}\right)\cdot\mathbf{n}^{T}=0 $$ 符号$f_x$、$f_y$、$c_x$和$c_y$表示相机内参。c和n分别代表$\pi_P$的中心点和法向量。 点深度提取的方法被扩展用于恢复线的深度（13–23）。对于图像上检测到的每条线$e_1e_2$，其三维位置$\mathcal{L}$通过世界坐标系中的端点$E_1$和$E_2$来参数化。如图4(c-d)所示，我们在$\{e_m\}$上均匀采样n个二维点$e_1e_2$，并查找它们最近的激光雷达点。这些点被用来从$\{\pi_{\mathcal{L}_m}\}$中索引出n个相关平面$\mathcal{M}_L$（见图4(e)及第14–18行）。随后，对$\{\pi_{\mathcal{L}_m}\}$执行RANSAC算法，以拟合一个新的平面$\pi_\mathcal{L}$，预计$\mathcal{L}$会位于该平面上（见图4(f)及第19行）。与上一段落类似，我们可以通过公式(9)求解两个端点$e_1$和$e_2$的深度，并通过公式(8)恢复它们在相机中的三维坐标。最后，我们使用$x_k$将端点转换到世界坐标系，从而参数化$\mathcal{L}$（第20–22行）。此外，在$\{l_m\}$上采样n个具有最大Shi-Tomas分数的三维点$\mathcal{L}$，这些点将被用来构建基于Patch的线残差，详见第IV-B节。 利用更新后的状态$\boldsymbol{x}_k$，我们将下采样的激光雷达扫描数据$\mathbf{L}_k$重新转换到世界坐标系，并按照[^29]计算它们的协方差。然后，这些点被插入到激光雷达地图$\boldsymbol{M}_L$中，同时最旧的体素会被移除。 ### V. 实验 在本节中，我们将所提出的PL-LIVO与通用场景及具有挑战性的退化场景下的现有最先进里程计系统进行了比较。我们提出的方法LM-VDE在相对稀疏的16线旋转激光雷达传感器上的泛化能力也得到了验证。所有实验均在配备Intel i5-1135G7 CPU和16GB RAM的计算机上进行。此外，我们按照图5所示的平台在退化场景中采集了数据集，该平台包括一台Livox Avia激光雷达传感器（内置IMU）和一个工业相机。所有传感器均按照FAST-LIVO2的方式严格同步[^30]。我们经验性地设定视觉Patch模式的参数a为4像素，以便充分覆盖视觉特征附近的梯度分布。在状态优化过程中，用于加权基于Patch的视觉残差的两个因子$r_P$和$r_L$被预先定义为100。我们采用了三级梯度图像金字塔进行处理。 --- ![Image](images/a75b094a5c6d1aee970ac22f8c34e0dc6f610d49ccbb69ae7302be9b62acb47e.jpg)
Fig. 5. Our platform with two usage configurations: (a) handheld and (b) robot-mounted. The LiDAR and the camera are hard synchronized with homogenous PWM signals.
TABLEI
ABSOLUTE TRAJECTORY ERRORS (ATE, METERS) ON NTU-VIRAL SEQUENCES
FAST-LIOVoxelMapFAST-LIVOR3LIVEPL-LIVO
eee_010.0280.0230.0840.1470.022
eee_020.0210.0280.0790.1590.020
eee_030.0390.0400.1760.1200.027
nya_010.0270.0240.0790.1110.024
nya_020.0320.0290.0980.1100.024
nya_030.0380.0350.1090.1230.026
sbs_010.0410.0400.0990.0800.029
sbs_020.0340.0330.0970.1380.025
sbs_030.0340.0340.0880.1460.027
对每个对齐单元进行4次位姿优化，以确保收敛。 ### A. 在NTU VIRAL数据集上的精度评估 定量实验是在公共数据集NTU-VIRAL的9个序列上进行的[^12]，该数据集提供了由Leica激光跟踪仪捕获的轨迹真值。我们将所提出的PL-LIVO与几种开源的现有最先进里程计方法进行了比较，包括两种激光雷达-惯性里程计：FAST-LIO [^8]和VoxelMap [^29]；以及两种激光雷达-惯性-视觉里程计：R3LIVE [^10]和FAST-LIVO [^11]。为了公平比较，我们将室内场景（nya_xx）的下采样滤波器体素大小统一设为0.2米，室外场景（eee_xx和sbs_xx）设为0.5米。激光雷达地图的网格大小在室内和室外场景中分别统一设为0.4米和1.0米。各系统中的其他参数均设置为默认值。 我们记录了IMU坐标系的轨迹，并利用NTU-VIRAL提供的官方评估工具计算了绝对轨迹误差[^12]。结果如表I所示。PL-LIVO的激光雷达子系统与VoxelMap相同[^29]，由于采用了自适应的粗到细体素构建方式，其在9个序列中的6个上比FAST-LIO表现出更高的精度[^8]。出乎意料的是，两种激光雷达-惯性系统性能反而优于两种激光雷达-惯性-视觉系统。原因在于R3LIVE[^10]和FAST-LIVO[^11]会在每次接收到测量数据（激光雷达扫描或图像）时更新状态，因此一旦缺乏激光雷达的约束，图像模糊就会显著影响位姿估计结果。相比之下，得益于基于图像的数据对齐机制，PL-LIVO能够利用来自激光雷达和图像的紧密耦合约束来共同更新状态。此外，所提出的PL-DVO增强了系统的精度，因为视觉线特征提供了更有效的约束。最终，PL-LIVO在9个序列中的8个上实现了最高的精度。 ### B. 在退化数据集上的鲁棒性评估 我们在多个退化数据集上评估了PL-LIVO的鲁棒性，包括5个公开序列和6个我们自行采集的序列。所有测试序列的起点和终点均相同，因此我们采用起点到终点的漂移误差（第一个位姿与最后一个位姿之间的距离）来评估所有候选方法。结果列于表II中。 FAST-LIO[^8]和VoxelMap[^29]在所有序列上都表现出明显的漂移误差，这是因为激光雷达在退化方向上提供的约束不足。虽然FAST-LIVO[^11]和R3LIVE[^10]通过引入视觉点的约束缓解了性能退化现象，并在部分序列中减少了漂移，但它们在某些序列中仍会出现显著漂移。例如，在Livox Avia激光雷达传感器面对具有重复纹理的瓷砖墙时，R3LIVE[^10]在LiDAR_Degenerate序列中表现不佳。这是由于图像采样频率较低（10 Hz），导致连续帧之间存在较大差异，进而使光流跟踪陷入局部最小值，给位姿估计带来错误的视觉约束。FAST-LIVO[^11]在degenerate_00序列中的表现也不尽人意，该序列中激光雷达传感器面向地面且运动剧烈。这一问题的根源在于激光雷达传感器距离地面过近，导致视野受限，能够用于补充视觉特征的激光雷达投影点不足。另一个导致FAST-LIVO在degenerate_xx序列中性能受限的因素是这些序列中缺乏硬件同步机制。在我们采集的具有挑战性的数据集上，FAST-LIVO[^11]和R3LIVE[^10]同样表现欠佳，原因是相机采样频率较低（10 Hz），加上剧烈运动和显著的曝光变化。与这些方法相比，我们提出的PL-LIVO能够提取充足的视觉特征（点和线），并通过LM-VDE恢复其三维位置；此外，它还利用梯度图像金字塔构建了更稳健的视觉约束。最终，它在11个序列中的10个上实现了最高的精度。值得注意的是，其在所有测试序列中的起点到终点漂移误差均小于0.1米，而其他方法在不同序列中的漂移程度则波动较大。这充分体现了PL-LIVO在处理退化环境时的鲁棒性。 我们进行了消融实验，以验证所提出的基于图像块的梯度残差和视觉线特征的有益效果。如表II所示，我们测试了两种PL-LIVO变体：P-LIVO（仅使用视觉点）和L-LIVO（仅使用视觉线）。针对每种变体，我们分别采用了光照残差（亮度不变性）和所提出的梯度残差（梯度不变性）。结果表明，在11个序列中，有9个、10个和11个序列采用梯度残差的变体漂移量更小。由于视觉点比视觉线更常见且能提供更多视觉约束，在11个序列中有6个、7个序列中采用相同视觉残差的P-LIVO精度高于L-LIVO。由于互补约束的作用，PL-LIVO在所有序列上的表现都优于这两种变体。 ---
TABLE II START-TO-END DRIFT ERRORS (METERS) ON DEGENERATE SEQUENCES
Degenerate $_{{(*)}}$ TypeCamera FrequencyFAST-LIOVoxelMapFAST-LIVOR3LIVEProposed
Brightness InvarianceGradient Invariance
P-LIVOL-LIVOPL-LIVOP-LIVOL-LIVOPL-LIVO
degenerate_00L, V220Hz5.2824.9276.0620.076failfail3.0212.0707.3240.061
degenerate_01L, V220Hz8.847fail0.2270.1000.1980.3770.0810.9170.1990.078
degenerate_02L, V130Hzfailfailfail0.102failfailfail0.013fail0.010
LiDAR_DegenerateL10Hz4.7323.3320.0348.0370.0420.7450.0210.0130.0210.007
Visual_ChallengeL, V110Hz1.0230.0290.0430.0570.0430.0470.0310.0350.0420.029
corridor_01L10Hzfailfail7.8901.1660.3780.1170.0860.0990.0390.014
corridor_02L10Hzfail3.4991.122fail0.2425.9520.1780.0830.7800.017
stairs_01L, V210Hz2.1342.3123.8123.046fail2.6091.0470.1730.6820.046
stairs_02L, V210Hzfail4.3947.913fail9.1222.2101.3510.5220.3590.010
campus_01L10Hz2.539fail1.6553.9370.0810.1980.0410.1890.0450.024
campus_02L, V210Hz2.393fail2.9469.2982.962fail1.2580.3260.9200.053
$^{*}$ 其中L表示激光雷达退化，V1、V2分别表示摄像头面对白墙或经历显著曝光变化的情况。 最佳结果以粗体标记，次佳结果则加下划线标注。 $^{2}$ “fail”指漂移误差超过 $\geq$ 10米。 ![Image](images/cac08eb19272f0a5dd71bd98f316f0a03ef4745df61dfb7e345c69cd13a89849.jpg)
Fig. 6. Localization and mapping result of PL-LIVO under the degenerate_00 sequence. In this sequence, the LiDAR experienced degradation while oriented towards the ground. Due to the constraints provided by the gradient residuals of visual points and lines, PL-LIVO achieved stable localization and accurately returned to the origin.
视觉点和视觉线共同体现了额外线特征对位姿估计的益处。 图1表明，尽管stairs_01序列中存在显著的曝光变化、激光雷达退化和惯性测量单元波动，PL-LIVO仍实现了稳定的定位和高精度建图。图6展示了PL-LIVO在degenerate_00序列中的强大定位与建图能力，即便该序列存在严重的激光雷达退化和受限的视野。此外，图6(a)–(b)展示了梯度图像上的二维视觉线和点，而图6(c)则显示了它们对应的三维点和线。 ![Image](images/fcb00eb47e30570cdd88eb8e6fa7bab328e4dfd625145ec2b2f40015d178a3b0.jpg)
Fig. 7. Line map constructed by Huang et al. [^16] and LM-VDE.
### C. 线条深度提取方法评估 为了验证我们所提出的LM-VDE在不同激光雷达传感器上的泛化能力，我们将LM-VDE与Huang等人提出的深度提取方法[^16]进行了比较。由于Livox激光雷达扫描密度较高，LM-VDE和[^16]都能恢复大多数检测到的线条的深度。然而，当处理旋转式激光雷达产生的相对稀疏的扫描数据时，提取任务就更具挑战性。在此次评估中，我们在NTU-VIRAL [^12]的eee_01、nya_01和sbs_01序列上，可视化并对比了两种方法构建的线条图。图7所示的结果表明，LM-VDE恢复的线条数量多于[^16]。这一差异体现了LM-VDE对相对稀疏的激光雷达数据具有更强的适应性，凸显了其在不同激光雷达传感器上的泛化能力。 ### D. 时间消耗评估 在本节中，我们评估了完整PL-LIVO系统在我们收集的数据集上的平均处理时间，该数据集涵盖室内和室外环境。平均处理时间 ---
TABLE III
AVERAGE PROCESSING TIME (MILLISECONDS) ON OUR COLLECTED DATASET
ModulesProcessTime
PreprocessingImage Preprocessing11.0
LiDAR Scan Preprocessing1.2
IMU Propagation1.2
Data Association $^{(*)}$Visual Observation Model6.6
LiDAR Observation Model25.6
State Estimation $^{(*)}$IESKF Update5.6
Feature Generation and Map ManagerLM-VDE and Visual Map Update4.5
LiDAR Map Update1.6
PL-LIVO57.3
该模块在优化过程中会迭代4次，表中展示的是这4次迭代的总耗时。 PL-LIVO每个对齐单元的处理时间为57.3毫秒，而FAST-LIVO [^11]的处理时间则为49.3毫秒。这一额外的计算开销源于二维视觉特征的提取及其三维位置的重建。尽管如此，我们的系统仍能保持实时性能，并提升了在退化环境中的鲁棒性。我们在表III中进一步详细列出了PL-LIVO的平均处理时间。 ### VI. 结论 在这封信中，我们提出了一种点线激光雷达视觉惯性里程计（PL-LIVO），它在缓解激光雷达退化问题上的表现优于以往的方法。基于补丁的梯度优化技术提升了该方法应对曝光变化和镜头眩光的鲁棒性。我们还提出了一种激光雷达地图辅助的视觉特征深度提取（LM-VDE）方法，用于恢复视觉线条的三维坐标，从而能够更好地适应相对稀疏的激光雷达数据，同时还能提取出更多的线条特征。对比评估表明，PL-LIVO在一般环境中能达到更高的精度，在具有挑战性的退化场景中也具有更出色的鲁棒性。在实际应用中，PL-LIVO可用于在恶劣环境下工作的巡检机器人或手持测绘设备。不过值得一提的是，PL-LIVO需要相机与激光雷达之间具有高精度的校准结果。在未来的研究中，我们计划通过引入利用点和线特征的在线外参校准来拓展相关研究，从而打造出更便于应用的系统。 ## 参考文献 [^1]: J. Zhang and S. Singh, "LOAM: LiDAR odometry and mapping in real-time," in Proc. Robot.: Sci. Syst., Jul. 2014, vol. 2, pp. 1–9. [^2]: H. Shen, Q. Zong, B. Tian, and H. Lu, "Voxel-based localization and mapping for multirobot system in GPS-denied environments," IEEE Trans. Ind. Electron., vol. 69, no. 10, pp. 10333–10342, Oct. 2022. [^3]: Y. Fang, K. Qian, Y. Zhang, T. Shi, and H. Yu, “Segmented curved-voxel occupancy descriptor for dynamic-aware LiDAR odometry and mapping,” IEEE Trans. Geosci. Remote Sens., vol. 62, 2024, Art. no. 5701916. [^4]: M. Helmberger, K. Morin, B. Berner, N. Kumar, G. Cioffi, and D. Scaramuzza, “The hilti SLAM challenge dataset,” IEEE Robot. Automat. Lett., vol. 7, no. 3, pp. 7518–7525, Jul. 2022. [^5]: I. Filip, J. Pyo, M. Lee, and H. Joe, "LiDAR SLAM comparison in a featureless tunnel environment," in 2022 22nd Int. Conf. Control, Automat. Syst., Nov. 2022, pp. 1648–1653. [^6]: H. Li, B. Tian, H. Shen, and J. Lu, “An intensity-augmented LiDAR-inertial SLAM for solid-state LiDARs in degenerated environments,” IEEE Trans. Instrum. Meas., vol. 71, 2022, Art. no. 8503610. [^7]: T. Shan, B. Engl. J. Med. [^8]: W. Xu and F. Zhang, "FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter," IEEE Robot. Automat. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021. [^9]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled LiDAR-visual-inertial odometry via smoothing and mapping," in 2021 IEEE Int. Conf. Robot. Automat., Jun. 2021, pp. 5692–5698. [^10]: J. Lin and F. Zhang, “R $^{3}$LIVE: A robust, real-time, RGB-colored, LiDAR-inertial-visual tightly-coupled state estimation and mapping package,” in 2022 Int. Conf. Robot. Automat., May 2022, pp. 10672–10678. [^11]: 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 2022 IEEE/RSJ Int. Conf. Intell. Robots Syst., Oct. 2022, pp. 4003–4009. [^12]: T.-M. Nguyen, S. Yuan, M. Cao, Y. Lyu, T. H. Nguyen, and L. Xie, "NTU VIRAL: A visual-inertial-ranging-LiDAR dataset, from an aerial vehicle viewpoint," Int. J. Robot. Res., vol. 41, no. 3, pp. 270–280, 2022. [^13]: J. Lin, C. Zheng, W. Xu, and F. Zhang, “R $^{2}$ 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. [^14]: A. P. Gee and W. Mayol-Cuevas, “Real-time model-based SLAM using line segments,” in Proc. Int. Symp. Vis. Comput., 2006, pp. 354–363. [^15]: S. Lin, X. Zhang, Y. Liu, H. Wang, X. Zhang, and Y. Zhuang, "FLM PL-VIO: A robust monocular point-line visual-inertial odometry based on fast line matching," IEEE Trans. Ind. Electron., early access, Apr. 23, 2024, doi: 10.1109/TIE.2024.3379661. [^16]: S.-S. Huang, Z.-Y. Ma, T.-J. Mu, H. Fu, and S.-M. Hu, "LiDAR-monocular visual odometry using point and line features," in 2020 IEEE Int. Conf. Robot. Automat., Jun. 2020, pp. 1091–1097. [^17]: K. Cao et al., “Tightly-coupled LiDAR-visual SLAM based on geometric features for mobile agents,” in 2023 IEEE Int. Conf. Robot. Biomimetics, 2023, pp. 1–8. [^18]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular SLAM," in Proc. Eur. Conf. Comput. Vis., Sep. 2014, pp. 834–849. [^19]: C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in 2014 IEEE Int. Conf. Robot. Automat., 2014, pp. 15–22. [^20]: J. Jiang, J. Yuan, X. Zhang, and X. Zhang, "DVIO: An optimization-based tightly coupled direct visual-inertial odometry," IEEE Trans. Ind. Electron., vol. 68, no. 11, pp. 11212–11222, Nov. 2021. [^21]: Y.-S. Shin, Y. S. Park, and A. Kim, “DVL-SLAM: Sparse depth enhanced direct visual-LiDAR SLAM,” Auton. Robots, vol. 44, no. 2, pp. 115–130, Jan. 2020. [^22]: R. Gomez-Ojeda, J. Briales, and J. Gonzalez-Jimenez, "PL-SVO: Semi-direct monocular visual odometry by combining points and line segments," in 2016 IEEE/RSJ Int. Conf. Intell. Robots Syst., Oct. 2016, pp. 4211–4216. [^23]: B. Fang, Q. Pan, and H. Wang, “Direct monocular visual odometry based on LiDAR vision fusion*,” in 2023 WRC Symp. Adv. Robot. Automat., Aug. 2023, pp. 256–261. [^24]: P. Bergmann, R. Wang, and D. Cremers, “Online photometric calibration of auto exposure video for realtime visual odometry and SLAM,” IEEE Robot. Automat. Lett., vol. 3, no. 2, pp. 627–634, Apr. 2018. [^25]: Z. Feng, J. Li, L. Zhang, and C. Chen, “Online spatial and temporal calibration for monocular direct visual-inertial odometry,” Sensors, vol. 19, no. 10, May 2019, Art. no. 2273. [^26]: J. Shi and Tomasi, “Good features to track,” in 1994 Proc. IEEE Conf. Comput. Vis. Pattern Recognit., 1994, pp. 593–600. [^27]: I. Suárez, J. M. Buenaposada, and L. Baumela, “ELSED: Enhanced line segment drawing,” Pattern Recognit., vol. 127, Jul. 2022, Art. no. 108619. [^28]: J. Sola, “Quaternion kinematics for the error-state Kalman filter,” 2017, arXiv:1711.02508. [^29]: C. Yuan, W. Xu, X. Liu, X. Hong, 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. [^30]: C. Zheng et al., "FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry," 2024, arXiv:2408.14035.