# HVL-SLAM：用于SLAM的混合视觉与激光雷达融合技术 王伟、王晨杰 $^{ID}$、刘军 $^{ID}$、苏欣 $^{ID}$、罗斌 $^{ID}$ 以及张成 $^{ID}$ 摘要—在同步定位与地图构建（SLAM）领域，基于地图的定位技术已被广泛应用于自动驾驶场景，尤其是全速域及全道路自适应巡航、自动泊车等高级功能。因此，激光雷达传感器常被用于基于视觉的SLAM中，以提升自身运动估计和环境重建的整体精度。本文提出了一种新颖的紧耦合单目混合视觉激光雷达SLAM（HVL-SLAM）算法，该算法在跟踪和建图过程中同时利用视觉与激光雷达数据。首先，所提出的方法通过物体分割和Delaunay三角剖分来降低特征的三维不确定性；随后利用混合跟踪模块估计相邻帧间的运动，该模块可最小化光度误差与重投影误差；最后提出了一种用于优化位姿的联合优化方法，该方法将视觉与激光雷达数据以动态权重的形式纳入优化过程，从而提升定位精度和鲁棒性。在公开的KITTI里程计基准测试集及真实室外数据集上的实验表明，HVL-SLAM在位姿估计和建图性能方面均优于现有最先进的方法。相关代码已向社区开源，代码获取地址为 。 索引词—特征深度提取、混合位姿估计、视觉激光雷达位姿联合优化、视觉激光雷达同步定位与地图构建（SLAM）。 ## 一、引言 由于在自动驾驶、智能机器人及虚拟现实等领域的广泛应用，同步定位与地图构建（SLAM）一直是研究热点。根据所使用的传感器类型，现有的先进SLAM系统可分为三类：视觉SLAM、激光雷达SLAM以及多传感器融合SLAM。在SLAM中融合多种传感器（如激光雷达与视觉融合SLAM）能够弥补单一传感器的固有缺陷。本文重点探讨SLAM系统中视觉与激光雷达数据的融合问题。 传统的视觉激光雷达SLAM方法大致可分为深度增强型视觉方法和基于激光雷达配准的方法，它们通过结合另一传感器的互补信息来改进单传感器视觉里程计/SLAM系统。深度增强型视觉方法利用激光雷达提供的精确深度信息来增强视觉里程计流程，从而提升自身运动估计的精度，并降低对光照条件的依赖。基于视觉特征的方法，如DEMO [^1]、LIMO [^2]、CamVox [^3] 和 RGB-L [^4]，会将激光雷达确定的深度值赋予对应的视觉特征点。与基于视觉特征的方法不同，基于直接法的方法，如DVL-SLAM [^5] 以及 [^6] 中提出的方法，会在相机运动跟踪过程中直接使用激光雷达提供的深度值。基于激光雷达配准的方法 [^7]、[^8] 则利用图像信息在特定维度上对激光雷达SLAM进行补偿，从而提升系统在激光雷达退化、急转弯及剧烈运动等易出错场景下的鲁棒性。 这些视觉-LiDAR SLAM方法主要为单一的LiDAR或视觉SLAM框架提供额外信息以提升其性能，但它们并未充分利用所有传感器测量数据。例如，视觉增强方法仅选取与视觉特征对应的LiDAR点云，从而丢弃了大部分LiDAR信息；而基于LiDAR配准的方法要么在回环检测中不包含视觉测量数据，要么在视觉SLAM中不使用LiDAR测量数据。 为了最大化多传感器信息融合的效益，研究人员提出了紧耦合融合方法，以提高整体SLAM系统的精度和鲁棒性。TVLO [^9] 会独立利用两种不同传感器生成地图，分别计算LiDAR几何残差和立体投影残差，再将其结合以估计自我运动。一些研究 [^10]、[^11] 在运动估计和回环检测中结合了视觉与LiDAR测量数据，取得了出色的位姿估计结果。然而，这类基于立体的融合方法（如TVL-SLAM [^11]）中的立体相机需要较长的基线才能实现精确的深度估计，而这一要求在现实场景中往往难以满足。此外，两台相机之间的校准易受机械变动影响，进而降低深度估计精度。因此，我们打算通过紧密整合单目相机与3D LiDAR来进行自我运动估计。 --- 作为紧耦合方法中的关键组成部分，众多深度补全技术 [^2]、[^4]、[^10]、[^12] 被开发出来，用于准确确定视觉特征的深度值。在这些方法中，每个特征点的深度先验是通过相邻区域来单独估算的。具体而言，首先会将区域内属于激光点云的点划分为前景点和背景点；随后利用前景点拟合平面，再通过特征点光线与平面相交来得到该特征点的深度。不过，这类方法受限于区域大小，且激光点云的密度对深度拟合结果有显著影响。如果点云较为稀疏，前景点与背景点的划分以及空间平面拟合的结果就会具有很大的不确定性。 除了建图问题外，基于所生成地图的定位是SLAM系统的重要组成部分，尤其是在无GPS环境或GPS无法提供精确定位结果的情况下。大多数仅使用LiDAR地图的定位方法 [^13]、[^14]、[^15] 在结构重复的场景中（如城市峡谷、隧道、地下停车场等）很容易出现定位偏差。此外，这些方法还需要初始全局定位信息，但由于LiDAR距离数据中缺乏显著特征，这一信息仍难以获取。 相比之下，基于视觉的方法在初始全局定位方面表现出更高的效率和性能，这得益于其丰富的角度分辨率和丰富的色彩信息，例如NetVlad [^16]、DBoW [^17] 和HF-Net [^18]。即便在最恶劣的条件下，HF-Net [^18] 也能借助全局视觉地图实现无比快速、稳健且精准的定位性能。不过，视觉地图是离线生成的，耗时较长；而且由于单目相机无法获取绝对深度尺度，其尺度可能无法参照绝对尺度。 为此，我们在本文中提出了一种融合映射系统，该系统将视觉传感器与激光雷达传感器紧密集成，无需GPS或地理标记数据库即可实现精确的位姿估计和高效的机器人定位。以下是该系统的主要贡献。 1) 基于点云物体分割与Delaunay三角形算法，提出了一种高效且鲁棒的特征点深度提取方法（详见第III-B节）。该方法无需复杂的参数设置，即便在激光雷达测距数据有限的情况下也能良好运行。 2) 提出了一种混合视觉-激光雷达融合跟踪模块（详见第III-C节），以提升自我运动估计的鲁棒性，尤其是在退化场景中。 3) 设计了一种视觉-激光雷达联合优化方法（详见第III-D节），该方法采用源自视觉与激光雷达测量的动态权重。通过非线性最小二乘误差来保持局部精度，同时动态调整不同因素产生的残差权重。 4) 我们设计了一种快速生成包含两种传感器特征点的视觉-激光雷达融合地图的方法，并通过重定位任务验证了其卓越性能。 ![Image](images/841eb7d207c0489e46bb85ea7180179412768d16bf512c24d53995ed444de5c0.jpg)
Fig. 1. Mapping results on large outdoor environment. (a) Fusion map. (b) LiDAR feature map, where colors are rendered by the altitude of points. (c) Map aligned with Google map. More details can be seen in > 图1展示了利用混合视觉-激光雷达SLAM（HVL-SLAM）构建融合地图的示例。 ### II. 相关工作 当前先进的视觉-激光雷达里程计研究可分为三类：深度增强型视觉方法、基于激光雷达配准的方法以及紧耦合融合方法。 ### A. 深度增强型视觉方法 视觉增强方法旨在利用已知激光扫描特征深度数据的视觉里程计流程。例如，DEMO [^1] 通过将激光雷达的深度测量值赋予视觉特征，来提升视觉里程计的精度。后续的研究，如LIMO [^2]、CamVox [^3] 和 PLVL-SLAM [^12]，均采用束调整技术来实现更高的运动估计精度。与这些将基于特征的视觉SLAM与相机跟踪相结合的方法不同，DVL-SLAM [^5] 采用的是直接视觉SLAM。它们将投影的激光点作为特征点，而非从图像中提取的显著梯度点。随后，利用这些特征点已知的深度值，执行类似DSO [^19] 的多帧光度优化，以估计关键帧的位姿。[^6] 中提出的方法基于光度图像对齐技术，采用了直接的激光-视觉里程计方案。该方法利用平面信息预测遮挡情况，并通过两阶段配准获得精确的逐帧自我运动估计。不过，深度增强型视觉方法存在一个共同问题：它们无法处理位于相机视野之外的激光点。像Velodyne激光雷达这类能够提供 $360^{\circ}$ 扫描范围的传感器，其大部分测距数据会被忽略。在这种情况下，系统的精度可能会降低，且更容易受到无纹理环境的影响。 ### B. 基于激光雷达配准的方法 与基于深度增强的视觉方法不同，基于激光雷达配准的方法试图利用图像信息在多个方面对齐整个点云。例如，Zhang等人提出了V-LOAM [^7]，该方法仅将视觉里程计结果作为迭代最近点（ICP）过程的初始猜测，从而降低ICP陷入局部最小值的可能性。近期研究表明，冗余的里程计数据能够显著提升自我运动估计的鲁棒性。Reinke等人 [^8] 提出了一种集成的视觉-激光雷达里程计系统。 --- 该系统并行运用多种里程计算法。通过合理性检查以及基于倒角距离的评分机制来选出最具潜力的估计结果。此外，图像既可用于回环闭合检测，也能辅助激光雷达进行初始运动计算。例如，Park等人 [^20] 提出了一种名为ElasticLiDAR++的新型框架，用于多模态以地图为中心的SLAM。其回环闭合检测模块会持续从视觉传感器中提取二维特征，并将其与先前生成的关键帧进行对比。 ### C. 紧耦合融合方法 视觉驱动的方法仅利用少量激光雷达点云来增强视觉特征，而忽略了大部分激光雷达测量数据。为了探究各传感器的优势并弥补彼此的不足，TVLO [^9] 同时采集视觉和激光雷达数据，在姿态优化阶段将两种模态的残差叠加并最小化，从而避免了将激光雷达深度值分配给不对应的视觉特征这一潜在问题。随后，TVL-SLAM [^11] 将该方法扩展为一套SLAM系统，在运动估计、回环检测、回环闭合以及外参标定中均融入视觉和激光雷达数据，最终取得了出色的姿态估计效果。TVL-SLAM是基于双目相机的方法，而我们采用的是单目相机。在前端，视觉和激光雷达传感器独立运行；而在后端优化中，所有视觉和激光雷达测量数据都会被整合起来。上述系统在估计传感器姿态时通常具有较高精度。不过，在现实世界的移动机器人应用中，这些系统的实时性能并不尽如人意，尤其是在移动机器人的计算资源有限时。 何等人[^10]提出了一种协作建图系统，该系统结合了视觉激光雷达里程计以实现鲁棒且精确的位姿估计，同时采用基于位姿图的协作SLAM流程来生成高质量的建图结果。最近，王等人[^21]提出了DV-LOAM，这是一种直接的视觉-激光雷达融合框架。该系统首先通过两阶段的直接视觉里程计模块进行高效的粗略状态估计，随后利用激光雷达建图模块优化粗略位姿，最后借助闭环检测模块修正累积的漂移误差。袁等人[^22]提出了SDV-LOAM，该系统将半直接式激光雷达辅助的深度增强视觉里程计与激光雷达里程计相整合，从而实现精确且鲁棒的位姿估计与建图。除了点特征外，一些紧耦合融合方法[^23]、[^24]通过融合多种类型的特征来提高精度。然而，大多数现有的紧耦合融合方法都依赖激光雷达点云投影来建立点或表面的对应关系，进而将激光雷达深度信息赋予视觉特征点。由于点云的稀疏性、拟合误差以及前景与背景之间的显著深度差异，当前方法中深度增强视觉特征的有效性和效率受到限制。此外，在后端优化过程中，大多数现有方法在最小二乘优化中采用固定的视觉点与激光雷达点权重，它们并未考虑在计算过程中自适应调整这些观测值的权重，以动态适应不同场景从而获得更佳性能。针对上述问题，我们提出了一种基于物体分割与Delaunay三角剖分的深度拟合方法，以实现特征点的快速且鲁棒的深度提取。另外，在联合优化阶段引入了自适应加权模块来调整视觉与激光雷达联合优化的权重，这不仅提高了位姿精度，还优化了视觉特征图。 ### 三、方法 ### A. 概述 每一帧$\mathcal{F}_k$由其对应的图像$\mathcal{I}_k$、激光雷达点云$\mathbf{P}_k$以及视觉点特征$\mathbf{F}_k$组成。我们假设图像与激光雷达点云是时间同步的，且它们之间的外部参数$\mathbf{T}_{\text{LiDAR}}^{\text{Cam}}$已知。因此，我们可以利用公式(1)将激光雷达点投影到图像空间坐标系中。 $$ \mathbf{P}_{k}^{C}=\mathbf{T}_{\mathrm{L i D A R}}^{\mathrm{C a m}}\mathbf{P}_{k}^{L}. $$ 此外，对于一个点$P_i \in \mathbf{P}_k^C$，我们可以利用相机投影函数$\pi(\cdot)$将其投影到图像平面上。 $$ \begin{aligned}\boldsymbol{p}_{i}&=\begin{bmatrix}u_{i}\\ v_{i}\end{bmatrix}=\pi(\boldsymbol{P}_{i})=\frac{1}{Z_{i}}\times\boldsymbol{K}\times\boldsymbol{P}_{i}\\&=\frac{1}{Z_{i}}\left[\begin{array}{ccc}f x&0&c x\\ 0&f y&c y\\ 0&0&1\end{array}\right]\left[\begin{array}{c}X_{i}\\ Y_{i}\\ Z_{i}\end{array}\right]\end{aligned} $$ 其中$\{f_{x}, f_{y}, c_{x}, c_{y}\}$为相机的内部参数。 每一帧的位姿$\mathcal{F}$由矩阵$\mathbf{T}_{\mathrm{c}(\mathrm{amera})_{\mathrm{w}(\mathrm{world})}} \in \mathrm{SE}(3)$表示。我们将相机的局部坐标设为机体坐标，世界坐标则作为机体坐标的起点。因此，$\mathcal{F}_m$与$\mathcal{F}_n$之间的相对位姿变换可表示为： $$ \mathbf{T}_{m}^{n}=\mathbf{T}_{c w}^{n}\mathbf{T}_{c w}^{m\mathrm{~-~}1} $$ 该变换也可用于不同坐标系下点的坐标转换，如下所示： $$ \begin{aligned}\boldsymbol{P}_{n}&=\mathbf{T}_{m}^{n}\boldsymbol{P}_{m}=\exp\left(\hat{\boldsymbol{\xi}}\right)\boldsymbol{P}_{m}\\&=\left[\begin{array}{cc}\mathbf{R}_{m}^{n}&\boldsymbol{t}_{m}^{n}\\\mathbf{0}^{T}&1\end{array}\right]\boldsymbol{P}_{m}\end{aligned} $$ 其中$P_m$和$P_n$为各帧中的点，$R_m^n \in \text{SO}(3)$和$t_m^n \in \mathbb{R}^3$分别为旋转矩阵和平移向量。李代数$\xi = \begin{bmatrix} \phi \\ \rho \end{bmatrix} \in \mathbb{R}^6$、$\phi$及$\rho$则是除以时间间隔后的角速度和线速度向量。 图2展示了我们方法的框架，该框架是基于ORB-SLAM2开发的。我们的系统包含三个运行线程：帧间跟踪线程、视觉-LiDAR联合优化线程以及回环检测线程。跟踪模块首先会在每一帧中提取视觉特征点。随后，采用混合视觉LiDAR跟踪方法（第三节C部分）来估计相邻帧之间的自身运动。一旦生成新的关键帧，便利用所提出的LSDT深度拟合算法（第三节B部分）获取特征的深度先验，再通过视觉LiDAR联合优化方法（第三节D部分）对位姿进行精细化处理。之后，会执行基于词袋模型（BoW）的回环检测以识别回环现象。最后，通过位姿图优化过程来维持全局地图的一致性。 --- ![Image](images/3ea3823df058f776c9b8042a9ea9d8f27326e39f16f1e785f7091aec089c31aa.jpg)
Fig. 2. Overview of the proposed HVL-SLAM method. The components highlighted in green are our main contributions in this work.
![Image](images/b36354f21e8380759529180c9799b307e1268a372b79f4bd5da074c29ba28362.jpg)
Fig. 3. Illustration of point feature depth extraction. (a) Extracted ORB features. (c) Is LiDAR segmentation result, each color represents an object cluster, while (b) shows the image projection result of object clusters. (d) Object Delaunay triangles generated by our method. Note that ground laser points are not drawn for better visualization.
跟踪方法（第III-C节）用于估算相邻帧之间的自我运动。一旦生成新的关键帧，便利用所提出的LSDT深度拟合算法（第III-B节）获取特征点的深度先验，随后通过视觉与激光雷达联合优化方法（第III-D节）来细化姿态。之后，会执行基于词袋模型（BoW）的回环检测，以识别回环现象。最后，进行位姿图优化处理，从而确保全局地图的一致性。 ### B. LSDT深度拟合 在本节中，我们将介绍基于物体分割与Delaunay三角剖分从激光雷达数据中提取点深度的拟合方法，如图3所示。 首先按照ORB-SLAM2中的方法提取定向FAST和旋转BRIEF（ORB）特征[^25]。随后，地面提取方法[^26]会将原始的激光雷达点云划分为地面部分和非地面部分。非地面点云会进一步通过基于距离图像的分割方法[^27]进行处理，从而形成不同的物体点簇。剔除较小的点簇后，同一簇内的点会被赋予唯一的物体标签。最后一步是为每个物体标签对应的点以及地面点云构建Delaunay三角剖分，如图3(d)所示。三角剖分结果在拟合特征点的深度时起着关键作用，深度值由光线与对应三角形平面的交点确定。算法1总结了整个深度拟合算法。 与上述方法[^2]、[^12]不同，我们假设同一物体上的深度是连续的。基于这一假设，我们为每个物体构建Delaunay三角剖分，以拟合位于该物体上的特征点的深度值。由于无需进行前景与背景分割或平面拟合， ```pseudo // Algorithm 1: LSDT Feature Depth Extraction Algorithm Require: LiDAR point cloud P; ORB feature points set ``` $\mathbf{F} = \{p_1, p_2, \ldots, p_n\}$；相机内参$\mathbf{K}$；相机与激光雷达之间的外部参数$\mathbf{T}_{LiDAR}^{Cam}$。 确保：ORB特征的深度$\{d_1, d_2, \ldots, d_n\}$。 // (1) 激光雷达分割 利用$\mathbf{P}$将[^28]划分为地面点云和非地面点云。 非地面点云通过$\{\mathbf{S}_1, \mathbf{S}_2, \ldots, \mathbf{S}_m\}$被分割成多个簇[^27]。 // (2) 基于2.5D Delaunay三角形生成物体（2.5D指的是像素坐标(u, v)和深度值d）。 for i = 1 to m do For each LiDAR point $P_k \in \mathbf{S}_i$, project $P_k$ into image plane using (1) and (2); Construct Delaunay triangle vertexes $\mathbf{v}_k = \{u_k, v_k, d_k, label = i\}$, and insert into vertex set $\mathcal{V}$; end for Generate Delaunay triangles DT using vertexes $\mathcal{V}$ // (3) Extract the depth via the generated DT. for each ORB feature point $p_i \in \mathbf{P}^V$ do if locate(DT, $p_i$) then Interpolate the depth $d_i$ of feature $p_i$ using the triangle via (5). end if end for 由于无需额外的拟合步骤，我们提出的特征深度拟合方法效率非常高，其典型运行时间仅为10毫秒。此外，基于距离图像的点云分割考虑了物体在空间中的连续性，因此比局部前景与背景分割更为稳健。即便使用VLP-16这类稀疏激光传感器，我们的深度拟合方法也能提供更精确的深度值。 --- ![Image](images/e4adb54b201d0708ac85217543db9f2c8e9a68f279a6b4e617cd33be39fc5ada.jpg)
Fig. 4. Illustration of feature depth fitting process.
该方法尤其能在物体边缘处提供更准确的深度值。 相应特征点 $d_{i}$ 的深度 $p_{i} = [u_{i} v_{i}]^{T}$ 可通过以下公式计算得出： $$ d_{i}=\frac{n_{1}X_{1}+n_{2}Y_{1}+n_{3}Z_{1}}{n_{1}(u_{i}-c_{x})/f_{x}+n_{2}\left(v_{i}-c_{y}\right)/f_{y}+n_{3}} $$ 其中 $\mathbf{n} = [n_1 \ n_2 \ n_3]^T$ 是空间平面的法向量，它是通过特征点射线所穿过的空间三角形的三个顶点计算得出的。$P_1 = [X_1 \ Y_1 \ Z_1]^T$ 则是图4中所展示的空间三角形的顶点。 ### C. 混合视觉激光雷达帧间跟踪 我们设计了混合视觉激光雷达帧间跟踪模块，以确保精确且快速的自我运动估计。该模块将重要的激光雷达点与视觉特征点相结合，以减少光度误差和重投影误差。它通过将显著的激光雷达点与视觉特征点融合，来优化光度误差和重投影误差。视觉特征会受到纹理的影响，而激光雷达点则会在场景质量较差时受到影响。这两类传感器数据的结合能够弥补各自的缺陷，从而实现稳健的跟踪效果。同时，由于结合了光度误差和几何重投影误差，我们的方法兼具直接法和基于特征的方法的优势。两种代价残差——光度残差和几何残差的具体细节将在下文详述。 1) 光度残差：我们仅针对具有深度的像素最小化光度误差，这些像素分为两类：选定的显著激光雷达点和ORB特征点。需要注意的是，在此步骤中，相机视野外的激光雷达点会被剔除。不过在后续的视觉-激光雷达联合优化过程中，当前帧中的所有激光点都会被用于优化位姿。图像块中的每个点都会被赋予该块中心特征点的相同深度。图5(a)分别用棕色三角形和红/蓝色圆圈展示了这两类像素的示例。对于帧 $p_k$ 中深度为 $d_k$ 的点 $\mathcal{F}_{m-1}$，其光度误差定义如下： $$ r(p_{k})=\mathcal{I}_{m-1}(p_{k})-a\mathcal{I}_{m}\big(\pi\big(\mathbf{T}_{m-1}^{m}\pi^{-1}(p_{k},d_{k})\big)\big)+b $$ 其中a和b分别为当前图像的增益和亮度。最终，我们的跟踪光度目标函数 ![Image](images/670be0523cfaf738e429cdeff3735b56918b8359b5c828c467daeb321b361eb3.jpg)
(a)
(c)
Fig. 5. (a) Illustration of the hybrid frame-to-frame tracking process, red points, blue points, and green points represent visual feature points, and brown triangle points are salient laser points. (b) Patch pattern that is used to calculate photometric residual. (c) Geometric reprojection residual.
定义如下： $$ \mathbb{E}_{\mathrm{pho}}\stackrel{\mathrm{def}}{=}\sum_{i=1}^{N}\sum_{p_{j}\in\Omega(p_{i})}w\big(r\big(p_{j}\big)\big)\big(r\big(p_{j}\big)\big)^{2} $$ 其中 $\Omega(\cdot)$ 是图5(b)中所示的稀疏模式图像块，基于t分布的权重函数 $w(\cdot)$ 定义如下： $$ w(r)\stackrel{\mathrm{d e f}}{=}\frac{\nu+1}{\nu+\left(\frac{r}{\delta r}\right)^{2}}. $$ 此处，自由度 $\nu$ 被设为5，而在执行高斯-牛顿优化时，每次迭代都会得到残差方差 $\delta r$。 2) 几何残差：对于每一帧新传入的图像 $\mathcal{F}_m$，首先会将图像特征与上一帧 $\mathcal{F}_{m-1}$ 进行关联。记 $p_i$ 和 $p_j$ 分别为上一帧和当前帧中匹配到的对应特征。因此，重投影误差可定义如下： $$ r(p_{i})=p_{j}-\pi\left(\mathbf{T}_{m-1}^{m}\boldsymbol{\pi}^{-1}(p_{i},d_{i})\right) $$ 其中 $d_{i}$ 是 $p_{i}$ 的深度，该值会在束调整模块中被优化。最终，我们的基于特征的重投影几何误差跟踪目标函数定义如下： $$ \mathbb{E}_{\mathrm{geo}}\stackrel{\mathrm{def}}{=}\sum_{i=1}^{N}w(r(p_{i}))(r(p_{i}))^{2}. $$ 我们使用的权重函数 $w(\cdot)$ 是ORB_SLAM2中采用的Huber核 [^29] $$ w(r)\stackrel{\mathrm{d e f}}{=}\left\{\begin{array}{l l}{1,}&{\mathrm{i f}rFig. 6. Graph structure for visual-LiDAR joint optimization.
在求解完非线性优化问题12之后，当前帧的相机位姿$\mathbf{T}_m$会立即被更新。随后，三维地图点会被重新投影到当前帧中，以判断该三维到二维的匹配是否为异常值。一旦检测到异常值，便会将其移除。 ### D. 视觉与激光雷达联合定位建图 由于基于扫描到地图的位姿优化是减少累积误差并提供更精确估计位姿的重要方法，它已被广泛应用于最先进的激光雷达系统中[^30]、[^31]、[^32]、[^33]。然而，在隧道或走廊等可见结构较少的退化环境中，这些方法仍容易失效。与前述基于激光雷达的方法[^2]、[^3]、[^12]相比，基于深度增强视觉特征的方法展现出优势，尤其是在可获得较高分辨率测距数据时，其在旋转估计方面表现更佳。原因可分为以下几类：首先，视觉特征的深度是直接由精确的激光雷达扫描生成的，而非利用先前估计的运动信息进行三角测量，从而降低了深度估计的不确定性；接着，视觉地图点的位置会通过束调整技术进行优化；最后，由于图像传感器提供了丰富的纹理信息，特征提取可达到亚像素级别，且特征匹配的准确性和鲁棒性也优于激光雷达特征关联。 因此，我们的核心思路是将视觉重投影因子与激光雷达扫描到地图因子相融合，以优化当前关键帧及局部视觉地图点的位姿。对于每一个传入的关键帧$\mathcal{H} \mathcal{F}_k$，会将当前关键帧的图像特征$\mathcal{F}_k$与局部视觉地图进行匹配。令$\mathcal{X}_k$表示时间k时所有匹配的特征。此外，同时根据计算出的局部曲率提取边缘点$\mathcal{E}_k$、平面点$\mathcal{P}_k$以及地面点$\mathcal{G}_k$，特征提取的详细内容可参见[^34]。需要注意的是，激光雷达特征点$\{\mathcal{E}_k, \mathcal{P}_k, \mathcal{G}_k\}$已利用上一节中获得的相对变换$T_{m-1}^m$进行了去畸变处理。我们会对单次扫描内的点进行线性插值并修正畸变，关于畸变的详细信息可参见[^33]。 图6展示了视觉与激光雷达联合优化的示意图。最终，通过求解下述非线性最小二乘问题即可估算出当前关键帧的位姿： $$ \min_{R,t}\frac{1}{\delta_{v}^{2}}\sum_{X_{k}^{i}\in\mathcal{F}_{k}}\rho\underbrace{\left(\|d_{\mathcal{R}}\left(X_{k}^{i}\right)\|^{2}\right)}_{visual\ constraints}+\frac{1}{\delta_{e}^{2}}\sum_{E_{k}^{j}\in\mathcal{E}_{k}}\rho\underbrace{\left(\|w_{j}\cdot d_{\mathcal{E}}\left(E_{k}^{j}\right)\|^{2}\right)}_{lidar\ edge\ constraints} $$ ![Image](images/14e4dbe8c667d622a175cecb06a111bc01e58cd594e93470411a321b320ec15a.jpg)
Fig. 7. Illustration of visual-LiDAR joint optimization error terms. (a) Visual constraints. (b) LiDAR edge constraints. (c) LiDAR plan constraints. (d) LiDAR ground constraints.
$$ \begin{array}{r l}{+}&{{}\frac{1}{\delta_{p}^{2}}\displaystyle\sum_{P_{k}^{m}\in\mathcal{P}_{k}}\rho\underbrace{\left(\|w_{m}\cdot d_{\mathcal{P}}\big(P_{k}^{m}\big)\|^{2}\right)}_{\mathrm{l i d a r~p l a n a r~c o n s t r a i n t s}}}\\ {+}&{{}\frac{1}{\delta_{g}^{2}}\displaystyle\sum_{G_{k}^{n}\in\mathcal{G}_{k}}\rho\underbrace{\left(\|w_{n}\cdot d_{\mathcal{P}}\big(G_{k}^{n}\big)\|^{2}\right)}_{\mathrm{l i d a r~g r o u n d~c o n s t r a i n t s}}}\end{array} $$ 其中$\rho(\cdot)$为Huber范数，$d_{\mathcal{R}}(\cdot)$代表视觉重投影误差，$d_{\mathcal{E}}(\cdot)$和$d_{\mathcal{P}}(\cdot)$分别为点到线误差和点到面误差。 1) 视觉约束：对于当前关键帧中的每个特征点$X_k^i \in \mathcal{F}_k$，视觉约束如式(13)所定义，如图7(a)所示。 $$ d_{\mathcal{R}}\big(X_{k}^{i}\big)=X_{k}^{i}-\pi\left(\mathbf{T}_{W}^{k}\times P_{j}\right) $$ 其中$P_j \in \mathbb{R}^3$是属于局部视觉地图的三维特征点，$\mathbf{T}_W^k$则是从第三节中获得的初始位姿。 2) 激光雷达边缘约束：对于每一个激光雷达边缘点 $E_k^j \in \mathcal{E}_k$，我们按照图7(b)所示，从 $\mathbb{E}_m$ 中找到五个最近的点。需要注意的是，点 $E_k^j$ 位于局部激光雷达坐标系中，而 $\mathbb{E}_m$ 中的局部激光雷达地图点则注册在全局坐标系中。为了找到 $E_k^j$ 在 $\mathbb{E}_m$ 中的最近点，需先利用 $E_k^j$ 将 $p_w = \mathbf{T}_W \times E_k^j$ 投影到全局坐标系。随后，激光雷达边缘约束的残差可定义如下： $$ d_{\mathcal{E}}\Big(E_{k}^{j}\Big)=\frac{|(p_{w}-p5)\times(p_{w}-p_{1})|}{|p_{5}-p_{1}|}. $$ 3) 激光雷达平面约束：对于当前关键帧中的每一个平面激光雷达点 $P_k^m \in \mathcal{P}_k$，我们同样按照图7(c)所示，在局部平面激光雷达地图 $\mathbb{P}_m$ 中找到五个最近的点。激光雷达平面约束的定义如下式所示： $$ d_{\mathcal{P}}\big(P_{k}^{m}\big)=\frac{(p_{w}-p_{1})^{T}((p_{3}-p_{5})\times(p_{3}-p_{1}))}{|(p_{3}-p_{5})\times(p_{3}-p_{1})|} $$ 其中 $p_w = T_W^k \times P_k^m$ ---
TABLE I
RELATIVE TRANSLATION (%)/ROTATION (DEG/100M) ERROR COMPARISON OF THE PROPOSED HVL-SLAM WITH EXISTING METHODS ON SEQUENCES 00–10 OF THE KITTI DATASET
Sequence (Total length [m])00 (3724)01 (2453)02 (5067)03 (560)04 (393)05 (2205)06 (1232)07 (694)08 (3222)09 (1705)10 (919)avg
Ours $^{{(*)}}$0.64/0.221.74/0.390.83/0.210.81/0.390.34/0.320.36/0.160.34/0.160.45/0.241.07/0.340.72/0.240.70/0.280.72/0.27
GAC-Mapping $^{{(*)}}$1.68/0.7815.22/0.871.98/0.771.94/1.102.74/1.101.13/0.521.19/0.580.94/0.671.69/0.871.56/0.803.14/1.373.02/0.85
DV-LOAM $^{{(*)}}$0.66/0.301.74/0.430.84/0.360.85/0.480.39/0.620.54/0.300.70/0.330.42/0.220.93/0.270.75/0.270.87/0.470.78/0.36
CamVox $^{{(*)}}$0.77/0.35$\times$0.89/0.340.84/0.310.70/0.710.59/0.340.89/0.480.44/0.312.24/0.830.79/0.370.76/0.37$\times$
RGB-L $^{{(*)}}$0.66/0.283.81/1.170.84/0.318.15/0.331.72/0.150.44/0.210.79/0.440.63/0.291.20/0.370.79/0.270.71/0.341.79/0.38
Ours $^{{-()}}$0.65/0.271.74/0.390.79/0.230.81/0.390.34/0.320.46/0.200.54/0.220.53/0.291.02/0.290.64/0.270.70/0.280.75/0.29
GAC-Mapping $^{{-()}}$2.05/0.8815.22/0.872.08/0.791.94/1.102.74/1.041.47/0.731.06/0.540.88/0.781.84/0.941.56/0.803.14/1.373.09/0.89
PLVL-SLAM[^20](-)0.99/#1.87/#1.38/#0.65/#0.42/#0.72/#0.61/#0.56/#1.27/#1.06/#0.83/#0.94/0.36
CamVox(-)1.46/0.61-1.62/0.530.84/0.310.70/0.710.62/0.371.04/0.600.61/0.382.24/0.830.70/0.330.76/0.371.06/0.50
RGB-L(-)0.89/0.383.83/1.240.89/0.321.00/0.331.72/0.150.55/0.250.71/0.290.85/0.401.19/0.380.69/0.290.76/0.331.19/0.40
LIMO[^2](-)1.12/#0.91/###0.53/#######0.93/0.26
DEMO[^1](-)1.05/#1.87/#0.93/#0.99/#1.23/#1.04/#0.96/#1.16/#1.24/#1.17/#1.14/#1.14/0.49
DVL-SLAM[^5](-)0.93/#1.47/#1.11/#0.92/#0.67/#0.82/#0.92/#1.26/#1.32/#0.66/#0.70/#0.98/0.40
DV-LOAM(-)0.65/0.301.74/0.430.97/0.330.85/0.480.39/0.420.54/0.300.65/0.330.51/0.330.89/0.320.73/0.320.87/0.470.80/0.38
SDV-LOAM(-)0.66/0.3011.4/0.270.71/0.230.87/0.200.60/0.520.66/0.300.50/0.270.84/0.531.09/0.370.63/0.340.68/0.411.69/0.34
A-LOAM(-)0.87/0.362.64/0.554.88/1.551.20/0.631.23/0.400.66/0.300.62/0.280.58/0.431.18/0.431.10/0.451.46/0.531.49/0.53
F-LOAM(-)0.78/0.372.64/0.561.43/0.451.21/0.621.20/0.390.65/0.310.65/0.370.54/0.411.11/0.431.05/0.441.46/0.491.16/0.44
T-LOAM(-)2.55/1.493.43/0.741.99/1.471.80/1.471.67/1.431.24/0.980.89/0.491.69/1.712.41/1.311.37/1.162.26/1.051.94/1.21
最佳平移/旋转误差用红色标出，次佳误差则用蓝色标出。(*)表示使用了回环闭合机制，(-)则表示未使用回环闭合机制。#表示该结果在原文中并未给出。×表示运行失败。 4) 激光雷达地面约束：与平面约束类似，激光雷达地面约束如图7(d)所示，其定义如下： $$ d_{\mathcal{P}}\big(G_{k}^{n}\big)=\frac{(p_{w}-p_{1})^{T}((p_{3}-p_{5})\times(p_{3}-p_{1}))}{|(p_{3}-p_{5})\times(p_{3}-p_{1})|} $$ 其中 $p_{w} = T_{W}^{k} \times G_{k}^{n}$ 此外，与直接使用距离函数作为残差的LOAM [^33] 不同，此处还会考虑线/面拟合质量w来对残差块进行加权，其评估公式如下： $$ w=\sqrt{\left(\lambda_{3}^{2}-\lambda_{f}^{2}\right)/\lambda_{3}^{2}} $$ 其中 $\lambda$ 为按升序排列的特征值($\lambda_{3} \geq \lambda_{2} \geq \lambda_{1} \geq 0$)，它们可根据激光雷达特征的局部几何分布求得。在线性拟合情况下，$\lambda_{f}$ 即为 $\lambda_{2}$；而在平面拟合情况下，它则变为 $\lambda_{1}$。 另外，为使视觉点残差与激光雷达边缘、平面及地面约束具有相同的度量标准，我们采用逆协方差 $\{(1/\delta_v^2), (1/\delta_e^2), (1/\delta_p^2), (1/\delta_g^2)\}$ 来调整它们的权重。为了在获得精确自我运动估计的同时保持效率，只有被超过n个关键帧观测到的地图点才会被纳入计算（详见第III-D节）。在本研究中，n取值为3。边缘、平面及地面特征因子的最大数量分别设定为1000、1000和200。非线性问题（第III-D节）由Ceres Solver [^35] 求解，协方差仅在第一次迭代时更新。 ### E. 回环闭合检测与位姿图优化 我们的系统基于ORB-SLAM2开发，关键帧生成策略与ORB-SLAM2保持一致。唯一的修改是引入了一个限制条件：最多每五帧才能添加一个关键帧，以防止局部点云地图过于稀疏。每当生成新关键帧时，便会执行回环闭合检测。在我们的框架中，回环闭合模块依赖于[^17]提出的DBoW算法以及ORB-SLAM2预训练的视觉词汇表。每当生成新关键帧时，该帧会被表示为BoWs向量，进而用于检测回环候选帧，这一过程与ORB-SLAM2中的操作一致。在检测到两个关键帧之间的回环后，会通过体素化广义迭代最近点（V-GICP）[^36]算法计算两个子地图点云之间的相对变换，从而得出两个基准帧间的相对位姿。随后，该相对位姿会作为约束边添加到位姿图中。 ### IV. 实验 ### A. 在KITTI里程计数据集上的验证 HVL-SLAM的精度主要在KITTI训练里程计基准测试集[^37]上进行评估。对比对象包括以下先进的基于激光雷达的里程计框架，如A-LOAM [^30]、F-LOAM [^31]、T-LOAM [^38]，以及视觉-激光雷达融合方法，如DEMO [^1]、LIMO [^2]、CamVox [^3]、RGB-L [^4]、GAC-Mapping [^10]、DVL-SLAM [^5]、DV-LOAM [^21]、SDV-LOAM [^22]和PLVL-SLAM [^12]。我们直接采用PLVL-SLAM、LIMO、DEMO和DVL-SLAM论文中提供的结果进行对比。其他方法的结果则是通过运行其开源代码获得。所有序列的评估结果见表I，图8展示了所提方法的样本轨迹。 在整个KITTI训练数据集中，我们的平均平移误差为0.75%，旋转误差为0.0029度/米，这一表现优于那些先进的视觉-激光雷达融合里程计方法。虽然LIMO的平均旋转估计结果比我们的更优，但它经常需要额外的语义标签信息来识别移动物体并调整各类地标的权重。获取此类语义信息通常需要借助GPU设备且耗时较长，因此不适合部署在计算资源和算力有限的移动机器人上。此外，与那些基于激光雷达的方法相比，我们的方法性能也优于F-LOAM。 --- ![Image](images/0b9d1c54a922b2d78d42baaeb02e737650081264bb3c1904e0c1f504f7504028.jpg) ![Image](images/ff25caaa3fbe2827da684f3d52c84c11c7254a837176ae47b63a0bd3957a2359.jpg)
Fig. 8. Trajectory diagram concerning the training set.
Fig. 9. LiDAR map generated by HVL-SLAM from the KITTI 02 sequence.
以及T-LOAM。图9和图10展示了使用本文所提方法对KITTI 02和08序列的激光雷达点云进行的建图结果。图11则呈现了视觉-激光雷达融合地图。 此外，我们还根据路径长度和车辆速度进行了误差分析，对比曲线如图12所示。当车辆行驶速度较快时，由于特征关联效果不佳，我们的系统精度会有所下降。这也解释了为何我们的方法在序列01上的表现相对较差——该序列属于极具挑战性的场景，车辆行驶速度极快。图13展示了所提方法在KITTI里程计序列00上的位置和方向变化情况。我们的方法显著降低了平移和方向定位误差。 ### B. 在我们自己的数据集上的验证 为了验证算法在现实世界中的准确性，我们在武汉大学停车场、图书馆以及江夏场景中对所提出的HVL-SLAM系统进行了进一步测试，这些场景中有大量树木、车辆、道路、建筑物和人行道，具体可见图14。停车场测试场景包含两个回环闭合，而图书馆数据集则不包含任何回环闭合。相比之下，如图1所示的江夏数据集包含三个回环闭合。 1) 实验一：主要实验旨在验证HVL-SLAM能够实现低漂移的自我运动估计，并能重建具有全局一致性的地图。我们在Whu-parking-lot数据集上评估了HVL-SLAM与两种主流SLAM框架以及真实轨迹之间的对比情况，如图15(a)所示；评估结果通过evo工具[^39]得出，见表II。为了验证本系统估计的位姿精度，我们将其与DV-LOAM[^21]（一种直接的视觉-激光雷达SLAM方法）以及LeGO-LOAM[^34]进行了比较。LeGO-LOAM通过引入分割与聚类机制来优化特征提取过程，从而在提高效率的同时增加了回环检测功能，以减少长时间运行中的漂移现象。 --- ![Image](images/343a34bcc09e69de709c2ce392f4b7fc0e7bea65a203b18e85de62233762c60e.jpg)
Fig. 10. LiDAR map generated by HVL-SLAM from the KITTI 08 sequence.
![Image](images/b8af1c9f8ae3fafd68233f54f31ec397d2b6311e6718adaa51bcfb9c7840ece2.jpg)
Fig. 11. Rotation and translation error analysis. Since LIMO only provided error curves according to vehicle speed, so we only draw DEMO, DVL-SLAM, DV-LOAM, PLVL-SLAM [^12] and ours error curves in the path length analysis.
![Image](images/410e7a2e4186921f12104b6228ce5ea0cf151a20928b15c6b0d0f2d1960e4f4d.jpg)
Fig. 12. Visual-LiDAR fusion map generated by HVL-SLAM. (a)–(f) Correspond to KITTI sequence 00, 05, 07, 09, 06, 08, respectively.
从表II中可以看出，我们的方法（1.73米）相比LeGO-LOAM（2.48米）和DV-LOAM（2.24米）具有更优的均方根误差（RMSE）。 2) 实验二：设计下述实验旨在验证所提方案在更复杂、恶劣场景下的稳定性。图书馆场景属于高度动态的数据集， --- ![Image](images/4d3601b7c4944e896bd2919abb18eeb37f3186dd024a281aff00609915c8504e.jpg) ![Image](images/faa1566ee6652abd2a37614af49acdd2a6e38fb78c1b16307a9380390ab3aed6.jpg)
Fig. 13. Change of position and orientation contrasted with ground truth on the KITTI sequence 00 odometry benchmark. Note that the coordinate of the result has been converted from xyz $\rightarrow$ zxy through calibration.
![Image](images/32bd32c683c6f6fb5ad73774a65ec4cb9229c2868c249fc504bf38d1a26a5b78.jpg) ![Image](images/538de68bde978064f601f37f2e1a8c6fadc347ecf07e91539740a7cea54ec414.jpg) ![Image](images/a0949f8c2c17cf8e9c614c8d2a3d3bccd175637de56834c1f619d7d80899bb94.jpg) ![Image](images/76e447e0d27850dc062bf5ab5a4990ad1f06402454bbe61a9a419627026a03ee.jpg)
Fig. 14. Experimental scenes and robot platform. (a) and (b) Are parking lot and library experiment scenes, respectively. (c) Robot experimental platform mainly equipped with VLP-16 and ZED2. (d) Sample data, the LiDAR points are projected to the image plane, and color-coded by their depth.
ABSOLUTE POSE ERROR (APE) (UNIT:M)
TABLE II
dataset (Length [m])parking-lot (200)library (700)
MethodOursDV-LOAMLeGO-LOAMMULLSOursF-LOAMLeGO-LOAMMULLS
Max2.533.383.773.675.787.916.7238.85
Mean1.672.102.322.282.374.313.4913.70
Min0.160.250.190.550.481.230.745.70
RMSE1.732.242.482.412.614.493.6915.56
Std0.600.760.870.781.081.251.217.37
最佳结果以红色标出，次佳结果以蓝色标示。 其中包含大量移动车辆和行人，这进一步加大了视觉里程计面临的挑战。由于动态障碍物的影响以及缺乏视觉特征图提供的重定位能力，DV-LOAM [^21] 和 SDV-LOAM [^22] 在该场景中无法正常工作。此外，由于大量激光点分布在地面上，且周围的树木分布稀疏，直接利用边缘特征和平面特征进行扫描到地图的位姿估计的F-LOAM，其估计误差较大，尤其是在高程方面，如图15(a)所示。通过引入两步位姿估计优化机制，LeGO-LOAM在自我运动估计方面表现出更优的性能。
TABLE III
RELATIVE TRANSLATION (%)/ROTATION (DEG/100 M) ERROR ON THE WHOLE KITTI TRAINING DATASET WITH DIFFERENT FEATURE DEPTH EXTRACTION METHODS
seq.Env.RGB-LCamVoxGAC-MappingLIMOOurs
00Urban0.89/0.381.46/0.612.38/0.330.88/0.390.75/0.32
01Highway3.83/1.245.21/1.61$\times$$\times$1.86/0.70
02Suburb0.89/0.322.75/0.340.89/0.360.89/0.360.81/0.32
03Country1.00/0.336.82/0.362.59/1.402.45/1.510.87/0.28
04Country1.72/0.151.14/0.447.50/0.460.78/0.581.09/0.13
05Urban0.55/0.251.10/0.272.39/0.290.75/0.370.57/0.26
06Urban0.71/0.291.04/0.602.95/0.300.78/0.360.70/0.23
07Urban0.85/0.401.45/0.442.59/0.480.84/0.450.80/0.38
08Suburb1.19/0.382.24/0.832.76/0.431.25/0.441.08/0.37
09Suburb0.69/0.291.28/0.393.05/0.31$\times$0.71/0.29
10Suburb0.76/0.331.54/0.303.05/0.340.92/0.410.81/0.37
avg1.19/0.402.37/0.56$\times$$\times$0.91/0.33
Best results are highlighted in red with second best in blue.
我们的方法融合了F-LOAM和LeGO-LOAM的优点，并引入了视觉特征进行联合优化，大幅提升了位姿估计的精确度，同时有效抑制了垂直方向上的误差积累。尽管场景中包含众多动态车辆和行人，但该方法仍能借助基于视觉-激光雷达融合特征图的重定位能力，保持出色的定位效果，即便因动态物体干扰而暂时失效也是如此。得益于高效的特征深度处理及视觉-激光雷达联合优化机制，我们的系统相较于现有最先进的激光里程计方法实现了最小的均方根误差，建图结果如图16所示。 3) 实验三：为了进一步验证所提系统在更大环境中的性能，我们使用测试车辆开展了实地实验。建图结果如图1所示，激光雷达地图的细节则如图18所示。图19展示了其他方法在该数据集上的建图结果。与这些方法相比，我们提出的HVL-SLAM在回环闭合检测模块的辅助下，实现了更优的全局一致性效果。图20显示了实验中观察到的前两次回环情况，更详细的结果可参见附带的视频。 ### C. 消融实验 我们还对本系统中各个模块的性能进行了评估。 --- ![Image](images/399263bda8300c621acee462f72e8d5e82dcd581e2c92d845096ff46ca790dba.jpg)
Fig. 15. Results of experiment I, where (a) 3-D trajectories and (b) point cloud map color-encoded with images. (c) 3-D LiDAR map.
![Image](images/f9a53fdaf7a1b413d641e08153e4728afc50e9b48ca21c36db5cfb55c442e3c6.jpg) ![Image](images/87ed2ae391948dddbe22e0947e17ed03a3673627b7c84bfa5eb5ebdd9066e245.jpg) ![Image](images/0ecf905a821b60c7c422aa29f7b1eecbdfbd3e6532027fe03879a6d4e844247e.jpg)
Fig. 16. Ours results on experiment II. (a) 3-D trajectories of different methods. (b) Visual LiDAR fusion map, where white represents corner laser points and green represents visual feature points. (c) LiDAR mapping result, which shows a good alignment with Google Map.
![Image](images/84f939be9bd21cc8cbd6157e944f7c69b649c70c3671ddb60fa25af26a8a632f.jpg)
Fig. 17. Mapping results on experiment II. (a) GAC-Mapping results. (b) LeGO-LOAM mapping results. (c) Ours mapping results.
---
TABLE IV
FEATURE DEPTH EXTRACTION EFFICIENCY
times (ms)features num.average time of each feature ( $10^{-3}$ ms)
RGB-LCamVoxGAC-MappingLimoOursRGB-LCamVoxGAC-MappingLimoOursRGB-LCamVoxGAC-MappingLimoOurs
002.077.2742.8517.9812.61811.57653.181425.64331.551205.630.261.133.125.421.05
011.976.4839.55$\times$11.76777.52627.871462.60$\times$1185.580.261.042.74$\times$1.00
022.107.2643.4318.0913.07788.77635.961344.01277.571143.500.271.153.256.521.15
032.167.3643.4718.2713.14830.97668.771302.56243.021125.790.261.113.357.521.17
042.127.2743.7818.1713.44734.38593.631238.96188.691022.900.261.243.559.631.32
052.107.1842.8018.2213.03837.40674.211454.69332.991229.310.251.082.965.471.07
062.047.1542.9618.1012.78778.95624.251455.98246.891201.920.271.162.967.331.07
072.057.0742.5718.0512.89820.36659.791419.52372.161191.030.251.083.014.851.09
082.037.7642.4418.1312.81794.98667.221394.43280.811161.350.261.163.066.461.11
092.067.1942.97$\times$13.07831.69672.501397.02$\times$1199.170.251.093.10$\times$1.10
102.127.1443.0418.6013.06847.30683.921505.28340.531233.040.261.072.875.461.07
avg2.077.1942.7118.1812.88804.90651.031400.06290.471172.660.261.123.096.521.11
![Image](images/9d989c7b38536e2b3d4437f4f17302013c83ada2123ab7dd313ddbc550e61edf.jpg)
Fig. 18. LiDAR map generated by HVL-SLAM from Urban sequence with zoomed-in views. (a)–(f) Highlighting detailed sections of the left area.
![Image](images/d9cb33c58bb86a9d6b7d49d3b0ada91e4d39358ec0057739539e09d8f588fb98.jpg)
Fig. 19. Mapping results. (a)–(c) Correspond to LeGO-LOAM, GAC-Mapping, and MULLS, respectively.
1) 深度拟合实验：为了验证本文提出的特征深度拟合算法的有效性，我们在RGB-L [^2] 系统中替换了LIMO [^3]、CamVox [^10]、Gac-Mapping [^4]以及我们提出的LSDT方法中的特征深度提取模块。实验在KITTI里程计数据集上进行，结果如表III所示。同时，为了展示该方法相较于其他方法的效率优势，我们在表VI中给出了与其他方法的时间效率对比。 2) 混合跟踪与联合局部建图实验：如表V所示，我们构建了三种系统，分别是：搭载混合跟踪模块的特征深度提取模块（Ours(1)）、搭载混合跟踪模块与激光雷达扫描到地图优化模块的特征深度提取模块（Ours(2)），以及搭载混合跟踪模块与视觉-激光雷达联合局部优化模块的特征深度提取模块（Ours(3)）。
TABLE V
RELATIVE TRANSLATION (%)/ROTATION (DEG/100 M) ERROR ON THE WHOLE KITTI TRAINING DATASET
seq.ORB-SLAM (mono)LeGO-LOAMCamVoxOurs(1)Ours(2)Ours(3)
002.63/0.291.51/0.71.46/0.610.72/0.300.69/0.310.65/0.27
01---6.87/2.212.66/0.741.74/0.39
025.99/0.271.96/0.781.62/0.530.93/0.251.03/0.490.79/0.23
031.86/0.361.41/1.000.84/0.311.23/0.241.12/0.210.81/0.39
041.78/0.201.69/0.830.70/0.710.77/0.450.37/0.410.34/0.32
052.71/0.271.01/0.540.62/0.370.71/0.280.62/0.300.46/0.20
066.06/0.220.90/0.471.04/0.600.81/0.250.57/0.220.54/0.22
074.92/1.890.81/0.560.61/0.380.83/0.450.60/0.340.53/0.29
0813.73/0.321.48/0.682.24/0.831.15/0.371.06/0.321.02/0.29
093.56/0.511.57/0.800.70/0.330.86/0.250.77/0.310.64/0.27
106.34/1.201.81/0.850.76/0.370.73/0.270.70/0.310.70/0.28
avg4.96/0.551.42/0.721.06/0.501.41/0.480.93/0.370.75/0.29
为了仅进行里程计层面的对比，这些方法中均禁用了回环检测模块。由于单目版ORB-SLAM2的尺度未知，因此在轨迹评估前需对轨迹坐标及尺度进行对齐。对比结果显示，与单目ORB-SLAM2相比，特征深度提取模块大幅降低了平移误差（从4.96%降至1.41%）。此外，与需要密集点云来提供深度的CamVox相比，我们提出的深度拟合方法在自我运动估计性能上也表现更优。另外，混合跟踪模块在提升位姿估计的鲁棒性方面也发挥了重要作用，尤其是在高速公路等环境较差的场景中。扫描到地图匹配方法在基于激光雷达的SLAM系统中被广泛用于保持自我运动估计的准确性。从表V中可以看出，在Ours(2)系统中，扫描到地图模块显著降低了位姿误差，平均相对平移误差从1.41%降至0.93%，而平均相对旋转误差则从$0.48^{\circ}/100$米减小到$0.37^{\circ}/100$米。与使用相同扫描到地图优化技术的LeGO-LOAM相比，其平移精度与方向精度均有显著提升。此外，当将视觉测量以动态权重的形式加入目标函数后，平移与旋转误差会下降约20%，因此我们可以得出结论：联合优化能够大幅提升我们系统中的位姿估计精度。 --- ![Image](images/db76bb7401f3fbf6c3e8733bfcff963193b45ad18a5e0fa54fe7c3e5d7f1bd3f.jpg)
Fig. 20. Loop-closure detection and pose graph optimization results of HVL-SLAM in experiment III. Ground LiDAR points omitted for better visualization. (a) Before the first loop closure. (b) After the first loop closure. (c) Before the second loop closure. (d) After the second loop closure.
TABLE VI
TIME COST(MS)
modulesKITTI-00 datasetJiangxia dataset
GAC-MappingOursGAC-MappingOurs
depth extraction42.8512.6128.012.18
frame-to-frame tracking113.0124.0087.399.52
local mapping62.3298.4922.38106.13
![Image](images/df68050b362e689874ba2eda8aa88fe952e4fa30ff141477caf8d012c05607da.jpg)
Fig. 21. Time efficiency of GAC-mapping and ours system in KITTI-00 dataset.
### D. 时间分析 所有计算均在运行Linux操作系统的计算机（Intel Core i7-7700 CPU，32GB内存）上完成。软件采用C++实现，并配有机器人操作系统（ROS）通信接口。关键操作的时间消耗如表VI所示。尽管视觉-激光雷达联合优化（局部建图）较为耗时，但它仅在生成关键帧时才会执行；借助OpenMP的帮助，我们的算法在KITTI数据集上的平均运行频率可达8Hz。图21直观展示了所提方法与GAC-Mapping [^10]方法在各模块上的时间效率对比。 ### V. 结论 本文提出了HVL-SLAM，一种精确且高效的激光雷达单目SLAM方法。它包含三个主要部分：一个简单但有效的特征深度提取模块，该模块基于激光雷达分割和Delaunay三角剖分，即便在激光雷达传感器数据稀疏的情况下也能提供深度信息；一个混合视觉-激光雷达跟踪模块，利用光度误差和重投影误差来实现鲁棒且精确的自我运动估计；还有一个视觉与激光雷达联合优化模块，每次生成新关键帧时都能进一步提升位姿估计的精度，同时保证效率。在KITTI里程计基准测试以及我们在各种环境中采集的数据上进行的实验表明，所提出的HVL-SLAM方法有效且鲁棒，能够比现有的视觉激光雷达里程计框架提供更高质量的融合建图结果，尤其是在特征退化场景下表现尤为突出。 在未来的工作中，我们将通过引入动态物体剔除模块来进一步优化该系统，以提高自我运动估计的精度和鲁棒性，并提升动态环境下的建图任务性能。 ## 参考文献 [^1]: J. Zhang, M. Kaess, and S. Singh, “Real-time depth enhanced monocular odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Chicago, IL, USA, Sep. 2014, pp. 4973–4980. 经授权许可使用，使用范围仅限于：上海科技大学。于2026年9月11日UTC时间05:26:19从IEEE Xplore下载。受限使用。 --- [^2]: J. Gräter, A. Wilczynski, and M. Lauer, "LIMO: LiDAR-monocular visual odometry," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Madrid, Spain, Oct. 2018, pp. 7872–7879. [^3]: 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. Autom. (ICRA), Xi'an, China, May 2021, pp. 5049–5055. [^4]: F. Sauerbeck, B. Obermeier, M. Rudolph, and J. Betz, "RGB-L: Enhancing indirect visual SLAM using LiDAR-based dense depth maps," in Proc. 3rd Int. Conf. Comput., Control Robot. (ICCCR), Mar. 2023, pp. 95–100. [^5]: Y.-S. Shin, Y. S. Park, and A. Kim, "Direct visual SLAM using sparse depth for camera-LiDAR system," in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Brisbane, QC, Australia, May 2018, pp. 5144–5151. [^6]: K. Huang, J. Xiao, and C. Stachniss, “Accurate direct visual-laser odometry with explicit occlusion handling and plane detection,” in Proc. Int. Conf. Robot. Autom. (ICRA), Montreal, QC, Canada, May 2019, pp. 1295–1301. [^7]: J. Zhang and S. Singh, "Visual-LiDAR odometry and mapping: Low-drift, robust, and fast," in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2015, pp. 2174–2181. [^8]: A. Reinke, X. Chen, and C. Stachniss, “Simple but effective redundant odometry for autonomous vehicles,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Xi'an, China, May 2021, pp. 9631–9637. [^9]: Y. Seo and C.-C. Chou, "A tight coupling of vision-LiDAR measurements for an effective odometry," in Proc. IEEE Intell. Vehicles Symp. (IV), Paris, France, Jun. 2019, pp. 1118–1123. [^10]: J. He, Y. Zhou, L. Huang, Y. Kong, and H. Cheng, “Ground and aerial collaborative mapping in urban environments,” IEEE Robot. Autom. Lett., vol. 6, no. 1, pp. 95–102, Jan. 2021, doi: 10.1109/LRA.2020.3032054. [^11]: C.-C. Chou and C.-F. Chou, "Efficient and accurate tightly-coupled visual-LiDAR SLAM," IEEE Trans. Intell. Transp. Syst., vol. 23, no. 9, pp. 14509–14523, Sep. 2022. [^12]: 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 Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Paris, France, May 2020, pp. 1091–1097. [^13]: X. Chen, I. Vizzo, T. Läbe, J. Behley, and C. Stachniss, “Range image-based LiDAR localization for autonomous vehicles,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2021, pp. 5802–5808. [^14]: A. Y. Hata and D. F. Wolf, “Feature detection for vehicle localization in urban environments using a multilayer LiDAR,” IEEE Trans. Intell. Transp. Syst., vol. 17, no. 2, pp. 420–429, Feb. 2016. [^15]: Z. Wang, J. Fang, X. Dai, H. Zhang, and L. Vlacic, “Intelligent vehicle self-localization based on double-layer features and multilayer LiDAR,” IEEE Trans. Intell. Vehicles, vol. 5, no. 4, pp. 616–625, Dec. 2020. [^16]: R. Arandjelović, P. Gronat, A. Torii, T. Pajdla, and J. Sivic, "NetVLAD: CNN architecture for weakly supervised place recognition," IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 6, pp. 1437–1451, Feb. 2018. [^17]: D. Galvez-López and J. D. Tardos, “Bags of binary words for fast place recognition in image sequences,” IEEE Trans. Robot., vol. 28, no. 5, pp. 1188–1197, Oct. 2012. [^18]: P.-E. Sarlin, C. Cadena, R. Siegwart, and M. Dymczyk, “From coarse to fine: Robust hierarchical localization at large scale,” in Proc. IEEE/CVF Conf. Comput. Vis. Pattern Recognit. (CVPR), Jun. 2019, pp. 12708–12717. [^19]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 3, pp. 611–625, Mar. 2018. [^20]: C. Park, P. Moghadam, J. L. Williams, S. Kim, S. Sridharan, and C. Fookes, “Elasticity meets continuous-time: Map-centric dense 3D LiDAR SLAM,” IEEE Trans. Robot., vol. 38, no. 2, pp. 978–997, Apr. 2022. [^21]: W. Wang, J. Liu, C. Wang, B. Luo, and C. Zhang, "DV-LOAM: Direct visual LiDAR odometry and mapping," Remote Sens., vol. 13, no. 16, p. 3340, 2021. [^22]: 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, doi: 10.1109/TPAMI.2023.3262817. [^23]: K. Cao et al., “Tightly-coupled LiDAR-visual SLAM based on geometric features for mobile agents,” in Proc. IEEE Int. Conf. Robot. Biomimetics (ROBIO), Koh Samui, Thailand, Dec. 2023, pp. 1–8, doi:10.1109/robio58561.2023.10354794. [^25]: R. Mur-Artal and J. D. Tardós, “ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras,” IEEE Trans. Robot., vol. 33, no. 5, pp. 1255–1262, Oct. 2017. [^24]: C. Shu and Y. Luo, “Multi-modal feature constraint based tightly coupled monocular visual-LiDAR odometry and mapping,” IEEE Trans. Intell. Vehicles, vol. 8, no. 5, pp. 3384–3393, May 2023, doi: 10.1109/TIV.2022.3215141. [^26]: S. Lee, H. Lim, and H. Myung, "Patchwork++: Fast and robust ground segmentation solving partial under-segmentation using 3D point cloud," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Kyoto, Japan, Oct. 2022, pp. 13276–13283, doi: 10.1109/IROS47612.2022.9981561. [^27]: I. Bogoslavskyi and C. Stachniss, "Fast range image-based segmentation of sparse 3D laser scans for online operation," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Daejeon, South Korea, Oct. 2016, pp. 163–169. [^28]: H. Lim, M. Oh, and H. Myung, “Patchwork: Concentric zone-based region-wise ground segmentation with ground likelihood estimation using a 3D LiDAR sensor,” IEEE Robot. Autom. Lett., vol. 6, no. 4, pp. 6458–6465, Oct. 2021. [^29]: R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: A versatile and accurate monocular SLAM system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Oct. 2015. [^30]: T. Qin and S. Cao. (2018). A-loam. [Online]. Available: [^31]: 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. (IROS), Sep. 2021, pp. 4390–4396. [^32]: Y. Pan, P. Xiao, Y. He, Z. Shao, and Z. Li, “MULLS: Versatile LiDAR SLAM via multi-metric linear least square,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Xi'an, China, May 2021, pp. 11633–11640. [^33]: J. Zhang and S. Singh, “LOAM: LiDAR odometry and mapping in real-time,” in Proc. Robot., Sci. Syst., D. Fox, L. E. Kavraki, and H. Kurniawati, Eds., Berkeley, CA, USA: Univ. California, 2014, pp. 1–9. [^34]: T. 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. (IROS), Madrid, Spain, Oct. 2018, pp. 4758–4765. [^35]: S. Agarwal, K. Mierle, and T. C. S. Team. (Mar. 2022). Ceres Solver. [Online]. Available: [^36]: K. Koide, M. Yokozuka, S. Oishi, and A. Banno, “Voxelized GICP for fast and accurate 3D point cloud registration,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Xi'an, China, May 2021, pp. 11054–11059. [^37]: A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, "Vision meets robotics: The KITTI dataset," Int. J. Robot. Res., vol. 32, no. 11, pp. 1231–1237, 2013. [^38]: P. Zhou, X. Guo, X. Pei, and C. Chen, "T-LOAM: Truncated least squares LiDAR-only odometry and mapping in real time," IEEE Trans. Geosci. Remote Sens., vol. 60, pp. 1–13, 2022, Art. no. 5701013, doi: 10.1109/TGRS.2021.3083606. [^39] M. Grupp.（2017）。Evo：用于评估里程计和SLAM的Python软件包。[在线]。可获取地址：