文章 # DV-LOAM：直接视觉激光雷达里程计与建图 王伟 $^{ID}$、刘军、王晨杰 $^{ID}$、罗斌 $^{*}$ 和张成 武汉大学测绘遥感信息工程国家重点实验室，中国武汉珞喻路129号，邮编430079；（王伟）；（刘军）；（王晨杰）；（张成） - 通讯作者： 摘要：过去几年里，自动驾驶汽车取得了快速发展，而同步定位与建图（SLAM）被视为其核心能力。在本文中，我们提出了一种由三个模块组成的直接视觉激光雷达融合SLAM框架。首先，我们提出了一种两阶段的直接视觉里程计模块，该模块包含帧间跟踪步骤和改进的基于滑动窗口的精简步骤，能够在保证效率的同时估算出相机的精确位姿。其次，每次生成关键帧时，会将动态物体视为关键帧，以获得更高的定位精度和更强的鲁棒性。最后，我们采用了一种结合视觉词袋（BoW）和激光雷达虹膜特征的并行全局与局部搜索回环检测（PGLS-LCD）模块来进行位置识别，从而修正累积误差并保持全局一致的地图。我们在公开数据集以及我们的移动机器人数据集上开展了大量实验，以验证该框架中各模块的有效性。实验结果表明，所提出的算法相比现有最先进方法能够实现更精确的位姿估计。 ![Image](images/ba468c84d148cc680fbf832416a9818faadf2cc7dd9fb85bcdb8a622b235109d.jpg) 收稿日期：2021年7月4日 录用日期：2021年8月18日 发表日期：2021年8月23日 引用格式：王伟、罗斌、王晨杰、刘军、张成 DV-LOAM：直接视觉激光雷达 里程计与建图。《遥感》2021年，第13卷，第3340页。 出版商声明：MDPI对于所发论文中的管辖权声明及机构归属保持中立。 ![Image](images/5cd6abf5fed1ad3f2a3b0fe508279a949c9643e749b85441ef0bf3f861bddb5e.jpg) 学术编辑：Saeid Pirasteh 版权声明：© 2021作者所有。 授权方：瑞士巴塞尔的MDPI出版社。 本文为开放获取文章， 发布遵循相关条款与 条件，采用知识共享 署名（CC BY）许可协议（https:// creativecommons.org/licenses/by/ 4.0/). 关键词：同步定位与建图；直接视觉激光雷达里程计；回环检测 ## 1. 引言 自动驾驶导航领域的最新动态被视为下一代革命性技术，它将在诸多方面改变人们的生活[^1]。这一趋势引发了汽车行业的强烈反响，而SLAM技术在自动驾驶车辆中发挥着关键作用，尤其是在解决陌生环境中的定位问题方面。传统上，自动驾驶车辆的定位依赖于全球卫星导航系统以及惯性导航系统（GNSS/INS）[^2] [^3]。然而，由于信号遮挡、多径效应和磁噪声的影响，GNSS定位信息容易出现跳变[^4] [^5]；而在城市峡谷、大道及室内等GNSS信号不佳的环境中，惯性导航系统可能会产生累积误差。因此，研究如何利用SLAM技术来解决无GNSS环境下的定位问题，对于实现自动驾驶具有重要意义。其中，摄像头与激光雷达的融合方法正受到学者们的日益关注。 近年来，基于单目摄像头的视觉SLAM领域取得了显著进展，包括直接法和基于特征的间接法。传统的基于视觉特征的方法已趋于成熟，形成了稳定的视觉SLAM算法。其中最受欢迎的有PTAM[^6] [^7]和ORB-SLAM[^8] [^9]。不过，这类基于特征的方法存在一个缺点：当环境呈现简单的重复图案或无特征状态时，很难找到对应的特征关联。与基于特征的方法不同，直接法有望利用完整的图像信息，例如LSD-SLAM[^10]、DSO[^11]、SVO[^12] [^13]。由于它们仅使用一定数量的图像块，或梯度幅度超过阈值的区域来降低计算复杂度，从而确保直接法能够在CPU上实时运行。 --- 尽管无需依赖视觉特征的对应关系，直接法在实时运行于CPU上时能展现出出色的运动估计性能，但单目图像不足以估算自身运动——因为沿摄像头光轴方向的移动对视觉特征的影响极小，这可能导致估计结果出现偏差。 由于对环境变化的鲁棒性强且不受光照影响，基于激光雷达的SLAM系统比基于视觉的系统更受欢迎。其中许多系统基于迭代最近点（ICP）算法[^14]或其变体[^15] [^16]。目前已有多种激光雷达SLAM方法[17–24]被提出，且均取得了令人瞩目的成果。其中最具代表性的是LOAM[^17] [^18]，它被认为是6自由度激光雷达SLAM领域的顶尖技术。此后，Qin和Cao[^25]简化了特征提取流程，提升了LOAM在扫描点与地图匹配方面的效率。不过，纯激光雷达系统也存在局限性：由于激光扫描仪的光束数量有限，在隧道或走廊等具有重复结构的环境中，垂直方向的距离测量数据十分稀疏，容易导致系统失效[^26]。 由于图像能够提供丰富的视觉纹理信息，而激光雷达则具有稀疏但精确的距离测量能力以及360度视场角，因此视觉传感器与激光雷达传感器的结合在运动估计任务中或许能实现互补。在视觉激光雷达里程计领域的相关研究可参见参考文献 [^27] [^28] [^29] [^30] [^31] [^32] [^33] [^34] [^35] [^36]。例如，Shin等人的研究 [^30] [^31]，即DVL-SLAM，试图在DSO框架内解决视觉激光SLAM问题。他们采用投影后的激光点作为特征点，而非从图像中提取的显著梯度点。由于这些特征点的深度值已知且固定，他们便通过多帧光度优化来估算关键帧的姿态，这一过程与DSO中的操作类似。由于无需提取和匹配特征，且能借助激光雷达精确的距离测量结果，即便在大规模环境中深度测量数据极为稀疏，该系统仍能输出鲁棒、高效且精准的SLAM结果。不过，DVL-SLAM [^30] [^31] 仅使用了相机视场内的激光点，而由于Velodyne激光雷达等激光传感器可提供360度扫描，大部分测距数据会被丢弃。这种设置会导致系统精度降低，且在无纹理场景中表现较差。对于V-LOAM [^32]而言，它结合了视觉里程计方法DEMO [^28] [^29] 与激光雷达里程计方法LOAM [^17] [^18]。视觉里程计负责处理快速运动，而激光雷达里程计则能在光照条件不佳时保证低漂移率和鲁棒性，因此该系统能够应对包括平移和旋转在内的剧烈运动，也能应对完全白化或黑化图像中缺乏光学纹理的情况。由于里程计是通过随时间累积的小幅度运动来推算位置，长时间行驶过程中必然会出现漂移现象。因此，V-LOAM并不适用于大规模环境。 为缓解这一问题，系统会利用循环闭合作为触发机制，通过视觉或激光雷达方法识别已访问过的地点，进而启动位姿图优化。视觉方法包括使用词袋模型（BoW）[37–39]来识别地点，以及利用透视n点（PnP）算法来估算姿态修正量。在激光雷达方法中，地点识别可通过基于分段的算法如SegMatch [^40] [^41]，或基于全局描述符的方法[42–44]，当然还有基于深度学习的方法 [^45] [^46]；姿态修正则是通过ICP算法来估算。然而，已访问过的地点往往会因光照、天气或视角变化而发生改变，这给基于视觉的地点识别带来了难题。尽管激光雷达受此类环境变化的影响较小，但它缺乏基于词袋模型方法的快速性与通用性，因此无法执行全局搜索。通常需采用基于局部半径搜索的策略来缩小搜索范围，以提高循环闭合检测的效率。不过，当累积漂移量超过搜索半径时，这种策略可能会导致召回率下降。 在本文中，我们提出了一种全新的直接视觉激光雷达里程计与建图（DV-LOAM）方法，该方法将激光雷达的稀疏深度测量数据与单目相机相结合。该系统主要由三个部分构成：首先，在视觉里程计部分，我们首先采用基于深度增强的帧间直接视觉里程计方法…… --- 采用恒定速度运动模型来估计相邻帧的初始运动。由于它直接使用投影的激光点作为特征点，而非利用从图像中提取的显著特征点，因此其效率非常惊人。随后，通过基于滑动窗口的优化方法来进一步细化运动参数，这既保证了实时性能，又维持了运动估计的局部精度。其次，在激光雷达建图部分，每次生成关键帧时，我们都会应用基于扫描匹配到地图的激光雷达建图流程，该流程利用线条和平面等几何特征来优化运动估计，并消除由视觉里程计漂移引起的点云畸变。最后，在回环检测部分，我们提出了一种并行全局与局部搜索（PGLS）回环检测算法，以消除漂移误差并获得全局一致的地图。该算法首先并行使用词袋模型（BoW）和激光雷达虹膜技术[^44]来寻找回环候选，接着利用截断最小二乘估计与半定松弛算法（TEASER）[^47]计算出姿态修正的粗略估计值，最后再通过V-GICP算法[^48]对粗略估计值进行精细化处理。综上所述，本研究的主要贡献如下： 1. 我们提出了DV-LOAM，这是首个将深度增强型直接视觉里程计模块与激光雷达建图模块整合到定位与建图中的框架。它充分发挥了直接视觉里程计模块的高效性以及激光雷达扫描匹配方法的精确性，从而相比现有技术实现了更优的性能。 2. 在前端，我们提出了一种两阶段直接跟踪策略，以便在保证实时性能的同时维持运动估计的局部精度。首先，利用直接帧间视觉里程计来估计相机姿态，其效率高于基于ICP的里程计方法。其次，采用基于滑动窗口的精简模块来减少累积漂移。由于无需提取和匹配特征，该方法速度极快，甚至能在低纹理环境中正常工作。 3. 在后端，我们提出了PGLS-LCD模块；通过融合词袋模型与激光雷达虹膜技术，我们的方法不仅能大幅弥补基于视觉的回环检测能力的不足，尤其是在反向访问场景下，还能解决当累积漂移超出激光雷达方法的搜索半径时无法找到正确回环的问题。此外，以TEASER算法的估计结果作为初始猜测，再应用V-GICP来精细化回环对之间的姿态修正变换，即便在存在较大漂移的情况下也能获得更稳健、更精确的结果。 4. 我们开展了大量实验，从定量评估和可视化线索两方面详细测试并验证了该系统的有效性，同时分析了不同基线位置下各模块带来的精度提升。 本文其余部分的结构如下：第2节对相关研究进行了简要回顾；第3节介绍了所提出的DV-LOAM架构；第4节阐述了直接视觉里程计、激光雷达建图以及回环检测的细节；第5节讨论了所提DV-LOAM的精度评估情况及其与现有先进方法的对比；第6节介绍了讨论内容与未来工作；最后，第7节给出了本文的结论。 ### 2. 相关工作 由于本文的重点是研究视觉与激光雷达的融合方法，因此此处仅介绍视觉激光雷达融合的方法。将视觉传感器与激光雷达传感器相结合形成的视觉激光雷达里程计能够发挥两种传感器的优势，因此受到了越来越多的研究，这对现代SLAM应用具有很大帮助。视觉激光雷达里程计的常用方法可分为三类：基于视觉里程计的方法、基于点云配准的方法以及紧耦合融合方法。 --- #### 2.1. 基于视觉里程计的方法 基于视觉里程计的方法试图将带有激光扫描所得已知像素深度信息的视觉里程计流程加以应用。在LIMO [^27]中，利用激光雷达测量数据来提取深度信息。将点云投影到图像帧上后，再借助基于视觉关键帧的捆绑调整来进行运动估计与建图。DVL-SLAM [^30] [^31]中的研究工作提出了一种使用来自激光雷达的稀疏深度点云的直接视觉SLAM方法。他们采用投影后的激光点作为特征点，而非从图像中提取的显著梯度点。由于特征点的深度值已知且固定，他们便像DSO一样进行多帧光度优化，以估算关键帧的姿态。张等人[^28] [^29]提出的深度增强单目里程计（DEMO）方法也类似。该方法将视觉特征与深度测量结果相关联，并据此估算相机运动。随着低成本固态激光雷达的不断发展，采用视场角（FoV）激光雷达的视觉激光雷达融合方法也大量涌现，例如R²LIVE [^49]和CamVox [^50]。与传统机械式激光雷达相比，尽管视场角较小，但固态激光雷达价格相对低廉且点云密度更高。CamVox [^50]基于ORB_SLAM2框架构建，它采用Livox作为深度传感器来实现全新的“RGBD” SLAM方法。由于激光雷达点云的精度和测距范围优于其他深度相机，即便在户外强阳光场景下，CamVox也表现出更高的效率和鲁棒性。不过，这些方法并未考虑融合信息的不确定性，比如外部标定误差。为了克服这些问题，Voges等人[^51]提出利用区间分析对所有误差进行建模，并为相机和激光雷达引入有界误差模型，从而能够更精确地估算特征点的深度及深度的不确定性。然而，基于视觉里程计的方法普遍存在一个问题：它们没有考虑位于相机视场之外的激光点，导致激光雷达的大部分测距数据会被丢弃。这种设置会使系统的精度降低，且在无纹理场景中表现较差。 #### 2.2. 基于点云配准的方法 与基于视觉里程计的方法相比，基于点云配准的方法试图借助图像信息在多个方面对整个点云进行对齐。例如，[^33] 和 V-LOAM [^32] 中的方法仅将视觉里程计的结果作为 ICP 过程的初始猜测，从而降低 ICP 陷入局部极小值的可能性。这些算法遵循里程计框架，并未采用诸如束调整、回环闭合等 SLAM 技术。不过，这些方法中所用的视觉里程计属于基于特征的方法，这意味着需要提取特征，但在低纹理环境中很难提取到足够多的特征。此外，利用激光雷达提取特征的深度信息既耗时又复杂，尤其是当激光雷达的扫描光束受限时（如 VLP-16）。另外，为了应对自我运动估计失败的情况，Reinke 等人 [^52] 提出了一种集成式视觉-激光雷达里程计系统：在帧间跟踪阶段并行使用多种里程计算法，随后选择最符合约束条件的位姿——这些约束包括车辆的动力学和运动学约束，以及当前激光雷达扫描数据与局部点云地图之间的基于倒角距离的评分——从而充分发挥现有各类自我运动估计算法的优势。该系统展现出极强的鲁棒性，能够应对各种故障情况，也为我们提升前端里程计的鲁棒性提供了新思路。除了计算相邻帧之间的位姿变换外，视觉还可用于回环闭合检测，比如 [^53]。该方法首先利用词袋模型来检测回环闭合候选，随后采用基于激光雷达的几何验证策略来确认回环闭合，最后通过位姿图优化来获得全局一致的轨迹。尽管该方法取得了不错的效果，但仅靠视觉词袋模型来检测回环闭合无法解决反向访问的问题。 --- #### 2.3. 紧耦合融合方法 紧密耦合的融合方法试图利用视觉和激光雷达测量数据来进行更有效的里程计估计。Huang在[^35]中的研究提出了一种基于点和线特征的激光雷达-单目视觉里程计方法。线特征具有特定的优势，即它们对噪声[^54]、大视角范围[^55]以及运动模糊[^56]等问题不太敏感，而这些正是仅使用点的系统（如LIMO [^27]）的主要缺陷。通过将更多的环境结构信息纳入位姿估计，并采用稳健的特征深度提取方法，能够大幅降低特征的3D不确定性，从而提高位姿估计的精度。由于激光雷达点云中可能包含遮挡点，这些遮挡点会降低估计精度。[^36]中的研究通过提出一种带有遮挡点检测器和共面点检测器的直接SLAM方法来解决这一问题。此外，为了实现高精度估计，该方法采用了两阶段配准策略，优化一个同时兼顾点云紧密对齐与图像外观一致性的联合目标函数。为了最大限度地利用摄像头和激光雷达的测量数据，TVLO [^34]独立运行两条建图流水线来构建两种不同的地图，即激光雷达体素图和视觉地图。输入的激光雷达点与激光雷达体素图匹配以生成激光雷达几何残差，而视觉特征则与视觉地图匹配以构成立体投影残差。通过分别处理视觉和激光雷达测量数据，TVLO不会受到将激光雷达深度赋值给不对应的视觉特征这一潜在问题的影响，且在求解里程计残差时，相比现有方法[^27] [^28] [^32]能纳入更多几何约束。不过，TVLO的每一帧都需要进行上述特征提取和位姿优化，这一过程非常耗时。 与本文提出的方法类似的是DVL-SLAM [^30] [^31]。与DVL-SLAM相比，我们的方法做出了以下改进。首先，采用帧间跟踪策略来估计相邻帧的运动。与帧到关键帧的方法相比，它在高速公路和居民区等各种复杂环境中表现出更强的鲁棒性。此外，滑动窗口优化模块会优化每一帧的位姿，而非像DVL-SLAM那样仅优化关键帧，从而保持更高的局部精度。其次，由于DVL-SLAM仅使用摄像头视野内的激光雷达点，因此该系统在纹理较少的场景中精度较低且容易受影响。在本文中，我们采用基于激光雷达的扫描-地图匹配方法来减少视觉里程计模块积累的漂移，这显著提升了定位的精度和鲁棒性，尤其是在结构信息丰富的城市环境中。最后，与DVL-SLAM采用的基于词袋模型的地点识别模块相比，我们的PGLS-LCD方法结合了词袋模型和激光雷达虹膜技术的优势，因此性能更佳，尤其在重复访问场景中表现突出。 此外，V-LOAM [^32] 在某种程度上也与我们的方法具有相似性。不过，二者也存在以下几处明显的差异。首先，我们采用基于直接法的视觉里程计，而非基于特征的跟踪方法来估算粗略的运动。由于我们是直接将投影后的激光点作为特征点，而非从图像中提取特征，且不像 V-LOAM 那样利用激光扫描来拟合特征深度，因此我们的方法在视觉里程计模块中表现出更好的鲁棒性、效率和精度。另外，由于大多数 SLAM 方法都假设世界是静态的，真实环境中的移动物体可能会降低扫描匹配的准确性。为了减轻动态物体的影响，我们方法中的激光雷达建图模块采用了两种额外策略。首先，我们采用了 LeGO-LOAM [^19] 中描述的特征提取方法，该方法能避免从植被和小簇等噪声区域提取特征。第二种策略是 [^57] 中采用的两阶段激光雷达配准策略：我们先通过少量迭代进行位姿优化，再利用优化结果计算残差，去除最大的残差后再执行完整的位姿优化，从而获得更精确的位姿。最后，该方法的 --- 在大尺度环境中，我们方法的轨迹连续性是通过集成 PGLS-LCD 与位姿图优化模块来维持的。 ### 3. 系统概述 图 1 展示了所提出方法的整体架构。该系统由前端和后端模块组成，与一般 SLAM 方法类似。算法的输入为时间同步的顺序图像数据与顺序激光雷达扫描数据（例如通过硬件触发器获取），且已知它们在机器人上的相对变换关系。前端包含两个主要部分：视觉里程计（VO）模块和激光雷达建图模块。首先，当新帧——即带有相关稀疏深度的图像——到来时，会先应用直接帧间跟踪模块（第 4.2 节）来估算初始运动。一旦跟踪过程成功执行，滑动窗口优化模块（第 4.3 节）便会细化初始位姿，以在确保实时性能的同时保持局部精度。随后，若满足生成关键帧的条件，便通过激光雷达扫描到地图优化方法（第 4.4 节）进一步细化位姿。后端则包含回环检测模块和位姿图优化模块。所提出的 PGLS-LCD（第 4.5 节）用于地点识别；一旦检测到回环，便会执行全局位姿图优化（第 4.6 节）以维持全局一致性，同时更新激光雷达扫描到地图优化模块所使用的局部激光雷达地图。 ![Image](images/0f3483ade78949e80fbf15e2f6fcd538fa9daaad421f09f4b1740aa5765705dd.jpg)
Figure 1. Algorithm overview of proposed Direct Visual LiDAR Odometry and Mapping. Our main contributions in this paper are highlighted in green.
--- ### 4. 方法 #### 4.1. 符号说明 某一帧 $\mathcal{F}_i$ 包含灰度图像 $\mathcal{I}_i$ 和稀疏激光雷达点云 $P_i$。注意，激光雷达点云 $P_i$ 已被转换到相机坐标系下。每个点 $p_k \in P_i$ 均可通过投影函数 $u_k \in \mathbb{R}^2$ 投影到图像像素坐标 $\pi(p_k)$ 上。 $$ \pi(p_{k})=\begin{bmatrix}f_{x}&0&c_{x}\\ 0&f_{y}&c_{y}\\ 0&0&1\end{bmatrix}\begin{bmatrix}x_{k}\\ y_{k}\\ z_{k}\end{bmatrix}=\begin{bmatrix}u_{k}\\ v_{k}\end{bmatrix} $$ 我们使用变换矩阵 $\mathbf{T}_{cw}^n \in SE(3)$ 来表示某一帧在世界坐标系下的位姿，其中 n 代表帧索引。因此，$\mathcal{F}_m$ 与 $\mathcal{F}_n$ 之间的相对位姿变换可表示为： $$\mathbf{T}_{m}^{n}=\mathbf{T}_{c w}^{n}\mathbf{T}_{c ({w}}^{{m}})^{{}-1}$$ 该变换也可用于不同坐标系下点的坐标转换，如下所示： $$ p_{n}=\mathbf{T}_{m}^{n}p_{m}=\exp(\hat{\xi})p_{m}=\begin{bmatrix}R_{m}^{n}&t_{m}^{n}\\ \mathbf{0}^{T}&1\end{bmatrix}\begin{bmatrix}x_{m}\\ y_{m}\\ z_{m}\end{bmatrix} $$ 其中，$p_m$ 和 $p_n$ 是各帧中的点，$R_m^n \in SO(3)$ 和 $t_m^n \in \mathbb{R}^3$ 分别是旋转矩阵和平移向量。李代数 $\xi = \begin{bmatrix} \phi \\ \rho \end{bmatrix} \in \mathbb{R}^6$、$\phi$ 以及 $\rho$ 分别对应角速度和线速度向量。 #### 4.2. 直接帧间跟踪 直接帧间跟踪模块无需借助视觉特征即可完成快速运动估计的跟踪过程。由于使用全部激光雷达点会降低逐帧计算的效率，且低纹理区域中的点对运动估计的作用有限。在保持运动估计精度的同时，为降低计算复杂度，需先对激光点进行有效下采样。随后采用基于图像块的直接跟踪方法来估计相邻帧间的运动。与间接方法不同，这种跟踪过程速度更快，无需进行类似角点的特征提取与匹配。此外，仅使用与激光雷达相关的点省去了用于估计深度的三角测量步骤，这种简洁性对配备摄像头和激光雷达的各种机器人系统而言十分有利。与其他直接方法类似（[^11]、[^12]），该方法也采用了图像金字塔和恒定运动模型策略来加速跟踪过程。具体细节如下。 ##### 4.2.1. 显著点选择 尽管激光雷达的测量数据本身已较为稀疏，但我们用于估计运动的点更少，从而进一步提升了计算效率。选择显著点的详细过程在 [^30]、[^31] 中有描述。值得一提的是，这种类似策略广泛应用于各类SLAM方法中，包括直接法（[^11]、[^12]）和间接法（[^8]、[^9]）。图2展示了所选显著点的示例。 --- ![Image](images/d6e57e8727a69caa46835b7c83e08e8ce5095455e9f9d728fd90cdb7151947d8.jpg)
(a)
(b)
Figure 2. Salient point selection result. (a) All LiDAR points projected to the image plane without selection. (b) The result of selected salient points.
##### 4.2.2. 图像块模式选择 与逐像素跟踪相比，基于图像块的跟踪方法（[^11]）在保证足够性能与鲁棒性的同时，所需的像素数量略少。不过，计算复杂度会随着图像块尺寸的增大而升高。本文中使用的图像块模式如图3a所示。 ![Image](images/33a49d0d878f41bdabe58d5f2f407e808378f9d99fcdb4961e0f33fa5a83bfda.jpg)
(a) Patch pattern
![Image](images/37ffa0493e78f5b6942dfb08d892eebde3e95c2dc42fd3423bfcc113dfa27f82.jpg)
(b) Illustration of the patch based direct tracking problem
Figure 3. (a) The patch pattern used in our approach, the colored cells are pixels used in direct tracking process, and all pixels share the same depth with the intermediate dark grid obtained from the LiDAR points. (b) Illustration of the patch based direct tracking problem.
##### 4.2.3. 帧间跟踪 与DVL-SLAM（[^30]、[^31]）不同，我们在跟踪阶段采用帧间跟踪策略而非帧到关键帧跟踪，原因如下：首先，相邻帧间的视野重叠范围更大，这意味着可以纳入更多约束来求解里程计残差；其次，时间间隔越短，环境光照变化越小，直接法的亮度一致性假设就越成立；最后，时间间隔越短，机器人的运动变化越小，越符合恒定速度运动模型。 如图3b所示，上一帧 $p_m$ 中的显著点 $\mathcal{F}_m$ 通过初始变换 $\mathcal{I}_n$ 被投影到当前帧 $\mathcal{F}_n$ 的图像 $\mathbf{T}_m^n$ 上，该初始变换由 --- 恒定运动模型得出。随后可定义对应点的光度残差如下： $$ r(p_{m})=\mathcal{I}_{n}(\pi(\mathbf{T}_{m}^{n}p_{m}))-\mathcal{I}_{m}(\pi(p_{m})) $$ 此外，曝光设置或环境光线的变化可能会对基于直接法的方法产生较大影响。为解决这一问题，我们在方程中加入了光度参数，即增益$\alpha$和偏置$\beta$参数，它们可以表示为：$\mathcal{I}_a'(\cdot) \overset{def}{=} \alpha \mathcal{I}_a(\cdot) + \beta$。$\alpha$和$\beta$均为未知参数，将在优化过程中进行估计，具体细节可参见[^10]。因此，新的光度残差可以定义为： $$ r^{\prime}(p_{m})=\mathcal{I}_{n}(\pi(\mathbf{T}_{m}^{n}p_{m}))-\mathcal{I}_{m}^{\prime}(\pi(p_{m})) $$ 于是，用于估计相邻帧间运动的目标函数可定义如下： $$ \mathbf{E}_{t r a c k i n g}=\frac{1}{2}\sum_{p_{k}\in P_{m}}\sum_{p_{i}\in\Omega_{p_{k}}}w(r^{\prime}(p_{i}))\Big(r^{\prime}(p_{i})^{2}\Big) $$ 其中$\Omega_{p_k}$是显著点$p_k$的稀疏模式图像块，如图3a所示；我们使用的权重函数$w(\cdot)$的相关描述见[^58]，该函数在每次迭代中都会进行计算。 $$ w(r)\stackrel{d e f}{=}\frac{\nu+1}{\nu+(\frac{r}{\delta r})^{2}} $$ 此处，自由度$\nu$被设为5，残差方差$\delta\tau$在每次高斯-牛顿优化迭代时获取。基于t分布的权重函数的影响可参见[^58]。需要注意的是，为了更高效的计算，此处还采用了逆合成策略。关于该策略的更多信息，请参阅[^12]。此外，我们还采用由粗到精的策略，以避免优化陷入局部最小值，并应对优化过程中的大位移问题，相关内容见[^30]。本文中，图像金字塔的层数被设为$\mathbf{N}_p$，每层的最大迭代次数则为$\mathbf{N}_{mi}$。 #### 4.3. 滑动窗口优化 滑动窗口通过维持固定数量的关键帧，来实现视觉里程计模块处理时间的恒定、提升局部精度并降低计算负载。与DVL-SLAM中的滑动窗口优化相比[^30] [^31]，本文的方法主要有两点不同：一方面，我们的方法中滑动窗口用于优化每一帧的姿态，而非关键帧；另一方面，滑动窗口内关键帧的姿态是固定的，优化过程中仅对当前帧的姿态进行优化。具体流程将在后续章节中介绍。 ##### 4.3.1. 基于窗口的优化 本节详细介绍了通过基于滑动窗口的优化过程来优化姿态的方法。在前一节中，跟踪过程通过逐帧匹配来估计6自由度相对运动。一旦跟踪过程成功完成，当前帧的姿态就会通过滑动窗口优化过程进一步细化，以减少漂移并提升局部精度。与逐帧跟踪模块类似，我们也采用了光度误差模型。其与逐帧跟踪过程的主要区别在于：滑动窗口优化是在全局地图坐标系下进行的，且我们利用滑动窗口中多个关键帧的图像块来优化当前帧的姿态。 从图4中可以看出，滑动窗口$\mathcal{K}\mathcal{F}_i$中的每个关键帧$\mathbf{W}_{\mathcal{K}\mathcal{F}}$都有各自对应的图像$\mathcal{I}_i$、显著的激光雷达点$\mathbf{P}_i$以及姿态$\mathbf{T}_i$。滑动窗口$p_k^1$中第一个关键帧$\mathcal{K}\mathcal{F}_1$上的一个点$\mathbf{W}_{\mathcal{K}\mathcal{F}}$会通过……投影到当前帧的图像$\mathcal{I}_f$上。 --- 坐标变换矩阵为$T_{1}^{c}$。由于关键帧的位姿是在地图坐标系中定义的，因此我们可以通过以下公式求得初始变换$T_{1}^{c}$： $$ \mathbf{T}_{1}^{c}=\mathbf{T}_{c}\mathbf{T}_{k f}^{1}{}^{-1} $$ 其中，$\mathbf{T}_c$是通过帧间跟踪得到的当前帧的位姿，而$\mathbf{T}_{kf}^1$则表示$\mathbf{W}_{\mathcal{K}\mathcal{F}}$中第一个关键帧的位姿。因此，当前帧与第一个关键帧之间点$p_k^1$的残差定义如下： $$ r(p_{k}^{1})=\mathcal{I}_{f}(\pi(\mathbf{T}_{1}^{c}p_{k}^{1}))-\mathcal{I}_{k f}^{1}(\pi(p_{k}^{1})) $$ ![Image](images/3c874df37111113bbc040ce0a72f0e3ad75b6bcb5b2de2d9541cf3ed7c683a0c.jpg)
Figure 4. Illustration of sliding window-based optimization, the light blue area represents the local keyframes window ( $W_{K,F}$) which consists of N keyframes, and each keyframe has its image data and LiDAR points. Black dots represent the salient LiDAR points and colored rectangles represent the patches lying in the image. A point $p_{k}^{1}$ in the first keyframe of ( $W_{K,F}$) is projected into current frame with the transform $T_{1}^{c}$. A photometric residuals are then calculated between the patch at the existing point and the other patch at a projected point.
接着，对于滑动窗口中每个关键帧里的每个显著激光雷达点，都可以通过上述公式(9)得到其光度残差。 最终，我们可以利用如下所示的目标函数(10)来更新当前帧的位姿： $$ \mathbf{E}_{r e f i n e}=\frac{1}{2}\sum_{\mathcal{K F}_{i}\in\mathbf{W}_{\mathcal{K F}}}^{i=1:N}\sum_{p_{k}\in\mathbf{P}_{i}}\sum_{p_{j}\in\Omega_{p_{k}}}w\big(r(p_{j})\big)\big(r(p_{j})\big)^{2} $$ 其中，$\mathbf{N}$表示窗口内关键帧的数量，$p_k$指的是图2b中所展示的关键帧中的显著点，而$\Omega$则对应图3a中所展示的稀疏模式下的补丁。权重函数$w(\cdot)$采用了上一节中提到的t分布。完整的两阶段深度增强直接视觉里程计算法在算法1中进行了总结。 --- ```pseudo // Algorithm 1: Two-stage based Direct Visual-LiDAR odometry. Input: The current image $\mathcal{I}_n$ of frame $\mathcal{F}_n$ and the last image $\mathcal{I}_m$ of frame $\mathcal{F}_m$; The salient lidar points $\mathbf{P}_m$ of last frame; The initial transform between current frame and last frame obtained via constant velocity motion model: $\mathbf{T}_n^m$; The number of image pyramid levels $\mathbf{N}_p$; The maximal iteration of G-N optimization in each level $\mathbf{N}_{mi}$; The sliding window $\mathbf{W}_{\mathcal{K}, \mathcal{F}}$ and its size $\mathbf{N}_w$; The salient points $\mathbf{P}_k$ of each keyframe $\mathcal{K}, \mathcal{F}_k$ in $\mathbf{W}_{\mathcal{K}, \mathcal{F}}$; Output: The current frame pose $\mathbf{T}_n$; 1: //(1) The first stage: direct frame-to-frame visual LiDAR odometry 2: for $i = 1$ to $\mathbf{N}_p$ do 3: set flag = false; 4: for $j = 1$ to $\mathbf{N}_{mi}$ do 5: for each point $p_m \in \mathbf{P}_m$ do 6: Compute photometric residual $r'(p_m)$ using Equation (5); 7: Add $r'(p_m)$ to vector $V_{res}$; 8: if flag == false then 9: Compute Jacobian matrix of Equation (6); 10: end if 11: end for 12: set flag = true; 13: Compute the median value $\mu_r$ variance $\delta_r$ of residual vector $V_{res}$; 14: Compute the weight of each measurement using Equation (7); 15: Calculate the incremental transform $\delta\xi \leftarrow \text{GaussianNewtonSolver}()$ and update transformation using the equation: $\mathbf{T}_m^n \leftarrow \mathbf{T}_m^n \exp(-\delta\xi^\wedge)$; 16: if the non-linear optimization converges then 17: Break; 18: end if 19: end for 20: end for 21: Update current frame pose using: $\mathbf{T}_n = \mathbf{T}_m^n \mathbf{T}_m$ 22: // (2) The second stage: Slide window optimization 23: for $i = 1$ to $\mathbf{N}_p$ do 24: for $j = 1$ to $\mathbf{N}_{mi}$ do 25: for $k = 1$ to $\mathbf{N}_w$ do 26: Compute the relative transform between current frame $\mathcal{F}_n$ and keyframe $\mathcal{K}, \mathcal{F}_k$ using Equation (2); 27: for each point $p_m \in \mathbf{P}_k$ do 28: Compute photometric residual $r'(p_m)$ using Equation (9); 29: Add $r'(p_m)$ to vector $V_{res}$; 30: Compute Jacobian matrix of Equation (10); 31: end for 32: end for 33: Compute the median value $\mu_r$ variance $\delta_r$ of residual vector $V_{res}$; 34: Compute the weight of each measurement using Equation (7); 35: Calculate the incremental transform $\delta\xi \leftarrow \text{GaussianNewtonSolver}()$ and update frame pose using the equation: $\mathbf{T}_n \leftarrow \exp(\delta\xi^\wedge) \mathbf{T}_n$ 36: if the non-linear optimization converges then 37: Break; 38: end if 39: end for 40: end for ``` ##### 4.3.2. 关键帧生成与滑动窗口管理 滑动窗口中的关键帧数量被维持在固定值 $N_{w}$。滑动窗口通过保持固定数量的关键帧，来确保VO模块的处理时间恒定。我们用于生成关键帧并进行管理的标准也基于此。 --- 滑动窗口的重叠比率为 $\gamma_{kf}$，时间间隔为 $t_{kf}$，这一设定与DVL-SLAM相同；更多细节可参见 [^30] 和 [^31]。 #### 4.4. 激光雷达建图 由于激光雷达传感器具有测量精度高、抗干扰能力强、感知范围和视角广等优势，因此每当生成新的关键帧$\mathcal{K} \mathcal{F}_k$时，视觉里程计估算出的位姿$\mathbf{T}_k$会进一步由激光雷达建图模块进行优化。激光雷达建图模块包含两大步骤：一是激光雷达特征提取与去畸变步骤，二是扫描到地图的配准步骤。 ##### 4.4.1. 特征提取 特征提取模块由图5所示的三个子过程组成。首先，采用基于快速直线拟合的地平面提取方法[^59]，将原始激光雷达点云分割为地面点云和非地面点云。接着，通过基于距离图像的分割方法[^60]将非地面点云划分为点簇。最后，在剔除掉那些小簇之后，按照LeGO-LOAM中的描述流程[^19]从剩余的点簇中提取边缘特征和平面特征。特征提取过程的可视化结果如图6a–f所示。需要注意的是，与LeGO-LOAM不同，我们的平面特征点不包含地面点。 ![Image](images/bcd9f99fc77f0b21551c14c89dd08318f69b23e7eeeddb758316466f33134483.jpg)
Figure 5. LiDAR feature extraction flowcart. Among them, the yellow represents the input, and the green represents the output result. The gray rectangle represents the processing process, and the rounded rectangle represents the variable.
![Image](images/b7ecbd50266f976fdec0fa3e36ada0a4a188ac4f94cdf47cf72b09954ae88f74.jpg)
Figure 6. Feature extraction process for a scan (000000.bin) on sequence 00 of the KITTI odometry benchmark. (a) is all laser points in the current frame. (b) is the result of ground segmentation. (c) is the segmentation result of non-ground point cloud clusters, and different colors represent different point cloud clusters obtained by LiDAR segmentation. (d,e) represents the extracted planar points and edge points respectively. (f) is the visualization of ground points, planar points and edge points. The green ones represent the ground points, the yellow ones represent the planar points, and the blue ones represent the edge points respectively.
--- ##### 4.4.2. 激光雷达特征去畸变 由于激光点云并非同时采集，在数据采集平台移动过程中会产生一定畸变。为了降低激光雷达畸变对运动估计的影响，我们采用了基于线性插值的激光雷达去畸变方法。线性插值的细节可参见 [^17] 和 [^18]，而线性插值所需的相对变换则由视觉里程计模块提供。最终，我们就能得到无畸变的当前关键帧特征点云 $\mathcal{K}_{\mathcal{F}_k}$，它们分别是地面特征 $\mathbf{Q}_g$、边缘特征 $\mathbf{Q}_e$ 以及平面特征 $\mathbf{Q}_p$。 ##### 4.4.3. 扫描到地图匹配 目前，我们已经获得了初始位姿 $\mathbf{T}_k$，以及当前关键帧 $\mathbf{Q}^k = \{\mathbf{Q}_g^k, \mathbf{Q}_e^k, \mathbf{Q}_p^k\}$ 的 LiDAR 特征 $\mathcal{K}_{f_k}$。现在，LiDAR 扫描到地图匹配模块将 $\mathbf{Q}_e^f$、$\mathbf{Q}_p^f$、$\mathbf{Q}_g^f$ 中的特征分别与周围点云地图 $\mathbb{Q}^{k-1} = \{\mathbb{Q}_e^{k-1}, \mathbb{Q}_p^{k-1}, \mathbb{Q}_g^{k-1}\}$ 进行匹配，以进一步精化位姿，然后使用精化后的位姿将特征 $\mathbf{Q}^k$ 融合到局部点云地图中。需要注意的是，为了提高效率，特征点云 $\mathbf{Q}^k$ 和局部特征地图 $\mathbb{Q}^{k-1}$ 已经使用 PCL [^61] 中实现的基于体素的滤波器进行了下采样，边缘特征、平面特征和地面特征的分辨率分别为 $r_e, r_p$ 和 $r_g$。此外，KD-Tree [^62] 方法也用于加速特征关联的速度。 扫描到地图配准包含两类残差，即点到线残差和点到面残差，分别如图7b、c所示。本文所用配准过程的细节可参见Loam_livox [^57]。唯一的不同之处在于：我们在优化中将地面特征点视为独立的特征，不过地面特征点的残差与平面特征点的残差相同。 ![Image](images/0da78b0a6217626f2e8456e1649dbbe97b32de9732a76f36eebedcf794838e62.jpg)
(a) Illustration of scan to map registration step
![Image](images/d141e2e27708859fa0fc528b58fa128934525afde36df9549f4634fc606164ec.jpg)
(b) Residual of point-to-line
![Image](images/14f25b9e865dba24b59acbefb705658285a7efabd127ef12d6e8aade8d5ff400.jpg)
(c) Residual of point-to-plane
Figure 7. (a) Illustration of scan to map matching and map registration. (b) Residual of point to line. (c) Residual of point to plane.
#### 4.5. 闭环检测 闭环检测是SLAM中至关重要且极具挑战性的问题，其作用是消除漂移误差。更重要的是，它还能防止同一地标被多次注册，从而生成全局一致的地图，供后续机器人定位使用。本节将重点介绍以下内容：首先，我们在4.5.1节简要介绍了现有的基于视觉和激光雷达的闭环检测方法及其存在的问题；接着，针对现有问题，我们在4.5.2节介绍了所采用的思路与整体流程；最后，从4.5.3节至4.5.5节将详细介绍我们方法的具体流程。 ##### 4.5.1. 现有的基于视觉和激光雷达的闭环检测方法及其存在的问题 现有的闭环检测研究分为基于视觉的方法和基于激光雷达的方法。我们在KITTI数据集上评估了BoW和LiDAR-Iris方法的性能，结果如图8所示。闭环的真实情况是根据 [^44] 中的协议B策略生成的。在KITTI序列05中，所有闭环均发生在同一方向上，而在KITTI 08中，绝大多数闭环发生于 --- 方向相反。在BoW亲和矩阵中，图像中的数值越大，两帧之间的相似性就越高；而在LiDAR-Iris亲和矩阵中，图像中的数值越小，两帧之间的相似性就越高。 如图8所示，BoW方法在仅包含同方向回环的序列05上表现出较高的精度。然而，当序列仅包含反方向的回环（如KITTI序列08）时，该方法无法检测到任何回环，而LiDAR-Iris方法在同方向和反方向回环事件中均取得了不错的效果。不过，LiDAR-Iris方法仍存在一些不足。一方面，许多负匹配对也显示出较低的匹配值，这会大大增加验证的负担；另一方面，在我们评估的KITTI序列05和KITTI序列08实验中，从LiDAR-Iris图像中提取二进制特征并匹配两个二进制特征图的平均计算时间约为0.021秒。因此，需要采用半径搜索策略以确保SLAM系统的效率。然而，当累积漂移量大于搜索半径时，该策略可能会导致漏检。 ![Image](images/f59c4baa134f1994671b68eed04df3faeba2b1828677e95f87aa0144afaa0ef9.jpg) ![Image](images/ad7231a3e07209d495789949c51d1f798f5b3bce7bfa07d6697bdcc5b0d26f37.jpg) ![Image](images/40b7c48884cc81892231bf9f9002824d53d0c4e4b39af2e13d9a2c1a13ff88e6.jpg) ![Image](images/698e44ca3f49c13855b690ffd7499f630ad1272e079792f503096d794debaea5.jpg)
(a) GT Loop Closure
(b) GT Affinity Matrix
![Image](images/d9d45f365647ae2ecdc08120a04173643277bd5494776c4b0319febe3ab317ed.jpg)
(c) DBoW Affinity Matrix
(d) LiDAR-Iris Affinity Matrix
Figure 8. Affinity matrices obtained by the Bow and LiDAR-Iris method on two sequences. The first row corresponds to KITTI sequence 05, and the second row corresponds to KITTI sequence 08. (a) Ground Truth loop closure with trajectory. (b) Ground truth affinity matrix. (c) BoW affinity matrix and (d) is LiDAR-Iris affinity matrix respectively.
##### 4.5.2. 并行全局与局部搜索回环检测方法 所提出的PGLS-LCD方法的流程图如图9所示。对于每个新添加的关键帧，首先会提取其特征，包括用于BoW的ORB特征和全局特征LiDAR-Iris。然后，利用BoW和LiDAR-Iris技术并行提取回环候选者。一旦成功提取出回环候选者，便通过一致性验证过程来确认是否存在真正的回环。之后，首先使用TEASER [^47]估计正确的变换，再由V-GICP [^48]进行精细化处理。最后，将精确的变换作为约束添加到位姿图中。当位姿图优化完成后，会根据关键帧的最新位姿更新回环数据库和点云地图。具体过程将在后续章节中介绍。 --- ![Image](images/93dfd6c671135d9ffe696fc94ba1e2f1510f3794cd0879cf0cfe708f7685dfb5.jpg)
Figure 9. The proposed PGLS-Loop Closure Detection flowchart.
##### 4.5.3. BoW向量提取与LiDAR-Iris描述符提取 每次在后端插入新的关键帧$\mathcal{K}\mathcal{F}_k$时，会先提取图10c所示的ORB特征和图10b所示的LiDAR-Iris特征。随后，ORB特征会通过离线生成的词袋模型转换为BoW向量$v_k$。同时，还会从LiDAR-Iris图像中用四个LoG-Gabor滤波器提取二进制特征图$f_k$。因此，一个关键帧$\mathcal{K}\mathcal{F}_k$可以用$v_k$和$f_k$来表示。 ![Image](images/e00ce7d3235178d9da8b36e3c975509de0ee18c59b3713f416db5b140cccf0c8.jpg)
(a)
![Image](images/5722868f0db2ea50b8a8da9a557395f84384fb9a07ad97f26b3493dea33a84f7.jpg)
(c)
![Image](images/8078c6af1bc998c18de0e39e6c4eeed3b7bb724ab1f5411cebd51a54fca99844.jpg)
(b)
![Image](images/fadbbbbc7ac719a022e2ddc7571edca0a2598ec05b1b30b842f8c184f0673c48.jpg)
(d)
Figure 10. Features extraction for loop closure detection. (a) Illustration of encoding height information of surrounding objects into the LiDAR-Iris image. (b) Two LiDAR-Iris images (bottom row) extracted from the bird's eye views of two LiDAR point clouds (top row) in a 3D (x,y,yaw) pose space, respectively. (c) ORB features extract from raw image. (d) shows the rotation invariance in matching two point clouds shown in (b).
##### 4.5.4. 回环候选者提取 为防止误检，我们采用动态相似度阈值作为允许回环候选者的标准，该阈值是根据当前关键帧及其相邻关键帧计算得出的。值得一提的是，ORB-SLAM2也采用这种方法来提升回环检测的性能。完整的回环候选者提取过程总结在算法2中。 --- ```pseudo // Algorithm 2: Loop closure candidates extraction. Input: The BoW vector $v_b$ and the LiDAR-Iris feature $f_k$ of current keyframe $\mathcal{F}_k$; The pose of current keyframe $\mathbf{T}_k$; The search radius $\phi$; Number of adjacent keyframes N; Minimal keyframe gaps between current keyframe and candidate keyframe $N_g$; Output: The loop closure candidate keyframe set $\mathcal{L}_{kf}$; 1: Extract the N adjacent keyframes of $\mathcal{F}_k$ into set $Adj_{kf}$; 2: // parallel addition for CPU with multi cores for extracting loop candidates via BoW and LiDAR-Iris features. 3: // (1) extracting loop candidates via BoW. 4: for each keyframe $\mathcal{F}_i$ in adjacent keyframe set $Adj_{kf}$ do 5: Compute the similarity score $s_i$ of BoW vector between $\mathcal{F}_i$ and $\mathcal{F}_k$, and insert to vector $S_b$; 6: end for 7: Compute maximal and minimal similarity score in $S_b$, $s_{min} = \min(s_1, s_2, ..., s_N)$ and $s_{max} = \max(s_1, s_2, ..., s_N)$, where $s_i$ is the $i - th$ value in $S_b$; 8: Calculate local BoW threshold $s_{th} = \max(s_{min}, 0.8 \times s_{max})$ 9: Extract loop closure candidates using BoW database and insert those keyframes into $\mathcal{L}_{kf}$ if the similarity score between candidate keyframe and $\mathcal{F}_k$ is beyond $s_{th}$, and the keyframe index interval is larger than $N_g$; 10: // (2) extracting loop candidates via LiDAR-Iris. 11: for each keyframe $\mathcal{F}_i$ in adjacent keyframe set $Adj_{kf}$ do 12: Compute the similarity score $s_i$ of LiDAR-Iris feature between $\mathcal{F}_i$ and $\mathcal{F}_k$, and insert to vector $S_l$; 13: end for 14: Compute maximal and minimal similarity score in $S_l$, where $s_{min} = \min(s_1, s_2, ..., s_N)$ and $s_{max} = \max(s_1, s_2, ..., s_N)$; 15: Calculate local LiDAR-Iris threshold $s_{th} = \max(s_{min}, 0.8 \times s_{max})$; 16: Using RadiusSearch( $T_k, \phi$) to extract nearby keyframes and insert to set $N_{kf}$; 17: for Each keyframe $\mathcal{F}_i$ in set $N_{kf}$ do 18: if The LiDAR-Iris similarity score between $\mathcal{F}_i$ and $\mathcal{F}_k$ is smaller than $s_{th}$, and the keyframe index interval is larger than $N_g$ then 19: Inseart $\mathcal{F}_i$ to loop closure candidate keyframe set $\mathcal{L}_{kf}$; 20: end if 21: end for ``` ##### 4.5.5. 一致性验证 给定一个新的查询关键帧$\mathcal{K}_{\mathcal{F}_m}$和一个候选关键帧$\mathcal{K}_{\mathcal{F}_n}$。如前一节所述，全局描述符LiDAR-Iris是对原始点云的高度简化表示。因此，不可避免地会忽略一些特征，这可能导致误检。同样，如果存在类似或重复的场景，使用基于BoW的方法时也很容易出现误检。因此，在闭环之前有必要检查一致性。在本文中，我们采用以下两种一致性验证方法： ### 1. 时间一致性检查 在SLAM系统中可以观察到，单次闭环的出现往往意味着相邻帧之间的高相似性，因为传感器的反馈在时间上是连续的[^63]。我们可以通过测量BoW的时间一致性$\zeta_{bow}$以及LiDAR-Iris的时间一致性$\zeta_{iris}$来验证闭环是否成立： $$ \begin{aligned}\zeta_{bow}&=\frac{1}{N_{c}}\sum_{i=1}^{N_{c}}(\varGamma_{BoW}(v_{m},v_{n-i}))\\\zeta_{iris}&=\frac{1}{N_{c}}\sum_{i=1}^{N_{c}}(\varGamma_{Iris}(f_{m},f_{n-i}))\end{aligned} $$ --- 其中$N_c$是用于时间一致性验证的帧数，$\Gamma_{BoW}$和$\Gamma_{Iris}$分别表示BoW和LiDAR-Iris的相似度，具体计算方法可分别参见[^37]和[^44]。需要注意的是，如果$\mathcal{K}\mathcal{F}_m$与$\mathcal{K}\mathcal{F}_n$之间的视角差大于90度，则$v_{n-i}$和$f_{n-i}$会相应地变为$v_{n+i}$和$f_{n+i}$。通过设定最终时间一致性得分的阈值$\zeta$，即可判定该闭环候选是否可被接受。 ### 2. 几何一致性检查 几何一致性检查流程如图11所示。分别从查询关键帧和候选关键帧的邻域构建的局部点云图中提取快速点特征直方图（FPFH）[^64]特征，然后进行匹配以找到特征对应关系。随后，利用TEASER[^47]根据特征对应关系得到刚体变换矩阵的初始估计。最后，以这个初始估计为起点，应用V-GICP[^48]来计算由查询关键帧$\mathcal{F}_m$和候选关键帧$\mathcal{F}_n$生成的局部点云图之间的最小距离误差。如果适配度得分低于阈值$\eta$，则说明成功检测到闭环，此时相对位姿变换$\mathbf{T}_m^n$会被作为闭环约束添加到位姿图中。 需要注意的是，当使用基于ICP的方法或直接跟踪方法（如DVL-SLAM）时，需要初始变换来计算相对位姿变换。然而，在闭环模块中初始估计往往是未知的。因此，这些方法很容易陷入局部极值。在本文中，我们采用TEASER来获取刚体变换矩阵的初始估计。TEASER是一种对初始变换具有不变性的全局配准方法，从而使我们的方法更加鲁棒。 ![Image](images/59149951d965eeaa0f3a88385ba6bec2bae802e86f786b1351bcba2ef863ed41.jpg)
Figure 11. The flow chart of geometry consistency check. The source point cloud and target point cloud were stiched in the coordinate of query keyframe and candidate keyframe using its neighbor keyframes respectively. The yellow point cloud represents the source point cloud and gray represent target point cloud start from FPFH feature extraction and matching process. In the feature extraction and matching process, the red points and blue points represent FPFH feature points respectively, and the green line shows the matching relationship between those points. In order to display clearly, we only show some of the feature points and their matching relationships.
#### 4.6. 位姿图优化 当获得查询关键帧与候选关键帧之间的相对位姿变换$T_m^n$后，我们将其作为闭环约束添加到位姿图中，随后会通过CeresSolver执行全局位姿图优化以减少漂移。需要注意的是，位姿图优化后会更新激光雷达建图所使用的局部点云图。此外，每次完成位姿图优化后，闭环检测过程会暂停一段时间$t_s$以确保效率。 --- ### 5. 实验结果 为了评估我们方法的性能，我们分别使用了KITTI数据集[^65]、NuScenes数据集[^66]以及实地测试实验，这些场景均可同时使用单目相机和激光雷达传感器。所提出的方法在Ubuntu 16.04系统和机器人操作系统（ROS）[^67]下用C++实现。所有实验均采用配备Intel Core i7-7700处理器和32 GB内存的消费级笔记本电脑进行。算法采用的主要参数如表1所示。需注意，系统中使用的其他参数与参考文献中的设置一致。我们的实验安排如下：5.1节首先介绍在KITTI数据集上的结果，以验证该方法在大场景下的性能；5.2节我们在NuScenes数据集上评估该方法，以验证其在雨夜或黑夜等各种场景下的鲁棒性；5.3节表明，与现有的基于激光雷达的方法相比，我们的方法在真实环境中的表现更优；5.4节我们详细分析了算法中各个模块的作用，包括深度增强的直接帧间里程计、基于滑动窗口的优化模块以及动态物体感知的激光雷达扫描到地图配准模块；5.5节展示了我们方法的闭环检测结果；最后，5.6节介绍了各模块的时间效率。
Table 1. Parameters for DV-LOAM.
ModulesParametersDescriptionKITTINuScenesOurs
Visual Odometry$N_p$Number of image pyramid levels333
$N_{mi}$Max iteration of direct tracking in each level100100100
$\gamma_{kf}$Ration for keyframe creation0.70.80.8
$t_{kf}$Time for keyframe creation (s)1.01.01.0
$N_w$Sliding window size (number of frames)335
LiDAR Mapping$\gamma_e, \gamma_p, \gamma_g$The resolution of point cloud voxel downsample (m)0.4, 0.8, 0.80.4, 0.8, 0.80.3, 0.6, 0.6
Loop Closing$\phi$The loop closure searching radius (m)10.010.010.0
$\eta$The geometry consistency threshold40.040.040.0
$t_s$The suspend detection time after pose graph (s) optimization10.010.010.0
$N_g$The minimal gaps between candidate keyframe and last loop keyframe202020
#### 5.1. 在KITTI里程计数据集上的验证 我们使用带有真实轨迹的KITTI序列00-10来评估我们方法的性能。表2中分别列出了这些序列在无闭环（-）和有闭环（*）情况下的结果以供对比，其中涵盖先进的基于激光雷达的框架：LOAM、A-LOAM、LeGO-LOAM，以及先进的视觉-激光雷达融合方法，如DEMO、LIMO、DVL-SLAM以及Huang等人的工作[^35]。需要指出的是，LOAM、DEMO、LIMO、DVL-SLAM以及Huang等人[^35]的结果直接引自相关文献，且未提供旋转误差数据。 ---
Table 2. Translation error [%]/rotation error [deg/100 m] comparison of the proposed DV-LOAM with existing methods on the whole KITTI training dataset, where (-) represents without using loop closure and (*) represents using loop closure. Best results of (-) in relative translational error/relative rotational error are highlighted in red and blue, respectively.
Seq. NoPath Len. (m)EnvironmentLOAM [^18] (-)A-LOAM [^25] (-)LeGO-LOAM [^19] (-)DEMO [^29] (-)LIMO [^27] (-)Huang [^35] (-)DVL-SLAM [^31] (-)Ours (-)Ours (*)
003714Urban0.78/-0.87/0.361.51/0.701.05/-1.12/-0.99/-0.93/-0.65/0.300.66/0.30
014268Highway1.43/-2.64/0.55-/-1.87/-0.91/-1.87/-1.47/-1.74/0.431.74/0.43
025075Urban+Country0.92/-4.88/1.551.96/0.780.93/-1.38/-1.11/-0.97/0.330.84/0.36
03563Country0.86/-1.20/0.631.41/1.000.99/-0.65/-0.92/-0.85/0.480.85/0.48
04397Country0.71/-1.23/0.401.69/0.831.23/-0.53/-0.42/-0.67/-0.39/0.620.39/0.62
052223Urban0.57/-0.66/0.301.01/0.541.04/-0.72/-0.82/-0.54/0.300.43/0.22
061239Urban0.65/-0.62/0.280.90/0.470.96/-0.61/-0.92/-0.65/0.330.70/0.33
07695Urban0.63/-0.58/0.430.81/0.561.16/-0.56/-1.26/-0.51/0.330.42/0.22
083225Urban+Country1.12/-1.18/0.431.48/0.681.24/-1.27/-1.32/-0.89/0.320.93/0.27
091717Urban+Country0.77/-1.10/0.451.57/0.801.17/-1.06/-0.66/-0.73/0.320.75/0.27
10919Urban+Country0.79/-1.46/0.531.81/0.851.14/-0.83/-0.70/-0.87/0.470.87/0.47
avg0.84/-1.49/0.53-/-1.14/0.490.93/0.260.94/0.360.98/0.400.80/0.380.78/0.36
我们使用KITTI评估里程计工具(，访问日期为2021年7月2日)来评估我们的方法在KITTI里程计训练数据集上的表现。相关的轨迹误差长度是100–800米长度范围内的平均值：相对平移误差百分比/每100米的相对旋转误差度数。需注意，所有轨迹均被转换到相机0坐标系下，与真实轨迹的对比结果如图12所示。 ![Image](images/982a5f76d154a58fa5c3f5a34d05fce41c7e2fedd18513b2fd55adce61aa500e.jpg)
(a) sequence 00
![Image](images/c5300fc8639459128454e2356416640409190b1ad57db9e3fd594676ac174f2b.jpg)
(b) sequence 01
![Image](images/dd60d8e48438efde919ab6e81de3f79fb16dadcddf7936ae70dbca9b8004f5dc.jpg)
(c) sequence 02
![Image](images/3a521815f2036e18a88c3992fcd10b0d7f4c410840ca236b79c8bf063e610c2a.jpg)
(d) sequence 03
![Image](images/37008dd8d7e305d3cf0b35d643c8fead589e60b4dd17cdc643b4bee02285a35b.jpg)
(g) sequence 06
(h) sequence 07
![Image](images/d5c8ca0e9200a03fab48e49e7c49de4799eb5155ccf6591136c5110d237a5ae6.jpg)
(e) sequence 04
(f) sequence 05
![Image](images/2fbbd3a4fa97af499b7362868e332db319dbdd538782db2a44117ca53b6f6558.jpg) ![Image](images/1dc56f36e6521786d9521dc63d4a50f88135b8452a926a8064ad261cf8c45569.jpg) ![Image](images/011d78224591298a7e29ddd8f34a7fed418ad5bfca13ec4ada7a209107ed5f0c.jpg)
(i) sequence 08
![Image](images/60e50026664b89bc85805c01cc89445d81b1af971dd0964e57d03d7f1f88c793.jpg)
(j) sequence 09
![Image](images/96b9ba3b2d2d96509362d11d8ed7288e7e6825a95b74c82d6a01a62f3e5f6150.jpg)
(k) sequence 10
Figure 12. Trajectory diagram concerning the training set and the corresponding sequence error results are shown in Table 2. The gray dotted trajectory is ground, which is collected based on differential GPS that the localization precision is around cm level. The optimized odometry of our scheme is presented in the blue trajectory: Ours-, and the trajectory with loop closure is drawn in orange: Ours*, all of these trajectories have been aligned with the ground truth.
##### 5.1.1. 与无闭环的现有方法的精度对比 为了验证里程计的性能，我们在排除闭环的情况下将所提出的算法应用于KITTI里程计基准数据集。从表2中可以看出，在整个KITTI里程计训练数据集中，我们取得了0.80%的平均相对平移误差和0.0038度/米的相对旋转误差，这一结果优于DEMO、LIMO、Huang等人的工作[^35]以及DVL-SLAM等先进的视觉激光雷达里程计融合方法。DEMO和LIMO是基于视觉里程计的融合方法，DEMO中特征点的深度仅通过三角测量网格获取，而LIMO则采用了更复杂的局部平面拟合特征深度提取策略。此外，在束调整过程中还采用了关键帧选择和地标点选择机制，因此LIMO的精度优于DEMO。与LIMO相比，尽管我们方法的旋转误差更大，但我们实现了更高的位置精度，即相对平移误差更低。 --- 该数值从0.93%降至0.80%。原因如下：一方面，视觉特征的提取精度可达到亚像素级别，特征匹配相对稳定，不易受光照等因素影响。因此，基于特征的方法通常比基于直接法的方法具有更高的旋转精度。另一方面，与单目相机相比，激光雷达传感器具备高精度的测距能力，从而带来更高的平移精度，尤其是在结构信息丰富的城区，如表2中所示的序列00、05、06和07。此外，LIMO需要语义分割来区分环境中的动态物体，如汽车和行人，以此提升系统的鲁棒性和精度，并避免移动物体的干扰。不过，语义信息会占用GPU计算资源，通常耗时较长。Huang等人[^35]通过利用点和线进行跟踪与局部建图来改进LIMO，其平移精度几乎与使用语义信息的LIMO相当。与Huang的研究[^35]相比，虽然我们的平均旋转误差略高，但平移误差却从0.94%显著降至0.80%。我们的方法源自DVL-SLAM，通过融合所提出的两阶段直接视觉里程计模块与激光雷达建图模块，取得了更精确的结果，尤其是相对平移精度得到大幅提升，平移误差从0.98%降至0.80%。 此外，与基于激光雷达的方法相比，我们的方法性能也优于LOAM（论文中平移误差为0.84%）、A-LOAM（平移误差1.49%，旋转误差0.0053度/米）以及LeGO-LOAM。另外，LeGO-LOAM在序列01中无法正常工作，该序列为高速公路场景，车辆大部分时间处于高速行驶状态。原因是扫描匹配模块无法提取足够的特征（除地面点外的边缘特征和平面特征）来进行跟踪。 此外，我们针对整个KITTI训练数据集，在不使用回环闭合的情况下，根据路径长度和车辆速度对旋转与平移误差进行了分析，图13展示了DEMO、DVL-SLAM、LIMO、A-LOAM、Huang的研究[^35]以及我们的方法之间的对比曲线。随着速度增加，我们方法的精度先缓慢下降，随后逐渐上升；尤其在高速状态下，精度下降得非常快，如图13a、b所示。这正是我们的方法在序列01中表现相对较差的原因——该序列属于高速公路环境，存在大量动态车辆，且实验平台移动速度极快。图13c、d显示了根据车辆移动路径长度产生的误差情况。如图13c所示，随着累计路径长度的增加，相对平移误差稳步减小；如图13d所示，相对旋转误差也同样降低。 图14展示了所提方法在KITTI里程计序列00上的位置与姿态变化情况。我们的方法显著降低了平移与姿态定位误差。尤其是二维位姿分量（x、z、偏航角），其精度已接近真实值。需要注意的是，DV-LOAM的位置与姿态误差主要源于高度、横滚角和俯仰角的收敛精度问题，这些问题对平坦地形上的导航任务影响较小。 --- ![Image](images/61b2074d97429ebc2d11e5898993d38039bdcba608aa00248f1fdbb6fd61e6bf.jpg)
A-LOAMDEMO-Huang[^12]LIMO-DVL-SLAM-Ours
![Image](images/400a4bc8f1facdac220e5cdd05e8f5de390a7f850bb048a50c10306562d4d780.jpg)
(a)
A-LOAM DEMO Huang $^{[12]}$ LIMO DVL-SLAM Ours
![Image](images/55e0d9b8f856144cb5b905162714fb864945f67fba67d7c1002ca4a1dd2fb769.jpg)
—A-LOAM—DEMO—Huang[^12]—DVL-SLAM—Ours
![Image](images/f1e5ed86bdf32305387dfac3ac1cb5f754f4d39ac4b6885569642c167f603ed5.jpg) ![Image](images/1530ab54f72837012d32afd1670f35f7775e1ec455226dc37ce1f9af694199f2.jpg)
A-LOAM DEMO Huang[^12] DVL-SLAM Ours
(b)
![Image](images/4930e9628f7e3bcd775ccb9dd2ac71c6f5c9f7734372a49e422890da98348b56.jpg)
(e)
(f)
Figure 13. The error analysis performed on the KITTI training dataset between A-LOAM, DEMO, DVL-SLAM, LIMO, Huang et al. [^35], and Ours. Since we only obtain error curves related to vehicle speed (a,b) of LIMO, we show A-LOAM, DEMO, DVL-SLAM, Huang [^35], and our error curves in terms of path length (c,d). The (e,f) show the percentage of different path segment length and vehicle speed in.
![Image](images/465ab83bf23da0b5393aa8e72403362d206fcf448c5d48321efecb71866e040b.jpg)
(a)
![Image](images/72973f6a5c885728ae87f613afeae00c91025bfa015b4af240659afe5b13e421.jpg)
(b)
Figure 14. Change of position and orientation compared with ground truth on the KITTI sequence 00. The trajectories have been transformed to the coordinate system of Camera 0 through extrinsic parameters (xyz→zxy). (a) represents the position change of the camera. (b) represents the change of the Euler angle of the camera.
##### 5.1.2. 引入回环闭合后的精度分析与全局地图展示 闭环检测有助于纠正漂移现象，从而保持全局一致的地图。此外，这些地图可用于GPS信号受限环境中的车辆定位。从表2中可以看出，我们方法的平均平移误差和旋转误差分别为0.78%和0.38 $\frac{deg}{100m}$。全局一致的地图是基于关键帧构建的，更多细节可参见下图15所示的激光雷达特征地图。需要注意的是，为了改善显示效果，图15中并未展示地面激光雷达特征。 --- ![Image](images/96e5e9e7b928c5c2f1fde789b5e70bf5eb61572c879c9fa33a4abab01a51dbfc.jpg)
(e) sequence 00
Figure 15. Mapping results of our approach on (a) sequence 01, (b) sequence 05, (c) sequence 06, (d) sequence 07 and (e) sequence 00 of the KITTI Vision Benchmark, also shown are some representative regions.
#### 5.2. 在NuScenes数据集上的评估 我们也在NuScenes数据集上对我们的方法进行了评估，该数据集是一个公开的大规模数据集，场景中包含许多动态物体，如汽车和行人。表3中展示的结果是使用NuScenes完整数据集（v1.0）第1部分得出的。该数据集共包含85个场景，且这些场景中均不存在闭环。此外，每个场景的轨迹长度均短于200米，因此我们参照[^35]中的做法，以5米、10米、50米、100米、150米、200米为分段标准来计算平均旋转误差和平移误差。总体而言，我们的方法平均平移误差为3.03%，平均旋转误差为0.0399 $\frac{deg}{m}$。图16中展示的地图是利用我们方法估算出的位姿构建的。 ---
Table 3. Translation error [%]/rotation error [deg/100 m] comparison of the proposed DV-LOAM with existing methods on the NuScenes full dataset (v1.0) part 1. Best results in translational error/rotational error are highlighted in red and blue, respectively.
Scene No.EnvironmentLeGO-LOAM [^19]A-LOAM [^25]Ours
77-91Urban5.19/12.583.53/3.441.68/3.28
111-119Urban9.60/15.093.20/3.021.77/3.74
140-148Country8.90/16.325.71/5.383.85/3.69
265-266Urban3.55/11.465.74/4.831.54/3.45
279-282Country8.05/17.743.08/3.144.33/2.85
307-314Country9.03/26.234.40/4.834.10/7.0
333-343Urban10.70/16.465.51/4.271.98/3.3
547-551Urban6.34/13.285.42/2.731.24/2.17
602-604rain,Urban4.53/12.255.79/4.981.17/3.15
606-624rain,Urban6.97/18.785.81/4.721.57/4.00
833-842Country9.07/22.054.34/4.734.77/6.8
935-943Country12.12/21.805.63/4.185.37/3.71
1026-1043Country, Dark night13.79/20.343.28/4.066.04/4.72
avg8.30/17.264.57/4.183.03/3.99
![Image](images/ac842cebf499e01237b9c02d9dd7f4b2e346761e982488f7e2b8fc8b1ad0d32f.jpg) ![Image](images/638cc24d48d8b41f90811296caddee075860ba17ce2a24afcb9fd764062bf72e.jpg)
Figure 16. Mapping results of some representative scenes in nuScenes dataset. In each column, from left to right are sample image in the scene, point cloud map color-encoded with images, and total LiDAR point cloud map, respectively. Where the red spheres in the middle represent the estimated trajectory of the camera.
#### 5.3. 在我们校园数据集上的评估 为了验证算法在现实世界中的准确性，我们利用图17b所示的移动机器人相机-激光雷达平台开展了实地实验。BUNKER (，访问日期：2021年7月2日)移动机器人平台主要配备了Nvidia Jetson Xavier处理器、ZED2立体相机以及VLP-16 3D激光雷达。该机器人在以1.0米/秒的平均速度行驶7.5分钟后，共采集了约4500帧数据。值得一提的是，数据是在武汉大学下午两点左右采集的，因此道路上行人及车辆较多。 --- ![Image](images/00e61d91d4c0ef79411f3553d5af0f5fdd6e5699ee8bc682aef76057a59d9b36.jpg)
(a)
![Image](images/befbd11298e0fadf6cb7c8250b0bca558a837dcdfad1b1f9c1d908048a366701.jpg) ![Image](images/e0b11485671da0cf6d71828cd1d63be0b0c4422ab2c2315695d92f37c58cde63.jpg)
(b)
(c)
![Image](images/5d421570654b82374b0c8c4e0fefbcf0baea32a3ecd7f4230e23ee7ad2a4cea7.jpg)
(d)
Figure 17. Data recording platform and corresponding collection environment. (a) data collection environment; (b) data collection platform; (c,d) are sample data collected at red and yellow box respectively showed in (a), the points are color-coded by their depth value.
##### 5.3.1. 相机与激光雷达的外参标定 我们先前的研究成果CoMask [^68]被用于标定单目相机与3D激光雷达传感器之间的外参。基于CoMask开发的Matlab工具已开源供社区使用，其网址为，访问日期：2021年7月2日。此外，由于ZED2相机的内部参数精度很高，我们直接采用了ZED2的内部参数。左相机的内部参数如下： $$ K=\begin{bmatrix}529.645&0.0&639.045\\ 0.0&529.645&356.408\\ 0.0&0.0&1.0\end{bmatrix} $$ 最终，通过CoMask标定的左相机到激光雷达的外参$T_{camera}^{LiDAR}$如下： $$ \mathbf{T}_{camera}^{LiDAR}=\begin{bmatrix}0.041688&-0.046796&0.998034&-0.016100\\ -0.999124&0.001642&0.041810&0.022844\\ -0.003595&-0.998903&-0.046686&-0.049066\\ 0.0&0.0&0.0&1.0\end{bmatrix} $$ 我们利用这些外参将激光雷达点投影到图像上，结果如图17c、d所示，其中激光点按照深度值进行了颜色编码。 ##### 5.3.2. 在校园数据集上的实验 图18展示了我们方法与现有先进的基于激光雷达的方法（包括A-LOAM和LeGO-LOAM）所生成的轨迹。从图18a可以看出，借助闭环检测技术，我们的方法和LeGO-LOAM在二维平面上均能达到较高的全局精度。由于A-LOAM没有闭环检测模块，无法纠正累积漂移，因此会产生较大偏差。此外，为了获得更高精度的轨迹，LeGO-LOAM需要惯性测量单元（IMU）来提供相邻帧之间的初始变换关系。然而，在本文中，我们 --- 仅使用了激光雷达传感器和单目相机。因此，如图18b所示，LeGO-LOAM的漂移会随时间逐渐增大，尤其是在高度方向上。 ![Image](images/b82f88846042bb1270536162c1aa0bb45d5b0c728532f394328aeff61198293a.jpg)
• Start Point---LeGO-LOAM(-)---Ours(-)
--- A-LOAM---LeGO-LOAM(*)---Ours(*)
(a)
![Image](images/40cf48866c8afb7d459f7994fce4040e9e361cb141bc22fecf857ddac37ba289.jpg)
(b)
Figure 18. The trajectories of proposed method and state of art LiDAR based methods, including A-LOAM and LeGO-LOAM, where (a) is the 2d trajectories in x-z plane and (b) is the corresponding 3-D trajectories.
图19展示了我们的方法与LeGO-LOAM在校园数据集上的建图结果，点云的颜色根据海拔高度在Rviz中编码（，访问于2021年7月2日）。从侧视图可以更清楚地看出，我们的方法在高度方向上的精度更高。图19最右侧一列是俯视图中红色框区域的放大示意图。可以看出，我们的方法几乎没有累积误差，而即使使用了闭环检测功能，Lego-LOAM仍存在一定的累积误差。 ![Image](images/faef4ce4ef7cb9811cc642ee4b911a0220b2af147437ba9703b5e7f2d1e79cb4.jpg) ![Image](images/d65c5d6e6d38ec18b9010c3a58eef9efe080a163dd761cba5836abc86fe47f4c.jpg)
(a)
![Image](images/446a8c66893b1778e6f7afe56c532438cdadd9620787bbedd6fc9276b336866d.jpg) ![Image](images/28eb284220ae01ce2fc16f2007af729442016cc48cd89da745173936641e5d8c.jpg) ![Image](images/9bb4b311af3ac95c04a86c27ebcc937ebd2d56ec68b4e15bd43d4882060f20e5.jpg)
(b)
![Image](images/9b757a3ba4cd0e883746d39b1012c8adcee585363e30dddd1080b73bb6d1708c.jpg)
Figure 19. Top view, side view, and zoom view of the final 3D maps of campus obtained using our method vs LeGO-LOAM. (a) is our method and (b) is LeGO-LOAM.
--- 我们算法生成的校园最终地图的更详细视图如图20所示。从图20可以看出，我们的方法在地图细节方面也具有高精度，例如A处的路边、B处的柱子、建筑物、D处的树木、E处的汽车等。 ![Image](images/65939f8a255d16a27997a2ebbb95e1c29d34c44f245fe35b0b08eef97d77f4b1.jpg) ![Image](images/63533e6078c049d114aa50410c8ec223377f0a584637b23d033ba70c5c653661.jpg) ![Image](images/b44392d5c3f03102cb343e518a716c7cacf83e6afa469f8f812722e9b4088086.jpg) ![Image](images/816911265548d0a59392105f1350b274b3f2c67865086fe4ff04ec9853a7cb3e.jpg) ![Image](images/be0302a5ea2d4d53f53e965ebbf630764b8a58c8a85ea1acfe65f7ee81733589.jpg) ![Image](images/9deeb9e3f53826add476c062c39e2229768e9893cff948f37ce0eebff632ef28.jpg) ![Image](images/405d17229630e82ff6263abc1a0591e260881893a30875aa7f66ffc3423ec468.jpg)
Figure 20. Mapping result of our approach on campus dataset, also shown are some representative regions from (A–F).
#### 5.4. DV-LOAM中各模块的精度分析 为了进一步分析各模块对里程计精度的影响，我们在KITTI数据的训练集上分别评估了每个模块，包括直接帧间视觉激光雷达里程计（Ours FF）、直接帧间到关键帧视觉激光雷达里程计以及带滑动窗口优化模块的帧间视觉激光雷达里程计（Ours FF_SW）。由于不同SLAM系统中创建关键帧的策略不同，我们将帧间到关键帧视觉激光雷达里程计分为三种类型：第一种是每3帧生成一个关键帧（Ours FK_3），第二种是每5帧生成一个关键帧（Ours FK_5），最后一种（Ours FK_*）则按照DVL-SLAM中采用的策略生成关键帧。此外，还纳入了基于激光雷达扫描匹配的帧间里程计（A-LOAM FF）作为对比。整个KITTI训练数据集上的定量评估结果如表4所示。 此外，图21展示了我们的系统在不同模块下以及KITTI训练数据集序列中的激光雷达帧间里程计所得到的几组具有代表性的运动跟踪轨迹。 ---
Table 4. Translation error [%]/rotation error [deg/100 m] comparison of the different direct visual odometry with ICP-based LiDAR odometry on the whole KITTI training dataset.
Seq. NoPath Len. (m)EnvironmentA-LOAM [^25] FFOurs FFOurs FK-3Ours FK-5Ours FK-*Ours FF-SWOurs FF-SW-SM
003714Urban4.11/1.701.08/0.521.00/0.501.67/0.881.12/0.550.95/0.430.65/0.30
014268Highway3.94/0.951.39/0.41-/--/-1.54/0.451.30/0.351.74/0.43
025075Urban+Country7.48/2.510.98/0.331.23/0.501.79/0.761.08/0.400.98/0.330.97/0.33
03563Country4.54/2.151.19/0.481.14/0.531.53/1.121.08/0.500.95/0.880.85/0.48
04397Country1.74/1.130.74/0.430.68/0.390.77/0.670.63/0.380.61/0.260.39/0.62
052223Urban4.13/1.751.20/0.580.88/0.420.96/0.500.98/0.480.79/0.360.54/0.30
061239Urban1.04/0.551.32/0.451.19/0.400.91/0.361.27/0.410.91/0.310.65/0.33
07695Urban3.14/1.991.26/0.851.12/0.701.39/0.881.39/1.550.88/0.550.51/0.33
083225Urban+Country4.85/2.091.60/0.681.49/0.621.68/0.811.47/0.611.39/0.510.89/0.32
091717Urban+Country5.73/1.850.95/0.460.87/0.461.30/0.650.93/0.460.71/0.330.73/0.32
10919Urban+Country3.61/1.781.05/0.490.96/0.421.29/0.660.87/0.430.77/0.360.87/0.47
avg4.03/1.681.16/0.51-/--/-1.12/0.570.94/0.430.80/0.38
![Image](images/509f1d65d5ee76bfd1e142c142655fbaac28585d49fe5cc8335e55b2f9947c06.jpg) ![Image](images/62371d68d42334bbcaa3024c32313a83a5553e6a5d3fdf1c025d33fab958e9d2.jpg) ![Image](images/0f2f1e38fe9705eed4bcf4f42a787b57f2587dab6b40b3367d768ddb932d969b.jpg) ![Image](images/89ea332567acb9e5d314921cec4016b405e3076d884dfa921f1cba0f4574caf0.jpg) ![Image](images/7412324fef4ec73f7412490c3a098a77f9cab69e004da454a704d3b9ba2b4701.jpg) ![Image](images/33d42b79fee2277dcc09e1e93774e72cf07147afa78c253216477b44b8680517.jpg)
Figure 21. Our motion tracking trajectories on several representative KITTI training dataset, with results by our direct frame-to-frame visual LiDAR odometry (Ours FF), the direct visual LiDAR odometry with slide window optimization module (Ours FF_SW) and LiDAR scan matching based frame-to-frame tracking odometry (A-LOAM FF) comparing with the ground truth (Ground Truth) trajectory separately.
##### 5.4.1. 帧间与帧间到关键帧的直接视觉激光雷达里程计对比 我们首先评估了直接帧间视觉激光雷达里程计与直接帧间到关键帧视觉激光雷达里程计的性能。从表4可以看出，尽管Ours FF的平均平移精度（1.16%）略逊于Ours FF_*（1.12%），但Ours FF在旋转误差方面的精度更好，分别为0.51 $\frac{deg}{100m}$和0.57 $\frac{deg}{100m}$。此外，Ours FF_3和Ours FF_5无法在KITTI序列01中定位自身，该序列为高速公路环境且车辆行驶速度较快。为了进一步量化速度和距离对视觉激光雷达里程计精度的影响，图22展示了整个KITTI训练数据集上基于路径长度和车辆速度的误差分析结果。 --- ![Image](images/7e372bdeeee0e82fe6e054a48ebd5c6cd39840de1551c8a4d0e862b0b6a57286.jpg) ![Image](images/77105ec850aa17d013a573f26dc8c3c6c57964af59215f4107c88c90aad6b710.jpg)
(a)
![Image](images/41534635d610634d27228bbdafd97f4a9915f911ddf049394d376f8bce4810c6.jpg)
(c)
![Image](images/b8eec1c12160b3bd55167fd187cdabaaa1ed44ba06d07b512e4e7b215e95d752.jpg)
(d)
Figure 22. The visual odometry error analysis performed on KITTI training dataset.
首先，我们分析了距离对里程计精度的影响。从图22a可以看出，尽管帧间到关键帧的整体平移误差优于帧间模式，但在较短距离（如200米）内，基于帧间的里程计平移误差更低。此外，从图22b可以看出，随着路径长度从100米增加到800米，基于帧间的视觉激光雷达里程计始终能实现更好的旋转误差。 关于车辆速度对里程计精度的影响。从图22c中可以看出，在不同速度下，帧间方法的平移误差比帧到关键帧方法更稳定，尤其是在高速公路等高速场景下。从表4中可知，在序列01中，我们的FF方法的平移误差和旋转误差分别为1.39%和0.41 $\frac{deg}{100m}$，无论是平移还是旋转方面都比FK_*方法更精确，而另外两种帧到关键帧方法则均未能正常工作。此外，从图22d中可见，无论高速还是低速条件下，帧间里程计的旋转精度都高于帧到关键帧方法。 因此，我们可以断言：在较短路径上，直接帧间视觉激光雷达里程计在平移和旋转方面的性能均优于帧到关键帧视觉激光雷达里程计，更适合用于获取鲁棒且精确的初始变换矩阵。 ###### 5.4.2. 直接帧间视觉激光雷达里程计与激光雷达扫描匹配里程计对比 我们还评估了我们的帧间跟踪方法（Ours FF）与基于激光雷达扫描匹配的帧间跟踪方法（A-LOAM FF）的性能。如表4所示，采用帧间视觉激光雷达方法得到的相对平移误差（1.16%）显著低于激光雷达帧间扫描匹配基线方法（4.03%），相对旋转误差（0.51 $\frac{deg}{100m}$）也低于后者（1.68 $\frac{deg}{100m}$）。造成这种情况的原因可能如下：首先，基于贴片的直接跟踪方法结合精确的激光雷达测距数据，即便在图像质量较差的情况下也能获得准确的运动估计。此外，与A-LOAM、LeGO-LOAM这类基于扫描配准的激光雷达里程计相比，Ours FF无需在相邻帧之间进行特征匹配。这一点在高速公路等环境退化的场景中尤为重要。由于激光雷达测量数据较为稀疏且周围环境十分开阔，难以检测到足够的明显特征，这给特征关联带来了巨大挑战，进而导致运动估计出现显著漂移或失效。 --- ##### 5.4.3. 滑动窗口优化的效果 我们还通过表4中的结果，对我们的滑动窗口优化模块（第4.2节）与我们直接帧间视觉激光雷达里程计（第4.1节）的精度进行了对比评估。对比结果显示，滑动窗口优化模块在降低平移误差方面发挥了重要作用，相对平移误差降低了约18.9%（从1.16%降至0.94%），相对旋转误差也从0.51 $\frac{deg}{100m}$降低至0.43 $\frac{deg}{100m}$，降幅约为15.7%。因此，我们可以得出结论：滑动窗口优化模块能够显著减小我们系统中的位姿估计误差。 ##### 5.4.4. 激光雷达扫描到地图优化的效果 扫描到地图匹配策略在基于激光雷达的SLAM系统中被广泛应用[17–19,25,32]，用于提升点云质量并补偿因帧稀疏导致的精度损失。为了在保持效率的同时维持局部精度，我们的系统仅在生成新关键帧时才会启用扫描到地图优化模块（见第4.4节）。从表4可以看出，激光雷达建图模块显著降低了平移误差和旋转误差：平均相对平移误差从0.94%降至0.80%，而平均相对旋转误差则从0.43 $\frac{deg}{100m}$ 降至0.38 $\frac{deg}{100m}$。其原因可归纳为以下几点：一方面，激光雷达传感器对光照、天气或视角不敏感；更重要的是，凭借高精度测距能力和宽测量范围，该方法在各种环境下均展现出出色的鲁棒性和低漂移特性，尤其是在包含大量边缘特征、平面特征和地面特征的城市区域，如KITTI数据集中的序列00、05、06和07。另一方面，扫描到地图匹配的两阶段配准过程也大幅消除了动态物体对运动估计的影响。例如，在KITTI序列04中，路面上存在许多移动车辆，与LOAM相比，我们的方法相对平移误差从0.71%降至0.39%。 #### 5.5. 回环检测结果 为了进一步验证所提出的PGLS-LCD方法的性能，我们在常用于位置识别的KITTI数据集上对该算法进行了评估。测试使用了多个序列，包括00、02、05和08。序列00和05因大多数回环位置均为前向访问而被常用于位置识别；序列02包含前向和后向访问，而序列08中的回环几乎都是后向访问，因此被认为对位置识别更具挑战性。 我们提出的方法在上述序列上的回环检测结果如图23所示，同时也与LeGO-LOAM [^19] 和立体视觉ORB-SLAM2 [^8] [^9] 等现有方法进行了对比。需要注意的是，立体视觉ORB-SLAM2的回环检测模块基于DBoW2 [^37] [^38]，这是一种最先进的基于视觉的位置识别方法。 在大多数SLAM方法中，系统在成功检测到回环并优化位姿图后会暂停检测一段时间。原因如下：首先，位姿图优化耗时较长，频繁的位姿图优化会降低效率；其次，实际上并非回环越多、位姿图优化次数越多，轨迹精度就越高。相反，如果引入了错误的回环，系统会立即崩溃。因此，无法对不同LCD方法的召回率进行定量评估。 --- ![Image](images/cd5ccca04e21933b9533934d300a73643750ed48e4cb4d097815e3ba60df907c.jpg) ![Image](images/53d5e3c3b91cf88216a3efe9a0b4ffb75c0db67313ef40ede533404ff4c4739d.jpg) ![Image](images/c2a968a9485a6dc5c36667d10944e3f05e1e811872033890e6f38c80f8161ea6.jpg)
(a)
![Image](images/030dc02347178caadea2a0eaf7c1cab0aeee5bda3b3a026203327bca732d7efd.jpg)
(b)
![Image](images/bae60691736b79e1e09494bc773b751da5d3e2affee49db2917d9c0a6c5346de.jpg)
(c)
![Image](images/a4d78a60ebc48cd961c151a369b13f8a12209a43df5e1ea8f832989e2c7d997b.jpg)
(d)
Figure 23. The loop closure detection results of partial KITTI sequences, where (a) is sequence 00, (b) is sequence 05, (c) is sequence 08 and (d) is sequence 02. The trajectory of Ground Truth is plot in dash gray line, the blue line and red line represent stereo ORB-SLAM2 and ours trajectories respectively. The loop closures detected by our approach are drawn with red circle, and the detected loop closures of LeGO-LOAM and stereo ORB-SLAM2 are drawn with triangle and pentagon respectively.
为解决这一问题，我们按时间将单个回环对划分为若干片段：时间上连续的回环将被视为一个完整的回环片段 $\mathcal{L} = \{l_i, l_{i+1}, ..., l_j\}$，其中 $l_i$ 表示第i个回环对。真实结果如图24所示，每个彩色框代表一个回环片段。在这种情况下，精确率和召回率可按算法3进行计算： --- ## 算法3 精确率和召回率的计算 ```pseudo // Algorithm 3: The precision and recall rate calculation. Input: The detected loop closure pairs set $C_L = \{l_1, l_2, ..., l_{N_1}\}$; The number of detected loop closure pairs $N_1$; The ground truth loop closure segments group $S_L = \{\mathcal{L}_1, \mathcal{L}_2, ..., \mathcal{L}_{N_2}\}$. ``` 真实闭环片段的数量$N_2$；成功检测到的闭环数量$N_{TP}$；被正确检测到的闭环片段数量$N_{TS}$； 输出结果：精确度p和召回率r； 1：初始化参数$N_{TP} = 0$、$N_{TS} = 0$； 2：遍历$\mathcal{L}_j$中的每个真实闭环片段$S_L$ 3：static flag = false； 4：遍历$L_i$中的每个闭环$C_L$ 5：如果$L_i \in \mathcal{L}_j$成立，则 6：如果flag == false，则 7: $N_{TS} = N_{TS} + 1$; 8： flag = true； 9： 结束判断 10: $N_{TP} = N_{TP} + 1$; 11：跳出循环； 12： 结束判断 13： 结束循环 14： 结束循环 15：计算闭环检测的精确度：$p = \frac{N_{TP}}{N_1}$； 16：计算闭环检测的召回率：$r = \frac{N_{TS}}{N_2}$ ![Image](images/bce79985e24986e0875584bb889e4a7947da9c63de163d8e3896079732c727fc.jpg) ![Image](images/004d3124482814105dfa41194e048260c959992bbe4f890a8218f0cb838c6a90.jpg) ![Image](images/80518a1f92db95feaf8ac92f469ab87566d66c5d55360275ce6dd22f231e29f0.jpg)
(a)
![Image](images/c111f16b90fbb84593545c11b7ca0b832a3737cab3b810ca1855009985f14821.jpg)
(b)
![Image](images/f9048485b9b37687eed9fb04fbad1a7ca67547927ffe42a417ce2681e663903c.jpg) ![Image](images/02ad9b412350f645677df13655759f234a56dd14f5571210e24ddb7c7d71b8dd.jpg)
(c)
![Image](images/6678bf280355421aeefac7c7af975b0428beb7126f83c15feb41a56a5f723fcf.jpg)
(d)
Figure 24. The ground truth loop closure segments of partial KITTI sequences. (a) KITTI sequence 00 with 4 loop closure segments. (b) KITTI sequence 02 with 3 loop closure segments. (c) KITTI sequence 05 with 3 loop closure segments. (d) KITTI sequence 08 with 3 loop closure segments. The top row is the GPS trajectory plotted from light to dark with time going, and the bottom row is the corresponding ground truth affinity matrices. We use the same color to indicate the correspondence between trajectory and affinity matrix.
表5列出了精确度和召回率的结果。在序列00和序列05上，所有方法都取得了具有竞争力的精确度和召回率，这是因为所有出现的闭环都属于正向访问情况。在更具挑战性的数据集序列08上，我们的方法和LeGO-LOAM的召回率明显更高，因为激光雷达传感器受光照、天气或视角的影响较小。而基于视觉的方法无法识别反向访问情况，因此召回率显著下降。对于序列02，既存在正向访问闭环事件，也存在反向访问闭环事件，且第一个闭环片段出现在长时间行驶之后。尽管累积误差较大，但借助BoW的全局搜索能力，立体ORB-SLAM2还是正确检测到了前两个正向闭环片段。相反，LeGO-LOAM未能检测到第一个闭环片段，因为漂移量超出了搜索半径。虽然它成功检测到了第二个闭环片段，但通过ICP计算出的校正变换所产生的误差导致了如图23d所示的全局轨迹严重偏差。我们的方法不仅成功检测到了全部三个闭环片段（包括最后一个反向闭环片段），还借助TEASER获得了闭环对之间正确且精确的相对变换，进而得到了全局一致的轨迹。 --- ### 5.6. 时间效率分析
Table 5. The loop closure precision and recall comparison with existing methods. The $N_{TS}$ in Recall Rate represents the number of detected loop closure, and N2 is the number of all existed loop closure segments. Best results are highlighted in bold.
DatesetApproachsPrecision (%)Recall Rate ( $N_{{TS}}$/N2)
sequence 00stereo ORB-SLAM21004/4
LeGO-LOAM1004/4
Ours1004/4
sequence 02stereo ORB-SLAM21002/3
LeGO-LOAM1001/3
Ours1003/3
sequence 05stereo ORB-SLAM21003/3
LeGO-LOAM1003/3
Ours1003/3
sequence 08stereo ORB-SLAM200/3
LeGO-LOAM1003/3
Ours1003/3
### 5.6. 时间效率分析 DV-LOAM在上述所有测试中均表现出出色的时间效率。在我们方案的视觉里程计模块中，包含两个阶段：直接帧间跟踪和滑动窗口优化，无需进行特征提取与匹配。激光雷达建图模块则包含三个步骤：首先采用基于直线拟合的激光雷达分割方法，将原始点云划分为地面点云和非地面点云；接着，与LeGO-LOAM类似，从非地面点云中提取边缘特征和平面特征；最后，利用三种不同特征通过扫描到地图的优化过程来细化位姿。如图25所示，为了评估里程计的时间效率，我们记录了KITTI里程计基准测试集[^65]中代表性序列00的各个处理阶段的平均运行时间（关于复杂度的直观展示，请参见图15e）。我们使用ROS的nodelet包来减少不同节点间的数据传输时间。 --- ![Image](images/a8cdcd3c7f44862242d7465fdd0e090b257364423ea9d22374ecfdce12cccdd2.jpg)
Figure 25. Processing time of each module on the KITTI sequence 00 odometry benchmark.
此外，我们在KITTI、Nuscenes以及带有回环检测功能的校园数据集上评估了系统各模块的效率。各模块的平均运行时间如表6所示。耗时最多的部分有两个：其一是扫描到地图的优化步骤，每次耗时约75毫秒；其二是回环检测，总耗时约200毫秒。由于扫描到地图的优化仅在生成新关键帧时执行，且每3-4帧才会选取一个关键帧；加之回环检测的发生频率较低，且可在另一个线程中执行，因此我们的方法在KITTI数据集上的平均频率可达6Hz。
Table 6. Average processing time of each module on the KITTI, nuScenes, and Our campus dataset.
ModulesSub ModulesKITTI (ms)nuScenes (ms)Our Campus (ms)
Visual Odometryframe-to-frame tracking25.248.5213.24
sliding window optimization48.3417.7635.73
LiDAR Mappingground extraction12.198.579.46
feature extraction27.8411.3010.23
scan-to-map optimization71.7241.8954.81
Loop Closingloop candidates extraction3.98-3.24
consistency verification110.37-102.53
pose graph optimization81.22-57.78
### 6. 讨论 - 激光雷达帧间里程计与视觉-激光雷达融合里程计：如表4所示，与基于激光雷达扫描到扫描的里程计相比，基于视觉-激光雷达融合的里程计在精度方面表现更优。主要原因如下：首先，激光雷达里程计中提取的特征（边缘点和平面点）是基于曲率选择的，它们之间的区分度不如视觉特征明显，且易受噪声影响，尤其是在乡村等无结构化环境中；其次，由于无法进行特征匹配，基于激光雷达的里程计通常需借助Kd-Tree搜索最近邻点来进行特征点关联，这往往会导致大量错误匹配，使得优化算法无法收敛或收敛到错误位置，尤其是在初始位姿不佳时；第三，由于视觉图像具有更丰富的纹理信息，且激光雷达能提供精确的距离测量数据，因此基于补丁的深度增强型直接视觉-激光雷达里程计即便在图像模糊的情况下也能实现更高的精度。此外，由于采用了金字塔策略，该方法在效率方面也表现更佳。 --- 在 LiDAR 里程计中基于曲率进行选择，它们之间的区分度不像视觉特征那样明显，并且容易受到噪声的影响，特别是在乡村等非结构化环境中。其次，由于无法执行特征匹配，基于 LiDAR 的里程计通常使用 Kd-Tree 来搜索最近邻，然后关联特征点。这常常导致许多错误匹配，使得优化算法无法收敛或收敛到错误的位置，特别是在初始位姿不佳的情况下。第三，由于视觉图像具有更丰富的纹理，并且 LiDAR 可以提供精确的距离测量，因此基于面片深度增强的直接视觉-LiDAR 里程计即使在图像模糊的情况下也能实现更高的精度。此外，由于采用了金字塔策略，该方法在效率方面也表现更好。 动态物体的影响：目前，大多数SLAM方法都是在世界为静态的假设下工作的。然而，在大多数真实环境中，这一假设很难严格成立。周围环境中总会存在移动物体，比如行走的人和行驶的汽车。由于图像的视野远小于具备360度扫描能力的激光雷达，移动物体对视觉方法的影响更大，尤其是当图像中存在大型移动物体时。如图26a、b所示，当有汽车经过时，基于直接法估计的轨迹会出现明显误差，而基于激光雷达的里程计方法则基本不受影响。这主要是因为激光雷达的视野更广，且在特征提取过程中会对激光雷达数据进行分块处理，最终只有少量来自移动物体的特征被提取出来。为了使算法更具鲁棒性，尤其是在动态环境中，我们计划采用以下思路进行改进。一方面，我们考虑将之前的研究成果——基于几何的多运动分割方法[^69]应用到DV-LOAM中，从而消除移动物体的影响并获得静态点云地图。另一方面，我们也计划将基于深度学习的运动分割技术[^70] [^71]引入我们的框架。通过去除位于动态物体中的显著点，能够进一步提升跟踪精度和定位精度。 显著点选择方法的改进：目前，我们采用DVL-SLAM[^30]中描述的策略来提取显著点。该策略首先将当前扫描的所有激光点投影到图像上，然后选取梯度较大的点用于自我运动估计。这里存在两个问题。其一，如[^36]中所述，明显的遮挡点常出现在物体边界处，而位于物体边界上的像素梯度相对较大，因此会选中许多明显的遮挡点，从而降低跟踪精度。另一方面，如[^51]中所介绍的，我们发现边缘点的深度不确定性大于平面点，边缘点在图26c中有所展示。因此，当外部参数不准确，或者相机与激光雷达传感器的时间同步不精确时，那些出现在边界处的所选显著点可能会在跟踪过程中带来更多不确定性。如表7所示，我们比较了采用不同显著点选择策略时的跟踪精度，其中FF(-)代表在显著点选择阶段使用所有激光点的跟踪结果，而FF(*)则是仅使用平面激光点和地面激光点的跟踪结果。尽管整体精度提升并不明显，但我们可以看到在KITTI 06数据集上，FF(*)的精度有显著提升。原因是KITTI 06数据集中存在许多孤立的树木，这会导致大量潜在的遮挡点。在未来的工作中，我们计划将深度不确定性作为权重函数纳入优化过程，以提升直接视觉-激光雷达里程计的性能。 --- ![Image](images/4703faa926a8bdd8dc099a92246d0cf63071156cbab1e2e5ae1cde11dc59cb01.jpg) ![Image](images/ec19aad89572a8b9a86e3a68f108db6acef432a1ccfd477ac123120fdd3d9e1b.jpg)
(a)
(b)
![Image](images/ce6173de61ce023d6878fc78da7bb27db7b85628b2cab1445c6fe57cd55ba75f.jpg)
(c)
Figure 26. (a) The sample images in our campus dataset with moving vehicle. (b) The trajectories of LiDAR-based odometry and direct visual-LiDAR fusion odometry. (c) Edge points extracted in LiDAR point cloud, they are more likely to be located at the boundary of objects.
Table 7. Translation error [%]/rotation error [deg/100 m] comparison of the different salient points selection trajectories in direct frame-to-frame visual-LiDAR odometry. Best results in translational error/rotational error are highlighted in red and blue, respectively.
0001020304050607080910avg
FF(-)1.08/0.521.39/0.410.98/0.331.19/0.480.74/0.431.20/0.581.32/0.451.26/0.851.60/0.680.95/0.461.05/0.491.16/0.51
FF(*)1.01/0.501.29/0.400.96/0.321.08/0.460.77/0.411.06/0.500.97/0.421.29/0.931.60/0.681.02/0.461.11/0.461.11/0.50
## 7. 结论 在本文中，我们提出了一种新颖的直接视觉激光雷达里程计与建图框架，该框架将单目相机与激光雷达的稀疏精确测距数据相结合。所提出的方法依靠以下几个方面来实现卓越的性能：(1) 用于运动估计的两阶段直接视觉里程计模块。由于无需提取和匹配特征，基于深度增强的 Patch 的帧间里程计即使在高速公路或夜间场景中也能表现出很高的鲁棒性。此外，基于滑动窗口的姿态优化能够防止局部漂移，其精度可与 Huang 等人[^35]提出的 DVL-SLAM 等先进的视觉激光雷达方法相媲美。(2) 每次生成关键帧时，激光雷达建图模块都会执行姿态精化处理，以减少累积漂移。(3) 结合了 BoW、激光雷达 Iris 特征以及位姿图优化器的 PGLS-LCD 方法能够在全局层面保持一致性。 --- 我们在 KITTI 数据集、NuScenes 数据集以及校园数据集上评估了该方法的表现，并将其与包括基于激光雷达的方法和视觉激光雷达融合方法在内的先进方法进行了比较。实验表明，通过整合直接视觉里程计模块与激光雷达建图模块，我们的方法相比现有技术能提供更精确的轨迹和符合真实世界指标的点云地图。在闭环检测方面，我们的方法不仅能处理反向访问事件，还能利用所有先前的关键帧进行全局搜索，从而在 KITTI 数据集的各种序列中取得更高的精确率和召回率。我们的方法代码已在 GitHub 上开源，网址为，访问日期为 2021 年 7 月 2 日。 作者贡献：B.L. 指导了算法设计。W.W. 撰写了手稿。W.W. 设计了全部实验与框架。C.W. 协助组织了论文。C.Z. 和 J.L. 为论文撰写提供了建议。B.L. 监督了研究的整个过程并润色了手稿。所有作者均已阅读并同意发表该手稿版本。 资助：本研究未获得外部资助。 机构审查委员会声明：不适用。 知情同意声明：不适用。 数据可用性声明：我们感谢 KITTI 和 NuScenes 提供用于 SLAM 的公开基准测试数据。文中的数据可通过以下链接获取： 和 ，访问日期为 2021 年 7 月 2 日。 致谢：作者谨向编辑和审稿人表达感激之情，他们提出的建设性且有益的意见对本文的显著改进起到了重要作用。 利益冲突：作者声明没有利益冲突。 ## 缩写词 本手稿中使用了以下缩写词：
SLAMSimultaneous Localization and Mapping
VOVisual Odometry
LiDARLight Detection And Ranging
GNSS/INSGlobal Navigation Satellite System/Inertial Navigation System
IMUInertial Measurement Unit
TEASERTruncated least squares Estimation And SEmidefinite Relaxation
FPFHFast Point Feature Histogram
ICPIterative Closest Point
PGLS-LCDParallel Global and Local Search - loop closure detection
BoWBag of Words
## 参考文献 [^1]: Wen, W.; Hsu, L.; Zhang, G. Performance Analysis of NDT-based Graph SLAM for Autonomous Vehicle in Diverse Typical Driving Scenarios of Hong Kong. Sensors 2018, 18, 3928. [CrossRef] [^2]: Carvalho, H.; Del Moral, P.; Monin, A.; Salut, G. Optimal nonlinear filtering in GPS/INS integration. IEEE Trans. Aerosp. Electron. Syst. 1997, 33, 835–850. [CrossRef] [^3]: Mohamed, A.H.; Schwarz, K.P. Adaptive Kalman Filtering for INS/GPS. J. Geod. 1999, 73, 193–203. [CrossRef] [^4]: Wang, D.; Xu, X.; Zhu, Y. A Novel Hybrid of a Fading Filter and an Extreme Learning Machine for GPS/INS during GPS Outages. Sensors 2018, 18, 3863. [CrossRef] [^5]: Liu, H.; Ye, Q.; Wang, H.; Chen, L.; Yang, J. A Precise and Robust Segmentation-Based Lidar Localization System for Automated Urban Driving. Remote Sens. 2019, 11, 1348. [CrossRef] ## [^6]: Klein, G.; Murray, D.W. Parallel Tracking and Mapping for Small AR Workspaces. In Proceedings of the Sixth IEEE/ACM International Symposium on Mixed and Augmented Reality, ISMAR 2007, Nara, Japan, 13–16 November 2007; IEEE Computer Society: IEEE: Piscataway, NJ, USA, 2007; pp. 225–234. [CrossRef] --- [^7]: Klein, G.; Murray, D.W. Parallel Tracking and Mapping on a camera phone. In Proceedings of the 8th IEEE International Symposium on Mixed and Augmented Reality 2009, ISMAR 2009, Orlando, FL, USA, 19–22 October 2009; Klinker, G., Saito, H., Höllerer, T., Eds.; IEEE Computer Society: IEEE: Piscataway, NJ, USA, 2009; pp. 83–86. [CrossRef] [^8]: Mur-Artal, R.; Montiel, J.M.M.; Tardós, J.D. ORB-SLAM: A Versatile and Accurate Monocular SLAM System. IEEE Trans. Robot. 2015, 31, 1147–1163. [CrossRef] [^9]: Mur-Artal, R.; Tardós, J.D. ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras. IEEE Trans. Robot. 2017, 33, 1255–1262. [CrossRef] [^10]: Engel, J.; Schöps, T.; Cremers, D. LSD-SLAM: Large-Scale Direct Monocular SLAM. In Proceedings of the Computer Vision—ECCV 2014—13th European Conference, Zurich, Switzerland, 6–12 September 2014; Fleet, D.J., Pajdla, T., Schiele, B., Tuytelears, T., Eds.; Proceedings, Part II; Lecture Notes in Computer Science; Springer: Berlin/Heidelberg, Germany, 2014; Volume 8690, pp. 834–849. [CrossRef] [^11]: Engel, J.; Koltun, V.; Cremers, D. Direct Sparse Odometry. IEEE Trans. Pattern Anal. Mach. Intell. 2017, 40, 611–625. [CrossRef] [^12]: Forster, C.; Pizzoli, M.; Scaramuzza, D. SVO: Fast semi-direct monocular visual odometry. In Proceedings of the 2014 IEEE International Conference on Robotics and Automation, ICRA 2014, Hong Kong, China, 31 May–7 June 2014; pp. 15–22. [CrossRef] [^13]: Forster, C.; Zhang, Z.; Gassner, M.; Werlberger, M.; Scaramuzza, D. SVO: Semidirect Visual Odometry for Monocular and Multicamera Systems. IEEE Trans. Robot. 2017, 33, 249–265. [CrossRef] [^14]: Besl, PJ.; Mckay, H.D. A method for registration of 3-D shapes. IEEE Trans. Pattern Anal. Mach. Intell. 1992, 14, 239–256. [CrossRef] [^15]: Rusinkiewicz, S.; Levoy, M. Efficient Variants of the ICP Algorithm. In Proceedings of the 3rd International Conference on 3D Digital Imaging and Modeling (3DIM 2001), Quebec, QC, Canada, 28 May-1 June 2001; pp. 145–152. [CrossRef] [^16]: Segal, A.; Hähnel, D.; Thrun, S. Generalized-ICP. In Proceedings of the Robotics: Science and Systems V, Seattle, WA, USA, 28 June–1 July 2009; Trinkle, J., Matsuoka, Y., Castellanos, J.A., Eds.; The MIT Press: Cambridge, MA, USA, 2009; doi:10.15607/RSS.2009.V.021. [CrossRef] [^17]: Zhang, J.; Singh, S. LOAM: Lidar Odometry and Mapping in Real-time. In Proceedings of the Robotics: Science and Systems X, Berkeley, CA, USA, 12–16 July 2014; Fox, D., Kavraki, L.E., Kurniawati, H., Eds.; IEEE: Piscataway, NJ, USA, 2014; doi:10.15607/RSS.2014.X.007. [CrossRef] [^18]: Zhang, J.; Singh, S. Low-drift and real-time lidar odometry and mapping. Auton. Robot. 2017, 41, 401–416. [CrossRef] [^19]: Shan, T.; Englot, B.J. LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain. In Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2018, Madrid, Spain, 1–5 October 2018; pp. 4758–4765. [CrossRef] [^20]: Hong, H.; Lee, B.H. Probabilistic normal distributions transform representation for accurate 3D point cloud registration. In Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2017, Vancouver, BC, Canada, 24–28 September 2017; pp. 3333–3338. [CrossRef] [^21]: Magnusson, M.; Lilienthal, A.J.; Duckett, T. Scan registration for autonomous mining vehicles using 3D-NDT. J. Field Robot. 2007, 24, 803–827. [CrossRef] [^22]: Deschaud, J. IMLS-SLAM: Scan-to-Model Matching Based on 3D Data. In Proceedings of the 2018 IEEE International Conference on Robotics and Automation, ICRA 2018, Brisbane, Australia, 21–25 May 2018; pp. 2480–2485. [CrossRef] [^23]: Chen, X.; Milioto, A.; Palazzolo, E.; Giguère, P.; Behley, J.; Stachniss, C. SuMa++: Efficient LiDAR-based Semantic SLAM. In Proceedings of the 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2019, Macau, China, 3–8 November 2019; pp. 4530–4537. [CrossRef] [^24]: Ruan, J.; Li, B.; Wang, Y.; Fang, Z. GP-SLAM+: Real-time 3D lidar SLAM based on improved regionalized Gaussian process map reconstruction. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2020, Las Vegas, NV, USA, 24 October–24 January 2020; pp. 5171–5178. [CrossRef] 25. Qin, T.; Cao, S. A-LOAM. 2018. 可在线查看：（访问日期：2021 年 7 月 2 日）。 [^26]: Shao, W.; Vijayarangan, S.; Li, C.; Kantor, G. Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping. In Proceedings of the 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2019, Macau, China, 3–8 November 2019; pp. 370–377. [CrossRef] [^27]: Gräter, J.; Wilczynski, A.; Lauer, M. LIMO: Lidar-Monocular Visual Odometry. In Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2018, Madrid, Spain, 1–5 October 2018; pp. 7872–7879. [CrossRef] [^28]: Zhang, J.; Kaess, M.; Singh, S. Real-time depth enhanced monocular odometry. In Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, IL, USA, 14–18 September 2014; pp. 4973–4980. [CrossRef] [^29]: Zhang, J.; Kaess, M.; Singh, S. A real-time method for depth enhanced visual odometry. Auton. Robot. 2017, 41, 31–43. [CrossRef] [^30]: Shin, Y.; Park, Y.S.; Kim, A. Direct Visual SLAM Using Sparse Depth for Camera-LiDAR System. In Proceedings of the 2018 IEEE International Conference on Robotics and Automation, ICRA 2018, Brisbane, Australia, 21–25 May 2018; pp. 1–8. [CrossRef] [^31]: Shin, Y.; Park, Y.S.; Kim, A. DVL-SLAM: Sparse depth enhanced direct visual-LiDAR SLAM. Auton. Robot. 2020, 44, 115–130. [CrossRef] ## [^32]: Zhang, J.; Singh, S. Visual-lidar odometry and mapping: Low-drift, robust, and fast. In Proceedings of the IEEE International Conference on Robotics and Automation, ICRA 2015, Seattle, WA, USA, 26–30 May 2015; pp. 2174–2181. [CrossRef] --- [^33]: Pandey, G.; Savarese, S.; McBride, J.R.; Eustice, R.M. Visually bootstrapped generalized ICP. In Proceedings of the IEEE International Conference on Robotics and Automation, ICRA 2011, Shanghai, China, 9–13 May 2011; pp. 2660–2667. [CrossRef] [^34]: Seo, Y.W.; Chou, C. A Tight Coupling of Vision-Lidar Measurements for an Effective Odometry. In Proceedings of the 2019 IEEE Intelligent Vehicles Symposium, IV 2019, Paris, France, 9–12 June 2019; pp. 1118–1123. [CrossRef] [^35]: Huang, S.; Ma, Z.; Mu, T.; Fu, H.; Hu, S. Lidar-Monocular Visual Odometry using Point and Line Features. In Proceedings of the 2020 IEEE International Conference on Robotics and Automation, ICRA 2020, Paris, France, 31 May–31 August 2020; pp. 1091–1097. [CrossRef] [^36]: Huang, K.; Xiao, J.; Stachniss, C. Accurate Direct Visual-Laser Odometry with Explicit Occlusion Handling and Plane Detection. In Proceedings of the International Conference on Robotics and Automation, ICRA 2019, Montreal, QC, Canada, 20–24 May 2019; pp. 1295–1301. [CrossRef] [^37]: Angeli, A.; Filliat, D.; Doncieux, S.; Meyer, J. Fast and Incremental Method for Loop-Closure Detection Using Bags of Visual Words. IEEE Trans. Robot. 2008, 24, 1027–1037. [CrossRef] [^38]: Gálvez-López, D.; Tardós, J.D. Bags of Binary Words for Fast Place Recognition in Image Sequences. IEEE Trans. Robot. 2012, 28, 1188–1197. [CrossRef] [^39]: Cummins, M.J.; Newman, P.M. FAB-MAP: Probabilistic Localization and Mapping in the Space of Appearance. Int. J. Robot. Res. 2008, 27, 647–665. [CrossRef] [^40]: Dubé, R.; Dugas, D.; Stumm, E.; Nieto, J.I.; Siegwart, R.; Cadena, C. SegMatch: Segment based place recognition in 3D point clouds. In Proceedings of the 2017 IEEE International Conference on Robotics and Automation, ICRA 2017, Singapore, 29 May–3 June 2017; pp. 5266–5272. [CrossRef] [^41]: Dubé, R.; Gollub, M.G.; Sommer, H.; Gilitschenski, I.; Siegwart, R.; Cadena, C.; Nieto, J.I. Incremental-Segment-Based Localization in 3-D Point Clouds. IEEE Robot. Autom. Lett. 2018, 3, 1832–1839. [CrossRef] [^42]: Kim, G.; Kim, A. Scan Context: Egocentric Spatial Descriptor for Place Recognition Within 3D Point Cloud Map. In Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2018, Madrid, Spain, 1–5 October 2018; pp. 4802–4809. [CrossRef] [^43]: Wang, H.; Wang, C.; Xie, L. Intensity Scan Context: Coding Intensity and Geometry Relations for Loop Closure Detection. In Proceedings of the 2020 IEEE International Conference on Robotics and Automation, ICRA 2020, Paris, France, 31 May–31 August 2020; pp. 2095–2101. [CrossRef] [^44]: Wang, Y.; Sun, Z.; Xu, C.; Sarma, S.E.; Yang, J.; Kong, H. LiDAR Iris for Loop-Closure Detection. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2020, Las Vegas, NV, USA, 24 October–24 January 2020; pp. 5769–5775. doi:10.1109/IROS45743.2020.9341010. [CrossRef] [^45]: Chen, X.; Labe, T.; Milioto, A.; Röhling, T.; Vysotska, O.; Haag, A.; Behley, J.; Stachniss, C. OverlapNet: Loop Closing for LiDAR-based SLAM. CoRR 2021, abs/2105.11344. [^46]: Wang, Y.; Qiu, Y.; Cheng, P.; Duan, X. Robust Loop Closure Detection Integrating Visual-Spatial-Semantic Information via Topological Graphs and CNN Features. Remote Sens. 2020, 12, 3890. [CrossRef] [^47]: Yang, H.; Shi, J.; Carlone, L. TEASER: Fast and Certifiable Point Cloud Registration. IEEE Trans. Robot. 2021, 37, 314–333. [CrossRef] [^48]: Koide, K.; Yokozuka, M.; Oishi, S.; Banno, A. Voxelized GICP for Fast and Accurate 3D Point Cloud Registration; EasyChair: Manchester, UK, 2020. [^49]: Lin, J.; Zheng, C.; Xu, W.; Zhang, F. R² LIVE: A Robust, Real-Time, LiDAR-Inertial-Visual Tightly-Coupled State Estimator and Mapping. IEEE Robot. Autom. Lett. 2021, 6, 7469–7476. [CrossRef] [^50]: Zhu, Y.; Zheng, C.; Yuan, C.; Huang, X.; Hong, X. CamVox: A Low-cost and Accurate Lidar-assisted Visual SLAM System. CoRR 2020, abs/2011.11357. [^51]: Voges, R.; Wagner, B. Interval-Based Visual-LiDAR Sensor Fusion. IEEE Robot. Autom. Lett. 2021, 6, 1304–1311. LRA.2021.3057572. [CrossRef] [^52]: Reinke, A.; Chen, X.; Stachniss, C. Simple But Effective Redundant Odometry for Autonomous Vehicles. CoRR 2021, abs/2105.11783. [^53]: Chen, S.; Zhou, B.; Jiang, C.; Xue, W.; Li, Q. A LiDAR/Visual SLAM Backend with Loop Closure Detection and Graph Optimization. Remote Sens. 2021, 13, 2720. [CrossRef] [^54]: Smith, P.; Reid, I.; Davison, A.J. Real-Time Monocular SLAM with Straight Lines. In Proceedings of the British Machine Vision Conference 2006, Edinburgh, UK, 4–7 September 2006; Chantler, M.J., Fisher, R.B., Trucco, E., Eds.; British Machine Vision Association: Durham, UK, 2006; pp. 17–26. [CrossRef] [^55]: Gee, A.P.; Mayol-Cuevas, W.W. Real-Time Model-Based SLAM Using Line Segments. In Proceedings of the Advances in Visual Computing, Second International Symposium, ISVC 2006 Lake Tahoe, NV, USA, 6–8 November 2006; Bebis, G., Boyle, R., Parvin, B., Koracin, D., Remagnino, P., Nefian, A.V., Gopi, M., Pascucci, V., Zara, J., Molineros, J., et al., Eds.; Proceedings, Part II; Lecture Notes in Computer Science; Springer: Berlin/Heidelberg, Germany, 2006; Volume 4292, pp. 354–363. [CrossRef] ## [^56]: Klein, G.; Murray, D.W. Improving the Agility of Keyframe-Based SLAM. In Proceedings of the Computer Vision—ECCV 2008, 10th European Conference on Computer Vision, Marseille, France, 12–18 October 2008; Forsyth, D.A., Torr, P.H.S., Zisserman, A., Eds.; Lecture Notes in Computer Science; Proceedings, Part II; Springer: Berlin/Heidelberg, Germany, 2008; Volume 5303, pp. 802–815. [CrossRef] --- [^57]: Lin, J.; Zhang, F. Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV. In Proceedings of the 2020 IEEE International Conference on Robotics and Automation (ICRA), Xi'an, China, 30 May–5 June 2020; pp. 3126–3131. [CrossRef] [^58]: Kerl, C.; Sturm, J.; Cremers, D. Robust odometry estimation for RGB-D cameras. In Proceedings of the 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 6–10 May 2013; pp. 3748–3754. [CrossRef] [^59]: Himmelsbach, M.; von Hundelshausen, F.; Wünsche, H. Fast segmentation of 3D point clouds for ground vehicles. In Proceedings of the IEEE Intelligent Vehicles Symposium (IV), 2010, La Jolla, CA, USA, 21–24 June 2010; pp. 560–565. [CrossRef] [^60]: Bogoslavskyi, I.; Stachniss, C. Fast range image-based segmentation of sparse 3D laser scans for online operation. In Proceedings of the 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2016, Daejeon, Korea, 9–14 October 2016; pp. 163–169. [CrossRef] [^61]: Rusu, R.B.; Cousins, S. 3D is here: Point Cloud Library (PCL). In Proceedings of the IEEE International Conference on Robotics and Automation, ICRA 2011, Shanghai, China, 9–13 May 2011. [CrossRef] [^62]: de Berg, M.; Cheong, O.; van Kreveld, M.J.; Overmars, M.H. Computational Geometry: Algorithms and Applications, 3rd ed.; Springer: Berlin/Heidelberg, Germany, 2008. [^63]: Milford, M.; Wyeth, G.F. SeqSLAM: Visual route-based navigation for sunny summer days and stormy winter nights. In Proceedings of the IEEE International Conference on Robotics and Automation, ICRA 2012, St. Paul, MI, USA, 14–18 May 2012; pp. 1643–1649. [CrossRef] [^64]: Rusu, R.B.; Blodow, N.; Beetz, M. Fast Point Feature Histograms (FPFH) for 3D registration. In Proceedings of the 2009 IEEE International Conference on Robotics and Automation, ICRA 2009, Kobe, Japan, 12–17 May 2009; pp. 3212–3217. [CrossRef] [^65]: Geiger, A.; Lenz, P.; Stiller, C.; Urtasun, R. Vision meets robotics: The KITTI dataset. Int. J. Robot. Res. 2013, 32, 1231–1237. [CrossRef] [^66]: Caesar, H.; Bankiti, V.; Lang, A.H.; Vora, S.; Liong, V.E.; Xu, Q.; Krishnan, A.; Pan, Y.; Baldan, G.; Beijbom, O. nuScenes: A Multimodal Dataset for Autonomous Driving. In Proceedings of the 2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition, CVPR 2020, Seattle, WA, USA, 13–19 June 2020; pp. 11618–11628. [CrossRef] [^67]: Quigley, M.; Gerkey, B.P.; Conley, K.; Faust, J.; Ng, A.Y. ROS: An open-source Robot Operating System. In Proceedings of the ICRA Workshop on Open Source Software, Kobe, Japan, 12–17 May 2009. [^68]: Yin, L.; Luo, B.; Wang, W.; Yu, H.; Wang, C.; Li, C. CoMask: Corresponding Mask-Based End-to-End Extrinsic Calibration of the Camera and LiDAR. Remote Sens. 2020, 12, 1925. [CrossRef] [^69]: Wang, C.; Luo, B.; Zhang, Y.; Zhao, Q.; Yin, L.; Wang, W.; Su, X.; Wang, Y.; Li, C. DymSLAM: 4D Dynamic Scene Reconstruction Based on Geometrical Motion Segmentation. IEEE Robot. Autom. Lett. 2021, 6, 550–557. [CrossRef] [^70]: Wang, C.; Li, C.; Liu, J.; Luo, B.; Su, X.; Wang, Y.; Gao, Y. U2-ONet: A Two-Level Nested Octave U-Structure Network with a Multi-Scale Attention Mechanism for Moving Object Segmentation. Remote Sens. 2021, 13, 60. [CrossRef] [^71]: Chen, X.; Li, S.; Mersch, B.; Wiesmann, L.; Gall, J.; Behley, J.; Stachniss, C. Moving Object Segmentation in 3D LiDAR Data: A Learning-Based Approach Exploiting Sequential Data. IEEE Robot. Autom. Lett. 2021, 6, 6529–6536. [CrossRef]