# SDV-LOAM：半直接视觉-激光雷达里程计与建图 袁子康 $^{ID}$、王庆杰 $^{ID}$、程肯 $^{ID}$、郝天宇 $^{ID}$、杨鑫 $^{ID}$，IEEE会员 摘要—视觉-激光雷达里程计与建图（V-LOAM）融合了相机和激光雷达的互补信息，是实现高精度、鲁棒位姿估计与建图的极具吸引力的解决方案。然而，现有系统可能会出现显著的跟踪误差，这些误差源于：1）3D激光雷达点与稀疏2D特征之间的关联（即3D-2D深度关联）；2）在六自由度（DOF）扫描到地图优化过程中垂直方向的明显漂移。本文提出的SDV-LOAM结合了半直接视觉里程计与自适应扫描到地图激光雷达里程计，有效避免了上述误差，进而实现高跟踪精度。SDV-LOAM的视觉模块直接提取3D激光雷达点投影其上且梯度较高的像素用于跟踪。为解决视觉里程计中匹配帧之间尺度差异过大的问题，我们设计了新颖的点匹配与传播方法，将宿主帧中的点传播到与当前帧更接近的中间关键帧，从而减小尺度差异。为降低垂直方向的位姿估计漂移，我们的激光雷达模块采用了自适应扫描到地图优化方法，该方法会根据垂直方向几何约束的丰富程度，自动选择优化3个水平自由度或完整的6自由度位姿。此外，我们提出了一种新颖的扫描重建方法，可将激光雷达点云的输入频率提升至与相机图像相同的水平，理论上可输出高频的激光雷达里程计结果。实验结果表明，我们的SDV-LOAM在KITTI里程计基准测试中排名第八，性能优于大多数激光雷达/视觉-激光雷达里程计系统。此外，我们的视觉模块性能优于最先进的视觉里程计，而自适应扫描到地图优化方法也能提升多个现有开源激光雷达里程计系统的性能。我们还在定制搭建的硬件平台上于大规模环境中对SDV-LOAM进行了验证，结果显示其兼具高精度和高输出频率。我们已发布SDV-LOAM的源代码，以供社区开发使用。 索引词—建图、位姿估计、视觉-激光雷达传感器系统。 手稿于2022年2月17日收到；2023年3月14日修订；2023年3月26日录用。2023年3月29日发表；2023年8月4日发布当前版本。本研究部分得到了国家自然科学基金项目62122029、和U20B2064的支持，也得到了武汉市科技局项目2020010601012167的支持。经H. Ling推荐录用。（通讯作者：杨鑫。） 袁子康任职于华中科技大学人工智能研究院，武汉430074，中国（电子邮箱：）。 王庆杰、程肯、郝天宇、杨鑫任职于华中科技大学电子信息与通信学院，武汉430074，中国（电子邮箱：；；；）。 ## 一、引言 数字对象标识符：10.1109/TPAMI.2023.3262817 视觉里程计和激光雷达里程计是两种广泛应用于6自由度位姿估计与建图的技术，也是无人驾驶汽车、自动导航等众多机器人及计算机视觉应用中的基础技术。仅使用RGB相机或激光雷达作为里程计的输入传感器时，二者各有优劣。视觉里程计[^1]、[^2]、[^3]、[^4]、[^5]、[^6]、[^7]能够以高频率（即30-60 Hz）输出位姿和地图点，但由于其对模糊图像和低纹理环境的鲁棒性较差，其精度不如激光雷达里程计。相比之下，激光雷达里程计[^8]、[^9]、[^10]、[^11]通常能提供比视觉里程计更精确的位姿估计与建图结果，不过其输出频率受限于输入点云的低频特性（例如10 Hz）。 现有文献中已有大量研究致力于结合视觉与激光雷达传感器的互补优势。目前最先进的视觉-激光雷达里程计方法可分为两类：松散集成型与紧密集成型。松散集成型方法（如DEMO [^12]、LIMO [^13]、DVL-SLAM [^14]、[^15]）仅利用激光雷达提供的3D点来为视觉里程计提供深度测量数据，完全忽略了激光雷达里程计在精确位姿估计方面的优势。因此，这类系统仅能被视为激光雷达辅助的深度增强型视觉里程计。相比之下，紧密集成型系统[^16]、[^17]对两种技术进行了更深入的融合，以更好地发挥它们的互补优势。其中最具代表性的紧密集成型方法是V-LOAM [^16]：该方法中，激光雷达辅助的深度增强型视觉里程计以高频率运行，而激光雷达里程计则以低频率对视觉模块输出的位姿进行优化。然而，在V-LOAM中，许多相机位姿并未经过激光雷达模块的优化。此外，V-LOAM的视觉模块采用基于特征的方法，这种方法不可避免地会存在3D-2D深度关联误差——由于能够关联到3D深度点的2D稀疏特征点较少，进而会产生插值误差。若采用视觉里程计的直接法，有望避免这种3D-2D深度关联误差，但这类方法依赖光度误差最小化来实现位姿估计，与基于特征的方法相比，它们更容易陷入局部最优，且对光度变化以及相机内参误差更为敏感[^18]。 在本文中，我们提出了SDV-LOAM，它结合了半直接式激光雷达辅助的深度增强型视觉里程计 --- 以及激光雷达里程计，以实现精确且鲁棒的位姿估计与建图。与V-LOAM不同，我们的视觉模块采用了半直接法，融合了基于特征的方法（通过重投影误差最小化实现精确位姿估计）与直接法的优势（消除3D-2D深度关联误差）。我们的半直接视觉里程计在SVO [^4]的核心思想基础上得到了显著改进，以应对视觉-激光雷达融合中的独特挑战： 1) 为确保有足够数量的2D像素能与激光雷达提供的3D深度点相关联，我们提出了一种新的点提取方法：该方法可提取图像上均匀分布、且源自3D激光雷达点在图像上投影的高梯度点。 2) 由于三维点云的频率较低，包含投影三维激光雷达点的帧之间的间隔较大，这可能导致匹配帧间的尺度差异显著。为了检测帧间可靠的像素对应关系，我们设计了一种新颖的点匹配方法：将宿主帧中的点传播到距离当前帧更近的中间关键帧，然后通过查找中间关键帧在当前帧上的对应像素，来获取当前帧与宿主帧之间的匹配点，从而减小尺度差异。 3) 为了提升视觉模块在不同场景下的鲁棒性，当当前跟踪点的纹理信息有限时，我们会自适应地添加不含激光雷达点的额外高梯度点作为补充。 大多数现有的激光雷达与视觉-激光雷达里程计系统（如A-LOAM、LeGO-LOAM、Fast-LOAM、ISC-LOAM和MULLS）面临的另一个主要问题是：激光雷达点在垂直方向上提供的约束较弱，因为室外场景中法向量竖直指向的表面大多是地面。这些表面通常位于同一平面（即地面平面），通过传统的迭代最近点（ICP）算法对3个垂直自由度（横滚、俯仰、上下方向）的位姿优化时，会引入较弱的约束。尽管最新的激光雷达里程计CT-ICP [^19] 通过将最初在IMLS-SLAM [^20] 中提出的加权策略引入传统ICP算法，缓解了这一问题。但当垂直方向的几何约束过于有限时，其性能仍不尽人意。在这类情况下，优化3个垂直自由度反而会降低位姿估计的整体精度。为解决这一问题，我们设计了自适应扫描-地图优化方法：该方法会根据当前输入扫描数据在垂直方向上的几何约束丰富程度，自动选择优化完整的6自由度位姿或仅优化3个水平自由度。 此外，我们提出了一种扫描重建模块，该模块根据相机图像的时间戳对扫描数据进行重叠与重建，以提高激光雷达点云的输入频率，进而提升激光雷达里程计的输出频率。在公开数据集上的实验结果 [^21]、[^22]、[^23] 表明：1）我们的视觉模块的性能优于现有最先进的激光雷达辅助深度增强视觉里程计，包括DEMO [^12]、LIMO [^13]、Huang等人提出的方法 [^15] 以及DVL-SLAM [^14]；2）我们的自适应扫描-地图优化方法能够提升多个现有开源算法的性能。 ![Image](images/22e8a3e5e9f910c31a2a1716e528beffb58035c76f2d340109d4d9ac23f1149d.jpg) ![Image](images/903a632d6c069302b24780f84d935d517ff66c262bfafd9f0a02be8ee72b44cf.jpg) ![Image](images/7dad7773dcb446c7466f947db026c7a56f23f8b993f3acea72ef91172991a1f2.jpg)
(c)
![Image](images/fbe9288af949b6e86761a965aed2f616b0c5098fbf69095730ceaf3b256cdd9a.jpg)
(d)
Fig. 1. (a) Our camera-LiDAR sensor system for collecting gray images and 3D point cloud measurements. (b) Exemplar input data collected by our camera-LiDAR sensor system. (c) The estimated trajectory overlaid on Google Map for visual illustration. (d) The 3D reconstruction result.
包括A-LOAM、LeGO-LOAM [^9]、Fast-LOAM、ISC-LOAM [^11]、MULLS [^24]和CT-ICP [^19]在内的激光雷达里程计系统；我们的SDV-LOAM在KITTI [^21]训练集、KITTI-360 [^22]以及KITTI-CARLA [^23]上的相对平移误差分别仅为0.47%、0.46%和0.038%，其性能优于A-LOAM、LeGO-LOAM、Fast-LOAM、ISC-LOAM、MULLS和CT-ICP；在KITTI测试集上的误差为0.60%，这一成绩在2023年1月之前的KITTI里程计基准测试中排名第8位，在现有的仅激光雷达/单目视觉-激光雷达系统中排名第6位，在所有开源系统中则排名第2位。我们进一步搭建了视觉-激光雷达里程计系统，该系统采用广角摄像头FL3-U3-13E4M-C（60 Hz）和Velodyne VLP-16E激光雷达（10 Hz），如图1所示。在大规模户外环境中的实时实验表明，配备扫描重建功能的该系统能够提高输入的激光雷达点云频率，并能以高精度输出来自激光雷达模块的位姿。 综上所述，本研究的主要贡献有五个方面： 1) 我们提出了一种新型的视觉-激光雷达里程计与建图系统，名为SDV-LOAM，与现有最先进的方法相比，它在理论上能够实现具有竞争力的精度和较高的输出频率； 2) 我们为SDV-LOAM的视觉模块提出了一种半直接法，该方法能够应对视觉-激光雷达融合中的独特挑战（即3D-2D深度关联误差、长间隔引发的像素匹配误差以及低纹理场景下的鲁棒性不足问题），其性能优于现有的最先进的激光雷达辅助深度增强视觉里程计 [^12]、[^13]、[^14]、[^15]； 3) 我们提出了一种自适应扫描-地图优化方法，该方法能够根据场景中的几何约束自动优化3个水平自由度位姿 --- 或6个完整自由度位姿。我们的自适应方法能够提升多个现有开源激光雷达里程计系统的精度； 4) 我们提出了一种扫描重建模块，该模块能够提高激光雷达点云的输入频率，从而在理论上加快激光雷达里程计的输出位姿频率； 5) 我们已发布本研究的源代码，以供社区开发使用。 $^{1}$ 本文的其余部分安排如下：第二节简要介绍相关文献；第三节阐述预备知识；第四节介绍我们的系统框架；第五节和第六节分别详细介绍视觉模块和激光雷达模块，第七节给出实验评估结果；第八节对全文进行总结。 ### 二、相关工作 RGB摄像头和激光雷达是两种广泛用于6自由度位姿估计的传感器。一般来说，基于视觉的里程计/SLAM系统可分为三类：1）基于特征的方法[^1]、[^25]、[^26]、[^27]、[^28]，这类方法依赖局部图像特征来实现位姿跟踪；2）直接法[^2]、[^3]、[^6]、[^7]、[^29]，这类方法直接使用原始传感器数据（如像素强度值）进行跟踪；3）半直接法[^4]，它先像直接法那样通过最小化光度误差来估算初始位姿，接着利用该初始位姿通过点匹配在帧间找到对应关系，最后像基于特征的方法那样通过最小化匹配特征的重投影误差来优化位姿。视觉里程计（VO）的一个明显优势是其位姿估计的输出频率很高。不过，当图像数据中存在较大噪声时，其精度会显著下降，且生成的地图通常较为稀疏。 基于 LiDAR 的里程计系统 [^8]、[^9]、[^10]、[^30] 依赖 LiDAR 点云中所包含的几何信息进行跟踪,并持续利用丰富的几何结构更新点云地图。例如,LOAM [^8]、[^30] 从原始输入点云中提取边缘点和表面点,然后构建点到线和点到面的残差,用于 6-DOF ICP 位姿估计。LOAM 并行执行跟踪与建图,以保证位姿估计的高效性。然而,由于需要处理的点数量庞大,其输出频率仍然较低。LeGO-LOAM [^9] 在 LOAM 的基础上,将大量几何信息较弱的点排除在处理流程之外,从而显著提升了效率。然而,如何有效排除无关点(例如由运动目标或错误测量引起的离群点)是一项非常困难的任务,而错误地剔除有用点反而会降低系统的鲁棒性。SuMa [^10] 提出使用基于面元(surfel)的表示方法来聚合 LiDAR 点的信息以构建地图。然而,SuMa 需要 GPU 加速才能实现实时性能,且其位姿估计精度并不优于基于 LOAM 框架的系统。Wang 等人 [^11] 提出了 Fast-LOAM,该方法去除了扫描到地图的优化模块,仅保留扫描到扫描的位姿估计。然后,他们提出了一种基于强度扫描上下文(ISC)的方法,以改善 Fast-LOAM 中回环检测的性能。不同于传统的 LiDAR 里程计系统仅提取边缘和表面特征,MULLS [^24] 提出在 LiDAR 里程计中利用更多类型的特征(例如立面、梁、柱和地面)。然而,由不同特征构建的残差项的权重难以合理设置,并且在 KITTI [^21] 的某些序列上同时使用所有这些特征可能会降低精度。大多数 LiDAR 系统 [^8]、[^9]、[^11]、[^24]、[^30] 普遍存在的一个主要缺陷是:这些方法执行 6-DOF ICP 位姿估计,而在室外场景中垂直方向上的约束通常较差,因此在垂直方向上往往会产生较大的累积误差。为缓解此问题,IMLS-LOAM [^20] 提出了一种 IMLS 位姿求解算法,以取代传统的传输 ICP。然而,IMLS 的巨大计算开销使 IMLS-LOAM 无法实时运行。在 IMLS-SLAM 的基础上,CT-ICP [^19] 采用了一种轨迹的弹性表示形式,扫描内位姿保持连续,扫描间位姿允许不连续,以提升快速运动下的鲁棒性。尽管 CT-ICP 相比其他 LiDAR 系统具有更高的精度和效率,但在垂直方向几何信息受限的场景(例如 KITTI 01 和 KITTI 03)中,其性能仍然不佳。LOL [^31] 应用了一种位置识别方法,通过 Segment Matching 算法检测在线三维点云与先验离线地图之间几何相似的位置。然而,先验离线地图在实际应用中可能并不可用,且 Segment Matching 需要强大的 GPU 进行计算。LIO-SAM [^32] 将 LiDAR-惯性里程计建模为因子图,以实现多传感器融合与全局优化。然而,LIO-SAM [^32] 的计算负担已接近其极限,因此其他传感器(例如相机、轮式里程计)难以融合到现有系统中。此外,他们未在任何公开数据集上对其工作进行测试,这使得与文献中的方法进行对比变得困难。 为了结合视觉系统与激光雷达系统的互补优势，一些研究通过整合这两种传感器来探索视觉-激光雷达混合里程计/SLAM技术。现有方法大致可分为两类：松散集成和紧密集成。松散集成方法[^12]、[^13]、[^14]、[^15]利用激光雷达点来辅助深度估计与建图，随后依靠基于优化后地图的视觉跟踪来进行位姿估计。例如，DEMO[^12]将提取的二维特征与三维点相关联，从而为每个特征点赋予深度值（即深度关联）。为提高精度，LIMO[^13]在DEMO的基础上增加了闭环模块，并利用语义信息来识别运动物体。除了点特征外，Huang等人[^15]提出使用线特征，这类特征对噪声[^33]、大视角变化[^34]以及运动模糊[^35]不太敏感，适用于跟踪与建图过程。然而，激光雷达获取的三维点并非与二维特征点或线一一对应，因此深度关联中不可避免地需要插值，进而引入误差。为避免深度关联误差，DVL-SLAM[^14]提出采用直接法进行视觉跟踪。在DVL-SLAM中，激光雷达点被投影到图像上以提取对应像素的灰度值。 --- ![Image](images/a977925d2a92f0fa0abcb866f4d28cf6c882b59b2d410f3a6ebd51ca36cac5ec.jpg)
Fig. 2. Illustration of the coordinate transformation of three coordinate systems. $(\cdot)^{w}$, $(\cdot)^{c}$ and $(\cdot)^{l}$ are defined as a 3D point in the world coordinates, in the camera coordinates and in the LiDAR coordinates respectively. The world coordinate coincides with $(\cdot)^{l}$ at the starting position. In all coordinates the x-axis points to the right, the y-axis points upward, and the z-axis points forward. We assume that the transformation $\mathbf{T}_{c}^{l}$ between camera and LiDAR is known and remains constant during operation.
不过，DVL-SLAM的位姿估计依赖于光度误差最小化，其效果通常不如重投影误差最小化。 紧密集成方法[^16]、[^17]旨在深度结合视觉与激光雷达里程计的优势。其中最著名的例子之一是V-LOAM[^16]：它首先利用激光雷达辅助的深度增强型视觉里程计以高频率（即图像帧率）进行粗略的帧间位姿估计，随后再通过低频运行的激光雷达里程计（即激光雷达点云采集率）来优化位姿。之后，高频相机位姿会被整合到低频激光雷达位姿中，最终以图像帧率输出位姿结果。由于视觉模块产生的许多位姿并未经过激光雷达模块的优化，最终的位姿结果实际上包含了两个系统的输出（即激光雷达里程计与激光雷达辅助的深度增强型里程计）。值得注意的是，这一问题并非由激光雷达里程计的计算能力有限导致，而是因为激光雷达点云的采集频率（10 Hz）与RGB相机图像的采集频率（30~60 Hz）存在差异。 ### 三、预备知识 ### A. 坐标系 我们用 $(\cdot)^{w}, (\cdot)^{c}$ 和 $(\cdot)^{l}$ 分别表示世界坐标系、相机坐标系和激光雷达坐标系中的三维点。我们定义世界坐标系 $(\cdot)^{w}$ 初始时与 $(\cdot)^{l}$ 重合。在所有坐标系中，x 轴指向右方，y 轴指向上方，z 轴指向前方。我们将在时刻 $i_{th}$ 拍摄 $t_{i}$ 图像帧的相机坐标系记为 $c_{i}$，将时刻 $t_{i}$ 对应的激光雷达坐标系记为 $l_{i}$，则从相机坐标系 $c_{i}$ 到激光雷达坐标系 $l_{i}$ 的变换矩阵（即外参）记为特殊欧氏群中的 $\mathbf{T}_{c_{i}}^{l_{i}} \in SE(3)$，其中 $\mathbf{T}_{c_{i}}^{l_{i}}$ 由旋转矩阵 $\mathbf{R}_{c_{i}}^{l_{i}} \in SO(3)$ 和平移向量 $\mathbf{t}_{c_{i}}^{l_{i}} \in \mathbb{R}^{3}$ 组成，如下 $$ \mathbf{T}_{c_{i}}^{l_{i}}=\begin{bmatrix}\mathbf{R}_{c_{i}}^{l_{i}}&\mathbf{t}_{c_{i}}^{l_{i}}\\ \mathbf{0}&1\end{bmatrix} $$ 外部参数通常只需离线校准一次，在在线姿态估计过程中保持不变；因此，为简便起见，我们可以用 $\mathbf{T}_{c_i}^{l_i}$ 来表示 $\mathbf{T}_c^l$。 ### B. 相机投影模型 世界坐标系中一个三维点的坐标记为 $\mathbf{p}^w = (x, y, z)^T \in \mathbb{R}^3$，它投影到二维图像上的坐标记为 $\mathbf{u} = (u, v)^T \in \mathbb{R}^2$，该结果可基于相机投影模型 $\pi: \mathbb{R}^3 \to \mathbb{R}^2$ 计算得出。 $$ \mathbf{u}=\pi\left(\mathbf{T}_{l}^{c}\mathbf{T}_{w}^{l}\mathbf{p}^{w}\right) $$ 其中 $\pi$ 由 $3 \times 3$ 相机内参 K 确定。类似地，相机坐标系中的三维点 $\mathbf{p}^c$ 可以通过逆投影模型 $\mathbf{u}$ 从其二维投影 $\pi^{-1}$ 中恢复：$\pi: \mathbb{R}^2 \to \mathbb{R}^3$ $$ \mathbf{p}^{c}=\pi^{-1}\left(\mathbf{u},d_{\mathbf{u}}\right) $$ 其中 $d_{\mathbf{u}} \in \mathbb{R}$ 表示二维点 $\mathbf{u}$ 在相机坐标系中的深度。 ### C. 数据管理 我们将激光雷达一次完成的完整扫描定义为一次扫描周期。在搭建传感器系统时，我们选用 Velodyne VLP-16 作为激光雷达传感器，它通过激光二极管旋转生成完整的 $360^\circ$ 点云（10 Hz），选用 FL3-FW-14S3M-C 作为相机传感器（60 Hz）。由于这两种传感器的运行频率不同，我们需要对齐它们的时间戳。假设相机在时间 $t_i$ 采集图像，激光雷达在 $t_i-0.1$ 秒开始扫描，并在时间 $t_i$ 完成扫描。一次完整扫描所收集的点云在后续文中被定义为 S。对于公开数据集 [^21]、[^22]、[^23] 的评估，相机和激光雷达的采集频率均为 10 Hz，数据集提供方已经对齐了两种传感器之间的时间戳和变换关系，且 [^21] 已对所有序列进行了运动畸变补偿。 在我们系统的视觉模块中，我们将一帧定义为包含一幅图像及其对应的激光雷达点的集合。由于相机的采集频率（如 60 Hz）可能高于激光雷达的采集频率（如 10 Hz），只有部分图像有对应的激光雷达点。对于没有对应激光雷达点的图像，该帧仅包含图像和空的激光雷达点集。只有包含非空激光雷达点的帧才能被选为关键帧（KFs）。 对于图像中能够直接投影出对应激光雷达点的二维像素，我们在后续文中将其定义为深度点 D。对于没有其他三维点对应的二维像素，我们将其定义为非深度点。对于像素坐标为 $\mathbf{u} = (u, v)^T \in \mathbb{R}^2$、对应的激光雷达点为 $\mathbf{p}^l = (x, y, z)^T \in \mathbb{R}^3$ 的深度点，$d_\mathbf{u}$ 的深度 $\mathbf{u}$ 可按如下方式计算： $$ d_{\mathbf{u}}=\left[\mathbf{T}_{l}^{c}\mathbf{p}^{l}\right]_{z} $$ --- ![Image](images/4a2c0e8cb09ced94927b29ffb6c286789d1affa98eb248cae3defcc67d45cca9.jpg)
Fig. 3. Overview of our SDV-LOAM which consists of two main modules: a semi-direct depth-enhanced visual odometry and a LiDAR odometry. The visual module and LiDAR module are combined by a sweep reconstruction block, which increases the input frequency of LiDAR point clouds to the same frequency as the camera images. The yellow blocks highlight our main contributions compared with existing methods. It is worth explaining that the "60 Hz visual pose" and the "60 Hz reconstructed cloud" are the ideal frequency assuming infinite computing resources. We use 60 Hz visual pose and reconstructed cloud here for the convenience of description. In fact, both our vision and LiDAR module can run at around 20 Hz.
其中 $[\cdot]_z$ 是该元素的 z 坐标。为简便起见，在公式 (4) 中我们没有考虑激光雷达点的运动畸变，这种畸变可通过恒定速度模型 [^8] 进行补偿。 ### IV. 系统概述 图 3 展示了我们系统的框架，该系统由两个主要模块组成：半直接式深度增强视觉里程计和激光雷达里程计。黄色块标出了与现有方法相比我们的主要创新点。 我们的视觉模块是基于DSO [^6]实现的，这是一种流行的单目直接法视觉里程计；我们对其进行了大量修改，以将其扩展为带有激光雷达辅助的半直接深度增强型视觉里程计。局部深度图是一种局部地图，用于存储滑动窗口内每个提取点的深度观测值，而宿主帧则是提取高梯度点时所依据的帧。这种半直接深度增强型视觉里程计以60 Hz的频率运行，用于估计每个相机帧的位姿$c_i$。首先，在直接位姿估计模块中，通过最小化$c_i$与最新关键帧$c_i$之间的光度误差来估算$k_n$的初始位姿。接着，利用初始位姿和局部深度图，通过点匹配与传播来寻找帧间的像素对应关系。最后，我们利用这些像素对应关系，在位姿优化模块中通过最小化$c_i$与局部深度图之间的重投影误差来进一步优化位姿。如果$c_i$未被选为新的关键帧，它就会被送往变换整合模块；否则，$c_i$会被送往新点提取模块。对于每一个新关键帧$k_n$，都会提取高梯度深度点和非深度点。随后，在基于滑动窗口的束调整（BA）模块中，我们会找到新提取的点与滑动窗口内先前关键帧上的像素之间的对应关系。根据滑动窗口内所有现有的对应关系计算重投影残差，并进行联合优化。每次优化之后，最旧的关键帧会被边缘化，然后从滑动窗口中移除，从该关键帧中提取的点也会从局部深度图中删除。最终，我们在变换整合模块中将来自位姿优化模块的帧间位姿与来自基于滑动窗口的BA模块的帧到世界位姿相结合，从而以与输入相机图像相同的频率得出最终输出位姿。点云预处理模块旨在当当前跟踪点的纹理信息有限时，自适应地添加没有激光雷达深度测量的额外高梯度点作为补充。 我们的激光雷达模块是基于CT-ICP [^19]实现的，我们对其做了两项主要改进以提高精度和输出频率。首先，我们提出了一种扫描重建方法，该方法能够将扫描数据重叠并重建，以匹配相机图像的采集频率（例如60 Hz）。其次，我们提出了一种自适应扫描到地图位姿优化方法，该方法能够根据垂直方向上几何信息的丰富程度，自适应地决定优化3个水平自由度还是6个全自由度的位姿。 ### V. 半直接深度增强型视觉里程计 现有方法[^12]、[^13]、[^15]主要采用基于特征的方法来进行激光雷达辅助的视觉跟踪。然而，3D激光雷达点的投影未必能与2D特征点精确重合。因此，在将2D特征与3D激光雷达点关联的过程中，通常需要进行3D点的插值，这会产生不小的近似误差。另一方面，直接法[^14]能够有效避免这一问题，因为它可以直接利用3D激光雷达点所投影到的2D像素来进行视觉跟踪。不过，直接法中采用的基于光度误差最小化的位姿求解方法容易陷入局部最小值，且对光度变化以及相机内参误差较为敏感[^18]。 在这项工作中，我们提出采用半直接法进行跟踪，该方法结合了基于特征的方法（通过重投影误差最小化来实现精确的位姿估计）与直接方法的优势（消除了在将激光雷达信息集成到视觉里程计过程中，因稀疏特征提取模块导致的2D-3D数据关联误差）。我们还设计了一系列改进方案（即新的点提取、带传播机制的点匹配以及自适应处理），以解决半直接式激光雷达辅助的深度增强视觉里程计中存在的技术难题。 --- 我们还会额外添加非深度点，以此来解决半直接式激光雷达辅助的深度增强视觉里程计中遗留的技术障碍。 我们的视觉模块仅存储局部深度图，该图包含了所有宿主帧位于滑动窗口内的提取出的深度点和非深度点。局部地图中的任意点p均被表示为$(\mathbf{u}, d_{\mathbf{u}})$，其中$\mathbf{u}$是点p在其宿主帧上的2D像素坐标，$d_{\mathbf{u}}$则是像素$\mathbf{u}$对应的深度值。一旦某帧从滑动窗口中移除，从中提取的点也会从局部地图中被删除。由于视觉模块的目标是输出高频率的位姿估计结果，因此它不会存储全局地图。 ### A. 点云预处理 根据公式(4)由激光雷达点计算得到的深度值精度很高，且在系统运行过程中保持不变。如果大部分激光雷达点能够投影到纹理丰富的区域，那么从新点提取模块中得到的具有高梯度的深度点就足以满足位姿跟踪的需求。然而，场景中有些区域往往没有纹理或纹理高度相似（例如地面和树叶）。如果这些无纹理区域中存在大量深度点，仅依靠这些深度点来估计位姿可能会不够准确。在这种情况下，我们就需要纹理区域中具有高梯度的非深度点作为补充来帮助跟踪。尽管非深度点的深度值是通过深度滤波[^6]得到的，其精度不如深度点，但它们仍包含有效的视觉信息，因此有助于提高跟踪精度。我们将仅使用深度点的情况定义为模式1，同时使用深度点和非深度点的情况定义为模式2。 点云预处理的功能是对输入的点云进行分析，并自适应地确定所用模式。对于每一帧非空激光雷达点云，都会执行点云预处理模块。一旦模式确定，就会一直保持不变，直到再次运行点云预处理模块为止。 点云预处理模块的工作流程如下：我们首先采用地面分割方法[^36]，基于原始输入点粗略检测出候选地面点，而地面通常属于无纹理区域。接着，我们利用这些候选地面点通过RANSAC算法拟合出精确的地面模型，进而得到当前帧的精细化地面点。我们计算当前扫描数据中地面点的占比。如果该占比高于阈值（例如在我们的系统中为0.8），视觉模块就采用模式2；否则，我们使用快速分割方法[^37]来进一步区分具有不规则结构信息的点。户外场景中不规则结构的投影通常位于树叶或草丛上，其像素强度与周围像素非常相似，难以用于视觉位姿估计。如果非地面点中不规则点的占比高于阈值（例如在我们的系统中为0.5），视觉模块就采用模式2；否则，就采用模式1。 ### B. 直接姿态估计 我们通过将 $c_i$ 中提取的高梯度点（即 $k_n$）投影到 $k_n$，相对于最新关键帧 $P_{k_n}$ 跟踪当前帧 $c_i$，然后计算 $k_n$ 和 $c_i$ 之间的光度误差，如下 $$ E_{k_{n}c_{i}}=\sum_{\mathbf{u}\in P_{k_{n}}}\sum_{\widetilde{\mathbf{u}}\in N_{\mathbf{u}}}\omega_{\widetilde{\mathbf{u}}}\left\|I_{c_{i}}\left[\widetilde{\mathbf{u}}^{\prime}\right]-b_{c_{i}}-\frac{e^{a_{c_{i}}}}{e^{a_{k_{n}}}}\left(I_{k_{n}}[\widetilde{\mathbf{u}}]-b_{k_{n}}\right)\right\|_{\gamma}^{2} $$ 其中$\|\cdot\|_{\gamma}$为Huber范数，$I(\cdot)$代表像素强度，$\omega_{u}$是一个权重，用于降低高图像梯度像素的权重，公式如下： $$ \omega_{\mathbf{u}}=\frac{con^{2}}{con^{2}+\left\|\nabla I_{c_{i}}(\mathbf{u})\right\|_{2}^{2}} $$ con是一个常数（例如在我们的系统中取值为50），u是$P_{k_n}$中提取的高梯度深度点的二维像素坐标。即便根据点云预处理模块的结果采用了模式2，$k_n$中那些非深度点的深度也不会收敛。因此，仅使用$P_{k_n}$中的高梯度深度点进行追踪。$u'$是u在$c_i$中的投影，计算公式如下： $$ \mathbf{u}^{\prime}=\pi\left(\mathbf{T}_{k_{n}}^{c_{i}}\pi^{-1}\left(\mathbf{u},d_{\mathbf{u}}\right)\right) $$ 其中$d_u$是$\mathbf{u}$的深度，该值根据式(4)计算得出。在式(5)中，$a_c$和$b_c$是影响优化过程中光照传递函数的硬件相关参数。$N_u$表示$\mathbf{u}$的8点模式，$\widetilde{\mathbf{u}}'$是将模式点$\widetilde{\mathbf{u}}$投影到$I_{c_i}$上的结果。最终，通过最小化$\mathbf{T}_{k_n}^{c_i}$来估算$a_{c_i}$、$b_{c_i}$和$E_{k_n c_i}$的值。 ### C. 带传播的点匹配 一旦估算出当前帧$\mathbf{T}_{k_n}^{c_i}$的初始姿态$c_i$，我们便将其转换为相对于$(\cdot)^{c_0}$的姿态$\mathbf{T}_{c_i}^{c_0} = \mathbf{T}_{k_n}^{c_0} \mathbf{T}_{k_n}^{c_i-1}$，随后利用$\mathbf{T}_{c_i}^{c_0}$来寻找局部深度图与$c_i$之间的对应关系，其中$c_0$是相机坐标系下的第一帧。 SVO 的作者 [^4] 最小化图块之间的光度误差（图 4 中以黑色方块表示），以在当前帧 $c_i$ 与最近的关键帧 $k_n$ 之间寻找二维像素对应关系。具体而言，对于滑动窗口中宿主帧为 $\mathbf{p}_1 = (\mathbf{u}_1, d_{\mathbf{u}_1}) \in P_{k_j}$ 的任意点 $k_j (j \in [1, n-1])$，SVO 首先取 $\mathbf{u}_1$ 在 $k_n$（即 $\mathbf{u}_1^{k_n}$）上的对应关系，该帧是 $c_i$ 最近的关键帧。然后将 $(\mathbf{u}_1^{k_n}, d_{\mathbf{u}_1^{k_n}})$ 投影到 $c_i$，通过 $\mathbf{u}_1^{c_i}$ 推导得到 $\mathbf{T}_{c_0}^{c_i}$，以寻找 $\mathbf{p}_1$ 在 $c_i$ 上的二维像素对应关系的初始位置，随后通过在 $\mathbf{u}_1^{c_i}$ 和 $\mathbf{u}_1^{c_i}$ 的邻域区域内最小化 (9) 来优化 $\mathbf{u}_1^{k_n}$ 的位置（即点匹配） $$ \mathbf{u}_{1}^{c_{i}}=\pi\left(\mathbf{T}_{c_{0}}^{c_{i}}\left(\mathbf{T}_{k_{n}}^{c_{0}}\boldsymbol{\pi}^{-1}\left(\mathbf{u}_{1}^{k_{n}},d_{\mathbf{u}_{1}^{k_{n}}}\right)\right)\right) $$ $$ \mathbf{u}_{1}^{c_{i}}=\underset{\mathbf{u}_{1}^{c_{i}}}{\mathrm{a r g m i n}}\sum_{\substack{\mathbf{\overline{{u}}}_{1}\in N_{\mathbf{u}_{1}^{c_{i}}},\widetilde{\mathbf{u}}\in N_{\mathbf{u}_{1}^{k_{n}}}}}\|I_{c_{i}}[\overline{\mathbf{u}}]-\mathbf{A}_{1}I_{k_{n}}[\widetilde{\mathbf{u}}]\|^{2} $$ 其中$A_{1}$是仿射变形矩阵，其详细信息见[^4]。 --- ![Image](images/ecbb2e158633911fe8a4f15139efbfa7e6038171b9556149dcea661af44d5698.jpg)
Fig. 4. Illustration of point matching with propagation. We first finds correspondences between a host keyframe $k_j$ and the current frame $c_i$ by minimizing photometric error between 2D patches (black squares) in $c_i$ and $k_j$. For an arbitrary 3D point $\mathbf{p}_1$, we project it from its host frame $k_j$ to $c_i$ to derive $\mathbf{u}_1^{c_i}$, then we take $\mathbf{p}_1$'s previously matched correspondence $\mathbf{u}_1^{k_n}$ from $k_n$ that is closest to $c_i$, and align $\mathbf{u}_1^{k_n}$ and $\mathbf{u}_1^{c_i}$ which have similar scale to refine the position of $\mathbf{u}_1^{c_i}$.
与采用立体相机和RGB-D相机的半直接法不同，并非所有提取出的点都有来自激光雷达点的深度观测值。这种现象会导致当前帧中找到的对应关系无法用于下一帧中查找新的对应关系。如图4所示，$\mathbf{u}_1$是从$k_j$中提取出的一个点，其深度值$d_{\mathbf{u}_1}$是根据公式(4)，由投影到$\mathbf{u}_1$上的3D激光雷达点计算得出的。$\mathbf{u}_1^{k_n}$是通过在$k_n$上进行点匹配找到的对应关系。然而，在激光雷达辅助的视觉里程计中，很难保证恰好有一个激光雷达点投影到$\mathbf{u}_1^{k_n}$上。结果就是，我们可能无法为$d_{\mathbf{u}_1^{k_n}}$中的$\mathbf{u}_1^{k_n}$提供深度观测值$k_n$，进而即使$\mathbf{u}_1$与$c_i$相近，也无法利用SVO中的方法找到$k_n$在$c_i$上的对应关系。 解决上述问题的合理方案是在间隔较大的帧之间进行点匹配。例如，我们可以直接在当前帧$\mathbf{u}_1$上找到$c_i$的对应关系。不过这种点匹配方法存在一个关键局限：最终得到的像素对应关系的准确性在很大程度上取决于两幅图像帧$I_{c_i}$和$I_{k_j}$之间的尺度差异。随着两帧之间间隔的增大，尺度差异可能也会显著增大，进而导致较大的匹配误差。如图4所示，对于示例点$\mathbf{p}_1 = (\mathbf{u}_1, d_{\mathbf{u}_1})$而言，其所属帧$k_j$与当前帧$c_i$相距较远，此时$\mathbf{u}_1$的邻域可能与$\mathbf{u}_1^c$的邻域截然不同——由于$\mathbf{u}_1$的分辨率降低，$k_j$邻域中的一些区域可能会消失（即$I_{c_i}$中的橙色矩形）。因此，$\mathbf{u}_1^c$的匹配误差可能会很大。我们在第VII-A1节中也定量测试了帧间间隔对匹配误差的影响。结果显示，当间隔从1个关键帧增加到7个关键帧时，像素对应关系的误差会从2.86像素增大到10.40像素。 为解决上述问题，我们提出了带传播的点匹配方法，该方法会将具有深度观测值的点从激光雷达传播到中间关键帧，以此缩短间隔。具体来说，以示例点$\mathbf{p}_1 = (\mathbf{u}_1, d_{\mathbf{u}_1})$为例，其所属帧为$k_j$，我们首先利用公式(10)将其投影以获得初始位置$\mathbf{u}_1^{c_i}$。接着，我们选取在$\mathbf{p}_1$中最接近$\mathbf{u}_{1}^{k_n}$的、$k_n$之前匹配到的对应关系$c_i$，并在考虑光度参数影响的前提下，通过对齐$\mathbf{u}_1^{c_i}$和$\mathbf{u}_1^{k_n}$来优化$\mathbf{u}_1^{c_i}$的坐标。 $$ \begin{aligned}&\mathbf{u}_{1}^{c_{i}}=\pi\left(\mathbf{T}_{c_{0}}^{c_{i}}\left(\mathbf{T}_{k_{j}}^{c_{0}}\pi^{-1}\left(\mathbf{u}_{1},d_{\mathbf{u}_{1}}\right)\right)\right)\\&\mathbf{u}_{1}^{c_{i}}=\underset{\mathbf{u}_{1}^{c_{i}}}{\operatorname{argmin}}\sum_{\substack{\overline{\mathbf{u}}\in N_{\mathbf{u}_{1}^{c_{i}},\widetilde{\mathbf{u}}}\in N_{\mathbf{u}_{1}^{k_{n}}}}}\\&\left\|I_{c_{i}}[\widetilde{\mathbf{u}}]-b_{c_{i}}-\frac{e^{a_{c_{i}}}}{e^{a_{k_{n}}}}\left(\mathbf{A}_{1}I_{k_{n}}[\widetilde{\mathbf{u}}]-b_{k_{n}}\right)\right\|^{2}\\ \end{aligned} $$ 我们并未直接对齐 $\mathbf{u}_1$ 和 $\mathbf{u}_1^c$，而是在进行点匹配时利用 $\mathbf{u}_1^{k_n}$ 作为中间变量。这是因为 $\mathbf{u}_1^{k_n}$ 的主帧（$i.e., k_n$）与当前帧 $c_i$ 十分接近，二者的局部分辨率也较为相似。如图4所示，$\mathbf{u}_1^{k_n}$ 的邻域与 $\mathbf{u}_1^c$ 的相似性要高于与 $\mathbf{u}_1$ 的相似性。因此，通过将 $\mathbf{u}_1$ 与 $c_i$ 对齐，我们就能在 $\mathbf{u}_1^c$ 上获得更精确的 $\mathbf{u}_1^{k_n}$ 像素对应关系。自然地，一旦 $c_i$ 被选为新的关键帧 $k_{n+1}$，当下一张相机帧 $c_{i+1}$ 到来时，我们只需将 $\mathbf{p}_1$（即 $c_{i+1}$）与 $\mathbf{u}_1^{k_{n+1}}$ 对齐，就能找到 $\mathbf{u}_1^{c_i}$ 在 $\mathbf{u}_1^{c_{i+1}}$ 上的对应关系。 ### D. 姿态优化 在获取局部深度图与 $c_i$ 之间的对应关系后，我们构建重投影误差以进一步优化 $\mathbf{T}_{c_i}^{c_0}$。 $$ \mathbf{T}_{c_{i}}^{c_{0}}=\underset{\mathbf{T}_{c_{i}}^{c_{0}}}{\mathrm{a r g m i n}}\sum_{k_{m}\in K}\sum_{\mathbf{p}_{s}\in P_{k m}}\left\|\pi\left(\mathbf{T}_{c_{0}}^{c_{i}}\mathbf{T}_{k_{m}}^{c_{0}}\mathbf{p}_{s}\right)-\mathbf{u}_{s}^{c_{i}}\right\|_{\gamma}^{2} $$ 其中，$\mathbf{u}_s^{c_i}$ 是 $\mathbf{p}_s$ 在 $c_i$ 上的对应点，K 表示滑动窗口内所有关键帧的集合，$P_{k_m}$ 则是从 $k_m$ 中提取出的点集。如果 $c_i$ 被选为最新的关键帧，它会被送入新点提取模块；否则，$c_i$ 的姿态会通过 $\mathbf{T}_{c_i}^{k_n} = \mathbf{T}_{c_0}^{k_n} \mathbf{T}_{c_i}^{c_0}$ 转换为帧间姿态，随后被送往变换积分模块。 ### E. 新点提取 我们的新点提取模块由两部分组成：新深度点提取和新非深度点提取。 如果点云预处理模块选择了模式I，该模块仅提取新深度点。在SVO中，会为每个新选的关键帧提取2D特征；但在将2D特征与稀疏的3D激光雷达点关联以获取深度观测值时，不可避免地会出现近似插值现象。为避免深度关联误差，我们选择提取带有深度观测值的高梯度点，而非2D特征。首先，我们根据公式(4)将 $k_n$ 的点云投影到图像上，得到对应的深度点集，记为 $D_{k_n}$。接着，我们从 $P_{k_n}$ 中提取子集 $D_{k_n}$，该子集由分布均匀的高梯度图像点构成。伪代码V-E总结了我们的新深度点提取过程，主要分为两步：第一步是将图像等分为大小为 $32 \times 32$、$\{T_1, T_2 \cdots T_q\}$ 的块，然后计算相应的 --- 伪代码1：新深度点提取。 输入：最新关键帧 $k_n$ 的图像及其深度点集 $D_{k_n}$ 输出：提取出的点集 $P_{k_n}$ 步骤1：计算强度阈值 将 $k_n$ 划分为 $32 \times 32$ 个大小为 $\{T_1, T_2 \cdots T_q\}$ 的块 对于 $T_i = T_1 \cdots T_q$ 根据 $th(T_i)$ 的强度直方图计算梯度阈值 $T_i$ 步骤2：自适应点提取 将 $k_n$ 划分为 $n \times n$ 个大小为 $\{m_1, m_2 \cdots m_l\}$ 的块 对应子集 $D_{k_n} : \{S_1, S_2 \cdots S_l\}$ for $m_i = m_1 \cdots m_l$ 从 $p_h = (u_h, d_{u_h})$ 中提取梯度最大的点 $S_i$，并找到 $T_p$ 所在的位置 $u_h$ 如果 $(grad(p_h) > th(T_p))$ 成立 将 $p_h$ 加入 $P_{k_n}$，同时 num++。 如果 num 不满足要求 清空 $P_{k_n}$，令 num = 0。 调整 n 的值，然后再次执行 Procedure2。 否则 返回 $P_{k_n}$。 根据 $th(T_i)$ 的强度直方图，为每个块 $T_i$ 计算梯度阈值 $T_i$。在第二步中，我们将图像均匀划分为 $n \times n$ 个块 $\{m_1, m_2, \ldots, m_l\}$，其中 n 与每幅图像上预期提取的点数成反比。n 的初始值由 $n = \sqrt{\frac{w \times h}{num\_expect}}$ 计算得出，其中 w 和 h 分别是输入图像的宽度和高度，$num\_expect$ 是我们希望从每帧中提取的点数。对于每个块 $m_i$，我们提取位于其中且属于 $D_{k_n}$ 的所有点，以形成子集 $S_i$。接下来，我们计算 $S_i$ 中所有点的梯度，并将对应于最大梯度的点定义为 $\mathbf{p}_h = (\mathbf{u}_h, d_{\mathbf{u}_h})$。如果 $\mathbf{p}_h$ 的梯度高于 $th(T_i)$（$T_p$ 是 $32 \times 32$ 所位于的 $\mathbf{u}_h$ 块），我们将 $\mathbf{p}_h$ 添加到 $P_{k_n}$ 中，作为新提取的高梯度点。如果 $P_{k_n}$ 中的点数不满足我们的要求，我们适当调整 n 的值，并重复第二步。 如果点云预处理模块选择了模式2，意味着大多数3D激光雷达点都位于无纹理区域。这种情况下，我们会额外提取高梯度但非深度的点，并将它们加入 $P_{k_n}$。这些非深度点主导着滑动窗口优化的收敛过程；而 $P_{k_n}$ 中的深度点主要用于保持准确的度量尺度。 ### F. 基于滑动窗口的光束法平差 基于滑动窗口的光束法平差（BA）通过最小化重投影误差来进行多帧联合优化，以提高精度和一致性。对于深度值来自激光雷达测量的深度点，我们在BA中不对它们的深度值进行优化；而对于深度值由深度滤波器估计的非深度点，我们会结合关键帧的姿态来优化其深度值。 对于任意一个点 $\mathbf{p}_1 = (\mathbf{u}_1, d_{\mathbf{u}_1}) \in P_{k_h}$，其所属帧为 $k_h$（即 $h \in [1, n-1]$），通过我们的点匹配与传播算法，可以在滑动窗口中找到它在其他关键帧（如 $\mathbf{p}_1$）上的匹配点 $\mathbf{u}_1^{k_j}$（例如 $k_j$）。不过，有匹配点对并不意味着可以构建双向重投影误差，因为如果没有激光雷达点投影到 $\mathbf{u}_1^{k_j}$ 上，就无法获得 $\mathbf{u}_1^{k_j}$ 的深度观测值。 具体来说，匹配对 $(\mathbf{u}_1, \mathbf{u}_1^k)$ 从 $k_j$ 到 $k_h$ 的重投影残差表示为 $$ \mathbf{r}_{k_{h}k_{j}}^{\mathbf{p}_{1}}=\pi\left(\mathbf{T}_{c_{0}}^{k_{h}}\mathbf{T}_{k_{j}}^{c_{0}}\boldsymbol{\pi}^{-1}\left(\mathbf{u}_{1}^{k_{j}},d_{\mathbf{u}_{1}}^{k_{j}}\right)\right)-\mathbf{u}_{1} $$ 其中深度值$d_{\mathbf{u}_1}^{k_j}$对于构建残差而言是必不可少的，但很可能没有激光雷达点被投影到$\mathbf{u}_1^k$上，进而导致$d_{\mathbf{u}_1}^{k_j}$无法获取。通过插值生成$d_{\mathbf{u}_1}^{k_j}$可能会引入非同小可的误差。因此，我们总能针对匹配对$(\mathbf{u}_i, \mathbf{u}_i^k)$从$k_h$到$k_j$构建重投影误差，但却可能无法得到该匹配对从$k_j$到$k_h$的重投影误差。这一限制使得重投影误差只能从旧关键帧构建到新关键帧，进而造成滑动窗口内关键帧的状态不一致，降低了多帧联合优化的效果。 我们提出的反向投影方法能够有效解决上述问题，且不会引入任何深度插值误差。具体而言，当最新的关键帧$k_n$到来时，我们按照第V-C节所述的方法在$P_{k_n}$上找到$k_1 \sim k_{n-1}$的二维对应点。为避免匹配点之间的间隔过长，我们首先在$\mathbf{p}_i \in P_{k_n}$上找到$k_{n-1}$的二维对应点。接着，在寻找$\mathbf{u}_{i}^{k_{n-2}}$在$\mathbf{u}_{i}^{k_{n-1}}$上的二维对应点时，将$\mathbf{p}_i$与$k_{n-2}$进行匹配。整个过程递归执行，直到在$\mathbf{p}_i$上找到$k_1$的对应点为止。对每个关键帧$k_n$完成点的反向匹配后，我们就能在滑动窗口中找到其提取出的点（即$P_{k_n}$）在其他关键帧（即$k_j \in K$）上的对应点。随后我们执行基于滑动窗口的BA优化$\boldsymbol{\chi} = \{\mathbf{T}_{k_1}^{c_0}, \mathbf{T}_{k_2}^{c_0}, \ldots, \mathbf{T}_{k_n}^{c_0}\}$和$\phi = \{d_{u_1}, d_{u_2}, \ldots, d_{u_m}\}$，其中$\phi$是滑动窗口内所有非深度点的深度值集合。 $$ \mathcal{X},\phi=\underset{\mathcal{X},\phi}{\mathrm{a r g m i n}}\sum_{k_{h}\in K}\sum_{\mathbf{p}_{i}\in P_{k_{h}}}\sum_{k_{j}\in K}\left\|\mathbf{r}_{k_{j}k_{h}}^{\mathbf{p}_{i}}\right\|_{\gamma}^{2}\quad(j\neq h) $$ $\mathbf{r}_{k_j k_h}^{\mathbf{p}_i}$是由三个元素决定的单一重投影残差：宿主帧$k_h$、点$\mathbf{p}_i = (\mathbf{u}_i, d_{\mathbf{u}_i})$以及目标帧$\mathbf{p}_i$上$k_j$的二维对应点（即$\mathbf{u}_i^{k_j}$）。公式14旨在联合最小化滑动窗口内的所有重投影残差。 与[^6]类似，我们利用边缘化方法来减轻窗口优化的计算负担，同时保留以往的信息。 ### G. 变换集成 对于关键帧$k_j$，我们直接采用基于滑动窗口的BA模块输出的$\mathbf{T}_{k_j}^{c_0}$作为其最终位姿。但对于非关键帧$c_i$，我们则整合帧到关键帧的位姿 --- ![Image](images/bb940aef9e79e166f981ea7d5a974bbf5abde921044ded020f4686292054aa5a.jpg)
Fig. 5. Illustration of our sweep reconstruction algorithm. 10 Hz input raw point cloud $S_j$, $S_{j+1}$ and $S_{j+2}$ are overlapped and reconstructed to obtain 60 Hz reconstructed point clouds $S_{i+7}$, $S_{i+8}$, ..., $S_{i+17}$.
利用位姿优化模块中两帧之间的位姿，以及基于滑动窗口的BA模块中得到的关键帧到地图的位姿，通过$\mathbf{T}_{c_i}^{c_0}$计算出其最终输出位姿$\mathbf{T}_{c_i}^{c_0} = \mathbf{T}_{k_n}^{c_0} \mathbf{T}_{c_i}^{k_n}$，其中$\mathbf{T}_{k_n}^{c_0}$是$k_n$之前最近的关键帧$c_i$的位姿。随后，$\mathbf{T}_{c_i}^{c_0}$会经由$\mathbf{T}_{l_i}^{w}$转换为$\mathbf{T}_{l_i}^{w} = \mathbf{T}_c^l \mathbf{T}_{c_i}^{c_0} \mathbf{T}_l^c$，再被送入激光雷达里程计模块。 ### VI. 激光雷达里程计 视觉模块与激光雷达模块输出位姿结合时的一个问题是，图像和激光雷达点的输入频率不同。现有方法[^16]、[^17]会将高频的相机位姿与低频的激光雷达位姿进行融合，从而得到最终输出位姿。在本文中，我们选择根据相机图像的时间戳来重叠并重构扫描数据，以确保视觉模块输出的每个位姿都能经过激光雷达里程计的优化。 此外，在垂直几何约束有限的情况下，直接利用激光雷达点进行跟踪可能会导致垂直方向上的较大漂移。为缓解这一问题，我们提出了一种自适应扫描线到地图优化方法，该方法能够根据当前输入扫描线在垂直方向上的几何约束数量，自动判断是优化三个垂直自由度（即横滚、俯仰和上下移动）还是优化六自由度位姿。 激光雷达模块同时包含局部地图和全局地图。全局地图以体素地图的形式存储，而局部地图则是整个体素地图的一部分。每次将点云注册到局部地图时，全局地图也会相应更新。我们采用与CT-ICP相同的采样策略，在将点云注册到局部地图之前对当前扫描线的点云进行采样。在将当前扫描线的采样点云注册到局部地图时，我们利用当前扫描线的优化后位姿将每个点转换到世界坐标系下，进而确定其所属的体素。为了更新全局/局部体素网格，我们采用了CT-ICP的处理流程：如果一个体素中已有20个点，则不再向该体素中添加新点；否则，便将该点加入体素中。 ### A. 扫描线重建 扫描重建旨在从 10 Hz 的原始输入点云 S 推导出 60 Hz 的重建点云 $S_i$。图 5 展示了我们扫描重建的过程。假设对于一次 LiDAR 扫描 $S_j$：$[t_{j-1}, t_j]$，它从 $t_{j-1}$ 开始，到 $t_j$ 结束，在 $[t_{j-1}, t_j]$ 期间捕获了 6 张相机图像。这 6 张相机图像分别在 $t_{i+1}$、$t_{i+2}$、$t_{i+3}$、$t_{i+4}$、$t_{i+5}$ 和 $t_{i+6}$ 捕获（$t_i = t_{j-1}, t_{i+6} = t_j$）。只有在 $t_{i+6}$ 捕获的相机图像具有对应的 LiDAR 点。 基于激光雷达在一段时间内连续采集数据的特性，我们将从$t_{i+1}$到$t_{i+7}$期间采集的点定义为重建扫描线$S_{i+7}$: $[t_{i+1}, t_{i+7}]$，它对应拍摄于$t_{i+7}$的相机图像。同理，我们还能得到重建扫描线$S_{i+8}$: $[t_{i+2}, t_{i+8}]$、$S_{i+9}$: $[t_{i+3}, t_{i+9}]$、……、$S_{i+17}$: $[t_{i+11}, t_{i+17}]$，它们分别对应拍摄于$t_{i+8}$、$t_{i+9}$、……、$t_{i+17}$的相机图像。通过这种方式，我们获得了与输入相机图像频率一致的重建扫描线。 值得一提的是，在绘图过程中我们并不会对每个重建扫描线$S_{i}$进行注册，而只会以10 Hz的频率将点添加到最终输出地图中，以避免重复添加点。 与同样旨在获得高频最终输出位姿的变换积分方法（V-LOAM中所采用的）相比，我们的扫描线重建能更好地保持估计轨迹的一致性。下文我们将详细解释变换积分中的不一致性问题。 --- 对于在 $t_i$ 时刻捕获的相机帧，我们将 $t_i$ 之前完成的最近一次扫描记为 $t_n$，而在 $t_n$ 时刻的扫描到地图位姿为 $\mathbf{T}_{l_n}^w$。通过变换积分，可估算出 $t_i$ 时刻的最终位姿（例如 $\mathbf{T}_{l_i}^{w}$）。 $$ \mathbf{T}_{l_{i}}^{w}=\mathbf{T}_{l_{n}}^{w}\mathbf{T}_{l}^{c-1}\mathbf{T}_{c_{i}}^{c_{n}}\mathbf{T}_{l}^{c} $$ 其中，$\mathbf{T}_{c_i}^{c_n}$ 是来自视觉里程计的位姿，$\mathbf{T}_l^c$ 是激光雷达与相机之间的外部参数。在实际应用中，$\mathbf{T}_l^c$ 通常只需离线标定一次，在在线位姿估计过程中保持不变。然而，外部参数的精确标定并非易事，而广泛使用的标定方法 [^38] 仍然存在明显误差。此外，由于车辆平台运动过程中的振动，外部参数通常会在在线阶段发生变化。一旦通过公式(15)计算出 $\mathbf{T}_{l_i}^w$，变换积分过程并不会对 $\mathbf{T}_{l_i}^w$ 进行任何优化，而是直接将其作为时间 $t_i$ 的最终位姿输出。结果就是，上述 $\mathbf{T}_l^c$ 中的误差并未得到减小，进而导致估计轨迹出现不一致现象，这一现象在第VII-B3节中有详细说明。 与变换积分相比，我们的扫描重建技术能够提高输入的激光雷达扫描频率，从而让视觉里程计生成的每个输出位姿都能经由激光雷达模块进行优化。具体而言，对于时间 $t_i$ 下的某个示例性相机位姿（例如 $\mathbf{T}_{c_i}^{c_0}$），我们首先将其从相机坐标系转换到激光雷达坐标系，具体过程如下： $$ \mathbf{T}_{l_{i}}^{w}=\mathbf{T}_{l}^{c^{-1}}\mathbf{T}_{c_{i}}^{c_{0}}\mathbf{T}_{l}^{c} $$ 随后，$\mathbf{T}_{l_i}^{w}$ 会被送入自适应扫描-地图优化模块，在此过程中 $\mathbf{T}_l^c$ 带来的误差便能被消除。因此，我们的扫描重建技术在实现高精度、高效率的位姿估计的同时，还能保持估计轨迹良好的局部一致性。 ### B. 自适应扫描-地图优化 我们在激光雷达模块中设计的位姿求解方法基于CT-ICP [^19] 中的算法，并引入了自适应扫描到地图优化的改进。具体而言，我们会根据重建扫描中垂直方向的几何约束数量，自动决定是优化3个水平自由度（即偏航角、前进方向和右向移动）还是6个完整自由度的位姿 $S_{i}$。具体来说，我们会检测指向垂直方向的表面是否均匀分布在同一个平面上。如果几乎所有垂直于竖直方向的点云表面都位于同一平面（通常是地面），那么这些表面提供的残差难以有效约束垂直方向，此时我们仅优化3个水平自由度的位姿。垂直方向上的位姿误差评估详见第VII-A1节。 当每个重建扫描 $S_i$ 到达时，我们采用第 V-A 节中介绍的方法来检测地面并计算地面法向量 $n_G$。在构建用于位姿估计的点到平面残差时，我们在每个目标点 $\varepsilon$ 周围找到表面 $\mathbf{p}$，并计算 $\varepsilon$ 的法向量 n。如果 n 的方向与 $n_G$ 非常相似，我们将 $\mathbf{p}$ 视为地面候选点。我们统计 $S_i$ 中候选点 N 的数量，以及位于地面的点 M 的数量。我们通过 $M/N$ 计算地面表面积与垂直表面积的占比。如果 $M/N$ 高于预设阈值（在我们系统中为 0.8），则认为垂直几何约束较差。在这种情况下，我们仅针对当前重建扫描 $S_i$ 优化 3 个水平自由度（即偏航、前向和横向）。否则，我们按照 CT-ICP 中的处理方式优化全部 6 个自由度。 我们将$S_i$定义为当前重建扫描点在坐标系中的点云（其中$\cdot\rangle^{l_i}$、$t_b$和$t_e$分别为$S_i$的起始时间和结束时间）。对于每个$S_i$，我们首先通过CT-ICP中的下采样策略从$P_i$中提取一组点$S_i$。对于在时间$\mathbf{p}^l \in P_i$采集的任意点$t_p$，我们借助视觉模块获取的$\mathbf{p}^l$的初始位姿，将$\cdot\rangle^{l_i}$从($\cdot\rangle^w$处投影到($S_i$)，从而得到$\mathbf{p}^w$。接着，我们在局部地图中找到$\mathbf{p}^w$周围最近的20个点来拟合平面，并计算该平面的法向量$\mathbf{n}$。据此，我们可以为$r^{\mathbf{p}^l}$构建如下的点到平面残差$\mathbf{p}^l$： $$ r^{\mathbf{p}^{l}}=\omega_{p}\mathbf{n}^{T}\left(\mathbf{p}^{w}-\mathbf{q}\right) $$ $$ \mathbf{p}^{w}=\mathbf{R}_{t_{p}}^{w}\mathbf{p}^{l}+\mathbf{t}_{t_{p}}^{w} $$ $$ \mathbf{R}_{t_{p}}^{w}=s l e r p\left(\mathbf{R}_{b},\mathbf{R}_{e},\alpha_{p}\right) $$ $$ \mathbf{t}_{t_{p}}^{w}=\left(1-\alpha_{p}\right)\mathbf{t}_{b}+\alpha_{p}\mathbf{t}_{e} $$ $$ \alpha_{p}=\frac{t_{p}-t_{b}}{t_{e}-t_{b}} $$ 其中$\mathbf{q}$是距离$\mathbf{p}^w$最近的点，$\omega_p$由[^20]定义，$\mathbf{R}_b$、$\mathbf{R}_e$分别是$(\cdot)^w$和$t_b$时刻相对于$t_e$的旋转矩阵，$\mathbf{t}_b$和$\mathbf{t}_e$分别是$(\cdot)^w$和$t_b$时刻相对于$t_e$的平移向量。$\mathbf{R}_b$、$\mathbf{R}_e$、$\mathbf{t}_b$、$\mathbf{t}_e$均为我们自适应扫描到地图模块中需要优化的变量，而$\mathbf{R}_e$和$\mathbf{t}_e$的初始值由视觉模块提供。 为了更好地保持轨迹的一致性，我们额外对时间间隔$(t_b, t_e)$内的位姿进行优化。假设在时间$t_j$（$0 \leq j \leq k-1$）和$t_j \in (t_b, t_e)$获取了k个重建扫描。对于在子时间间隔$\mathbf{p}^l$内被处理并获取的点$(t_j, t_{j+1})$，我们在当前优化轮次中对其对应的位姿（即$\mathbf{R}_j$、$\mathbf{R}_{j+1}$、$\mathbf{t}_j$、$\mathbf{t}_{j+1}$）进行优化。具体来说，我们构建了类似于式(17)中$r_a^{\mathbf{p}^l}$的额外点到平面残差$r^{\mathbf{p}^l}$。 $$ r_{a}^{\mathbf{p}^{l}}=\omega_{p}\mathbf{n}^{T}\left(\mathbf{p}^{w}-\mathbf{q}\right) $$ $$ \mathbf{p}^{w}=\mathbf{R}_{t_{p}}^{w}\mathbf{p}^{l}+\mathbf{t}_{t_{p}}^{w} $$ $$ \mathbf{R}_{t_{p}}^{w}=s l e r p\left(\mathbf{R}_{j},\mathbf{R}_{j+1},\alpha_{p}\right) $$ $$ \mathbf{t}_{t_{p}}^{w}=\left(1-\alpha_{p}\right)\mathbf{t}_{j}+\alpha_{p}\mathbf{t}_{j+1} $$ $$ \alpha_{p}=\frac{t_{p}-t_{j}}{t_{j+1}-t_{j}} $$ 其中$r_a^{\mathbf{P}^l}$是与变量$\mathbf{R}_j$、$\mathbf{R}_{j+1}$、$\mathbf{t}_j$、$\mathbf{t}_{j+1}$相关的额外残差。通过这种方式，对于任意点$\mathbf{p}^l$，我们可以构建两个残差（例如$r^{\mathbf{P}^l}$和$r_a^{\mathbf{P}^l}$）来优化八个变量，即$\mathbf{R}_b$、$\mathbf{R}_e$、$\mathbf{R}_j$、$\mathbf{R}_{j+1}$、$\mathbf{t}_b$、$\mathbf{t}_e$、$\mathbf{t}_j$、$\mathbf{t}_{j+1}$。 为了便于优化三个水平自由度，我们将旋转表示为 $$ \mathbf{R}=\mathbf{R}_{y}\mathbf{R}_{x}\mathbf{R}_{z} $$ ---
TABLEI
ABLATION STUDY OF OUR VISUAL ODOMETRY
DatasetSeq no.Ours w/o ProOurs w/o Bi-DirOurs w/o AdaOur VO module
KITTI000.690.700.810.67
010.981.092.920.96
020.800.760.900.75
030.860.840.800.86
040.750.751.300.77
050.680.670.860.66
060.460.580.480.44
070.820.810.880.74
081.111.101.191.07
090.550.600.550.53
100.500.600.570.51
00-10
avg0.750.771.020.72
11-21---0.88
mean---0.88
KITTI-360001.031.111.320.81
020.930.811.110.79
030.831.411.220.90
041.291.691.601.14
054.952.402.941.86
061.221.051.470.94
071.3310.261.721.31
091.211.151.431.03
102.264.003.071.89
avg1.672.651.761.18
KITTI-CARLA012.590.450.640.32
022.631.271.010.61
071.880.690.970.38
avg2.370.800.870.44
Denotations: All errors are represented as RTE[%] (the smaller the better). Bold fonts denote the first place. w/o: without, Pro: propagation, Bi-Dir: bi-directional point matching, Ada: adaptive tracking module selection, -: not available.
TABLE II
PIXEL ERROR OF CORRESPONDENCES AT DIFFERENT FRAME INTERVALS ON KITTI 00-10
Interval (keyframe)1234567
w/p Pro (pixel)2.864.876.557.938.869.0410.40
w Pro (pixel)1.853.154.285.266.046.396.77
TABLE III
RELATIVE TRANSLATIONAL ERROR OF FAST-LOAM AND FAST-LOAM++ IN X-Y-Z DIRECTION [%]
Seq no.Fast-LOAMFast-LOAM++
XYZXYZ
000.7729.410.500.839.740.69
010.88138.100.560.7719.201.28
020.9546.960.311.2911.660.58
030.5110.771.160.712.562.64
0428.5970.870.1139.0510.510.52
050.6025.790.781.237.041.40
064.6431.810.188.387.990.32
070.4123.430.360.7410.371.15
081.2721.241.411.8813.581.73
090.5232.050.360.8110.260.39
100.7628.770.660.384.430.49
![Image](images/8b9cf80c3177747d827924a731d27da91db54134c56bb59156ddb4742e149b88.jpg)
Fig. 6. Sensor configuration for experiments on our own platform. The sensor system is mounted on a car-like vehicle for outdoor experiments.
![Image](images/1252abdf1b723a3a74993ff4b2542728fb707aa4aeda0a4aec7913d03feaa346.jpg)
(a)
![Image](images/0d64fe1948829cc90654f6e5036aec649a2423c2ab3aa37be0031a082b553df3.jpg)
(c)
(b)
![Image](images/b3a78ef020082072f60e5b5ee0e01daa7868e88fb013f3bc4848dae28e16e0e9.jpg) ![Image](images/e3a5efa5fa89bad9caccc0ea204729d176f0a69b08a5609b348b18493d4a8947.jpg)
(d)
Fig. 7. The results of our outdoor experiments. (a) and (c) are the trajectory estimated by our system on two different vehicle routes. We overlaid (a) and (c) with Google Map to obtain (b) and (d) for better evaluation.
(a)
![Image](images/e627a1ccce1617263390d74b7d1d0df7d28d42af58409891855339cf0b9c211c.jpg) ![Image](images/7d387cbcb38c141b4abfb20e585d71c05db3af5976f3e0b55f7c3f92a51789aa.jpg)
(b)
Fig. 8. The resulting global point cloud map of trajectory Fig. 7(a) and (c).
---
TABLE IV ABLATION STUDY OF OUR LIDAR ODOMETRY
DatasetSeq no.A-LOAMA-LOAM+A-LOAM++LeGO-LOAMLeGO-LOAM+LeGO-LOAM++Fast-LOAMFast-LOAM+Fast-LOAM++
KITTI000.960.880.775.031.250.920.960.760.62
012.682.651.9121.023.351.272.801.111.00
025.041.521.472.801.740.831.571.090.84
031.161.151.031.321.120.851.091.670.94
041.391.360.771.631.730.611.430.770.61
050.710.680.650.930.870.510.800.700.57
060.720.720.550.840.850.440.720.480.47
070.540.530.490.770.660.480.551.040.76
081.181.180.981.581.350.941.161.171.08
091.211.190.881.481.750.611.290.740.69
101.611.571.431.861.961.091.770.860.49
00-10
avg1.561.220.993.571.510.781.340.940.76
11-21
mean---------
KITTI-360001.031.000.951.601.641.251.170.740.60
021.660.830.761.891.421.190.930.890.79
031.861.811.782.442.501.291.880.790.62
041.601.331.271.861.681.611.381.161.03
051.011.001.011.601.361.331.001.171.08
061.111.131.021.761.451.361.231.120.94
074.091.721.683.243.082.471.691.381.20
091.131.050.971.631.681.441.210.810.68
101.742.031.882.942.401.281.841.371.31
avg1.691.321.252.101.911.471.371.050.91
KITTI-CARLA014.150.170.130.450.780.410.120.110.10
020.120.100.102.070.880.630.080.090.07
073.230.620.202.491.250.901.640.400.38
avg2.500.300.141.670.970.650.610.200.18
DatasetSeq no.ISC-LOAMISC-LOAM+ISC-LOAM++MULLSMULLSMULLSCT-ICPCT-ICP+CT-ICP++
KITTI001.211.030.840.530.520.520.490.490.50
012.801.111.000.720.670.580.730.690.62
021.721.230.840.590.590.570.520.520.51
031.091.670.940.600.620.600.710.700.56
041.430.770.610.480.460.420.370.360.37
050.840.980.620.310.310.310.260.250.27
060.690.480.470.290.290.260.280.280.28
070.551.040.760.330.340.330.320.320.32
081.161.171.080.820.810.810.810.810.79
091.290.740.690.550.510.470.490.480.47
101.770.860.490.640.640.520.490.490.47
00-10
avg1.321.010.760.530.520.490.500.490.47
11-21
mean---0.92--0.62-0.60
KITTI-36000---1.621.581.140.420.400.40
02---1.311.331.260.320.320.31
03---1.041.020.740.370.370.35
04---1.691.651.520.680.670.66
05---1.311.291.210.390.380.37
06---1.811.801.410.430.420.40
07---5.725.241.310.540.530.52
09---1.281.281.160.430.430.42
10---0.930.920.830.730.710.70
avg---1.851.791.170.480.470.46
KITTI-CARLA01---0.560.540.530.0380.0380.035
02---0.200.230.150.0400.0390.038
07---1.041.010.390.0440.0450.040
avg---0.600.590.360.0410.0410.038
标注说明：所有误差均以RTE[%]表示（数值越小越好）。粗体字体表示第一名。+：使用视觉模块估算的位姿作为激光雷达模块的初始值；+：在“+”的基础上采用自适应扫描到地图优化策略；-：数据不可用；“/”右侧的数据为原文中记录的结果，左侧的数据是我们利用作者提供的开源代码测得的结果。 ---
TABLE V
RELATIVE TRANSLATIONAL ERROR COMPARISON OF LiDAR-ASSISTED DEPTH-ENHANCED VO ON KITTI ODOMETRY
Seq no.DEMOLIMO $^{{*}}$Huang et al.DVL-SLAMOur VO module
001.051.120.990.930.67
011.870.911.871.470.96
020.93-1.381.110.75
030.99-0.650.920.86
041.230.530.420.670.77
051.04-0.720.820.66
060.96-0.610.920.44
071.16-0.561.260.74
081.24-1.271.321.07
091.17-1.060.660.53
101.14-0.830.700.51
00-10 avg1.160.850.941.980.72
11-21 mean1.140.93--0.88
标注说明：所有误差均以RTE[%]表示（数值越小越好）。粗体字体表示第一名。U：城市道路；H：高速公路；C：乡村道路；$*$：包含语义信息；-：数据不可用。 其中 $\mathbf{R}_x$、$\mathbf{R}_y$、$\mathbf{R}_z$ 是根据欧拉角 $r_x$、$r_y$、$r_z$ 按照 Y-X-Z 顺序规则重构得到的。相应地，需要求解的最终变量为 $$ \boldsymbol{x}=\left[r_{x},r_{y},r_{z},t_{x},t_{y},t_{z}\right]^{T} $$ $$ \chi=\left[x^{b},x^{e},x^{0}\ldots,x^{k-1}\right] $$ 其中 $\mathbf{t} = [t_x, t_y, t_z]^T$。将所有点到平面残差整合到一个优化函数中，我们便可得到 $$ \mathcal{X}=\underset{\mathbf{x}}{\mathrm{a r g m i n}}\sum_{\mathbf{p}^{l}\in P_{i}}\rho\left(r^{\mathbf{p}^{l}}+r_{a}^{\mathbf{p}^{l}}\right) $$ 其中 $\rho(\cdot)$ 是一种鲁棒的损失函数，用于降低异常值残差的影响。如果将垂直几何约束视为较弱，我们仅针对当前重构的扫描区域优化 3 个水平自由度（即偏航、前进、向右），而需要求解的变量为 $$ \boldsymbol{x}_{3}=\left[r_{y},t_{x},t_{z}\right]^{T} $$ $$ \begin{array}{r}{\chi_{3-D O F}=\left[\boldsymbol{x}_{3}^{b},\boldsymbol{x}_{3}^{e},\boldsymbol{x}_{3}^{0},\ldots,\boldsymbol{x}_{3}^{k-1}\right]}\end{array} $$ 优化函数为 $$ \mathcal{X}_{3-D O F}=\underset{\mathcal{X}_{3-D O F}}{\mathrm{a r g m i n}}\sum_{\mathbf{p}^{l}\in P_{i}}\rho\left(r^{\mathbf{p}^{l}}+r_{a}^{\mathbf{p}^{l}}\right) $$ 如果垂直几何约束足够强，我们将 (29) 中的内容设为变量，并根据 (30) 执行完整的 6 自由度优化，这与 CT-ICP [^19] 的处理方式一致。无论是 3 自由度还是 6 自由度优化，我们都在系统中采用了与 CT-ICP 相同的局部一致性约束和恒定速度约束。 在估算出当前重构扫描区域的位姿后，我们便依据第 VI 节所述方法，将 $P_{i}$ 中的点注册到局部地图中。 ### VII. 实验 我们在公共数据集 [^21]、[^22]、[^23] 以及我们自研的相机-激光雷达传感器硬件系统上对我们的 SDV-LOAM 算法进行了评估。所有实验均使用配备英特尔酷睿 i7-11700 处理器和 16 GB 内存的消费级笔记本电脑完成。 ### A. 在公共数据集上的评估 我们在三个公共数据集上对 SDV-LOAM 算法进行了评估：KITTI [^21]、KITTI-360 [^22] 和 KITTI-CARLA [^23]。KITTI 和 KITTI-360 的数据均来自安装在乘用车顶部的传感器在道路驾驶场景下采集的数据。KITTI 车辆配备了彩色立体相机、单色立体相机、Velodyne HDL-64E 激光雷达扫描仪以及用于采集真实值的高精度 GPS/INS 设备；KITTI-360 车辆则配备了透视立体相机、两台鱼眼相机、Velodyne HDL-64E 激光雷达扫描仪以及用于采集真实值的高精度 GPS/INS 设备。KITTI-CARLA 是由 CARLA 模拟器 [^39] 生成的虚拟数据集，其中的数据由模拟的 64 通道旋转式激光雷达传感器和模拟立体相机产生。对于这三个数据集，我们仅将激光雷达点云和立体相机的左图像作为输入。KITTI、KITTI-360 和 KITTI-CARLA 的激光雷达点云和相机图像采集频率均为 10 Hz。KITTI 提供的点云数据中的运动畸变已被消除，KITTI-CARLA 中的点云运动畸变则通过高频真实值进行了补偿。因此，在评估 KITTI 和 KITTI-CARLA 时我们无需处理畸变问题。 KITTI里程计基准测试包含11个提供GPS/INS真实值的序列（即训练集）以及11个没有真实值的序列（即测试集）。训练集中的最高行驶速度可达85公里/小时（23.6米/秒）。这些数据主要涵盖三种类型的环境：“城市”环境中有建筑物，“乡村”环境为有小路和植被的场景，“高速公路”则道路宽阔且车辆行驶速度快。我们在KITTI训练集上评估系统的各个模块。对于测试集，我们会在官方网站上公布结果。$^{2}$ KITTI-360 [^22]包含8个序列，其场景与KITTI类似，但长度远长于KITTI。KITTI-CARLA [^23]由7个序列组成，每个序列包含5000张立体图像和扫描数据。在本研究中，我们仅测试KITTI-CARLA的序列01、02和07，因为其他序列均为无纹理场景，我们的视觉里程计无法在其中正常工作。对于所有这三个数据集，我们均采用通用的评估指标——KITTI相对平移误差（RTE）作为评估标准。由于我们的SDV-LOAM由视觉里程计和激光雷达里程计组成，我们在第VII-A1节中对每种里程计进行了消融研究，随后在第VII-A2节和第VII-A3节中将我们的视觉里程计及SDV-LOAM与现有最先进方法进行比较。 1) 消融研究：我们的视觉里程计的消融研究。我们通过从视觉模块中排除其中一个组件，来检验所提出的三个组件的有效性。在表I中，“Ours w/o Pro”、“Our w/o Bi-Dir”和“Ours w/o Ada”分别表示未采用点匹配传播、双向点匹配以及自适应跟踪机制的视觉里程计。 ---
TABLE VI
RELATIVE TRANSLATIONAL ERROR COMPARISON OF VISUAL-LiDAR ODOMETRY
DatasetSeq no.A-LOAM+LeGO-LOAM+Fast-LOAM+ISC-LOAM+MULLS+CT-ICP+V-LOAMOurs
KITTI000.881.250.761.030.520.49-0.50
012.653.351.111.110.670.69-0.62
021.521.741.091.230.590.52-0.51
031.151.121.671.670.620.70-0.56
041.361.730.770.770.460.36-0.37
050.680.870.700.980.310.25-0.27
060.720.850.480.480.290.28-0.28
070.530.661.041.040.340.32-0.32
081.181.351.171.170.810.81-0.79
091.191.750.740.740.510.48-0.47
101.571.960.860.860.640.49-0.47
00-10
avg1.221.510.941.010.520.49-0.47
11-21
mean------0.540.60
KITTI-360001.001.640.74-1.580.40-0.40
020.831.420.89-1.330.32-0.31
031.812.500.79-1.020.37-0.35
041.331.681.16-1.650.67-0.66
051.001.361.17-1.290.38-0.37
061.131.451.12-1.800.42-0.40
071.723.081.38-5.240.53-0.52
091.051.680.81-1.280.43-0.42
102.032.401.37-0.920.71-0.70
avg1.321.911.05-1.790.47-0.46
KITTI-CARLA010.170.780.11-0.540.038-0.035
020.100.880.09-0.230.039-0.038
070.621.250.40-1.010.045-0.040
avg0.300.970.20-0.590.041-0.038
说明：所有误差均以RTE[%]表示（数值越小越好）。粗体字表示第一名。-：暂不可用。 首先，对比“Ours w/o Pro”与我们的视觉里程计模块可知，点匹配传播机制能够降低大多数序列的RTE值。我们进一步展示了不同帧间隔下的匹配误差（即真实匹配像素与匹配像素之间的距离）。表II中的结果显示，若不使用我们的点匹配方法，随着帧间隔增大，匹配误差会急剧上升；相比之下，我们的点匹配方法能显著减小不同帧间隔下的匹配误差。其次，从表I中“Ours w/o Bi-Dir”与我们的视觉里程计模块的对比可见，双向点匹配方法可使KITTI-360和KITTI-CARLA所有序列的RTE分别降低2.47%~87.23%和28.89%~51.97%，对于KITTI的序列00-10而言，除序列04外，RTE可降低1.32%~24.14%。最后，从“Ours w/o Ada”与我们的视觉里程计模块的结果来看，自适应跟踪模式选择方法能使KITTI-360的跟踪误差降低23.84%~38.64%，KITTI-CARLA的跟踪误差降低39.60%~60.82%，KITTI训练序列00-10的跟踪误差降低3.64%~67.12%，其中序列03除外。 我们对 LiDAR 里程计的消融研究。我们分析了自适应扫描到地图优化在 LiDAR 里程计中的有效性。为此，我们提出了方向姿态误差，它是通过使用 100 m、200 m、……、800 m 的分段轨迹，基于每个方向（即 X（向右）、Y（向上）和 Z（向前））的 3D 坐标对相对误差进行平均来计算的。具体而言，对于每个长度 $\Delta \in \{100\,m, 200\,m, \ldots, 800\,m\}$，我们将 $\Delta$ 起始处的估计姿态定义为 $\widehat{\mathbf{T}}_b$，将 $\Delta$ 结束处的估计姿态定义为 $\widehat{\mathbf{T}}_e$。类似地，我们将 $\Delta$ 起始处的真实姿态定义为 $\mathbf{T}_b$，将 $\Delta$ 结束处的真实姿态定义为 $\mathbf{T}_e$。然后，从 $\Delta$ 结束到 $\Delta$ 起始的相对姿态通过以下方式计算：$\Delta\widehat{\mathbf{T}} = \widehat{\mathbf{T}}_b^{-1}\widehat{\mathbf{T}}_e$ 和 $\Delta\mathbf{T} = \mathbf{T}_b^{-1}\mathbf{T}_e$。相应地，姿态误差通过 $\Delta\widehat{\mathbf{T}}^{-1}\Delta\mathbf{T}$ 计算得出。我们还统计了 $\Delta$ 期间在 X-Y-Z 中行进的总距离，以获得 $s_x, s_y$ 和 $s_z$。最后，$\Delta$ 期间在 X-Y-Z 中的误差通过以下方式计算 $$ \begin{aligned}\boldsymbol{e}_{x}&=\left[\Delta\widehat{\mathbf{T}}^{-1}\Delta\mathbf{T}\right]_{x}/s_{x}*100\%\\\boldsymbol{e}_{y}&=\left[\Delta\widehat{\mathbf{T}}^{-1}\Delta\mathbf{T}\right]_{y}/s_{y}*100\%\\\boldsymbol{e}_{z}&=\left[\Delta\widehat{\mathbf{T}}^{-1}\Delta\mathbf{T}\right]_{z}/s_{z}*100\%\end{aligned} $$ 其中$[.]_{x}$、$[.]_{y}$和$[.]_{z}$分别表示该元素在X、Y、Z方向的平移量。表三展示了Fast-LOAM与Fast-LOAM++在KITTI序列00-10中X、Y、Z方向的平移误差，其中的“++”表示采用我们的自适应扫描匹配优化且初始位姿来自视觉模块的结果。显然，Y方向的平移误差明显高于X和Z方向。 我们进一步将自适应扫描匹配优化应用于六种最先进的开源方法，包括A-LOAM、LeGO-LOAM [^9]、Fast-LOAM、ISC-LOAM [^11]、MULLS [^24]以及CT-ICP [^19]。A-LOAM是基于LOAM的工程优化版本；Fast-LOAM和LeGO-LOAM均为LOAM的加速版本。ISC-LOAM在Fast-LOAM的基础上提出了一种全新的全局描述符——强度扫描上下文（ISC），它同时利用几何与强度特征来提升回环检测的效率。MULLS则提出在激光雷达里程计中使用更多类型的特征（如立面、梁、柱和地面），而非传统的边缘和平面特征。CT-ICP采用了弹性建模方式 --- 对轨迹进行建模，确保单次扫描内的位姿连续、扫描间位姿不连续，从而更能适应传感器运动中的高频变化。 对于每种方法，我们在KITTI、KITTI-360和KITTI-CARLA的每个序列上分别测试三种结果：即纯激光雷达系统得出的估计位姿、未采用自适应扫描匹配优化但借助我们视觉模块辅助的激光雷达系统得出的估计位姿（在表四中用后缀“+”表示），以及采用自适应扫描匹配优化且借助我们视觉模块辅助的激光雷达系统得出的估计位姿（在表四中用后缀“++”表示）。几乎没有任何系统会公布能够复现其论文中所记录精度的源代码最终版本。为了公平比较，在消融实验中我们依据作者提供的源代码来获取结果。 表IV中的结果显示，我们提出的自适应扫描到映射优化方法能够显著提升所有六种激光雷达里程计在KITTI、KITTI-360和KITTI-CARLA数据集上几乎所有序列的精度，从而证明了该方法的有效性。值得注意的是，我们没有提供ISC-LOAM在KITTI-360和KITTI-CARLA上的结果，因为这两类数据集的序列过长，导致ISC-LOAM无法正确执行全局图优化。 2) 我们的视觉里程计性能：我们将自己的视觉里程计与四种最先进的激光雷达辅助深度增强型视觉里程计系统进行比较，即DEMO [^12]、LIMO [^13]、Huang等人提出的方法 [^15] 以及DVL-SLAM [^14]。前两者是常用的基于特征的视觉里程计系统，其中LIMO利用语义信息来识别运动物体并包含闭环检测功能；Huang等人提出的方法同时利用点特征和线特征，而DVL-SLAM则采用基于直接法的方式。DEMO、Huang等人提出的方法和DVL-SLAM在序列00-10上的平均RTE值分别直接引自 [^12]、[^15] 和 [^14]；LIMO在序列00-10上的结果则引自 [^15] 中测试所得的结果。由于LIMO所需的语义标签数据仅对序列00、01、04可用，因此仅记录了这些序列的结果。序列11-21上的平均RTE值记录自原始的DEMO论文 [^12] 和LIMO论文 [^13]，而其他方法并未在序列11-21上进行评估。此外，在整个测试过程中，我们对所有序列采用了统一的参数配置。 表V中的结果表明，我们的视觉模块在所有序列上的表现均优于DEMO；在KITTI数据集的大多数序列上，其性能也优于LIMO、Huang等人提出的方法和DVL-SLAM。在序列00-10的平均结果中，我们方法的RTE比DEMO低37.9%，比LIMO低15.3%，比Huang等人提出的方法低23.4%，比DVL-SLAM低26.5；在序列11-21的平均结果中，我们方法的RTE比DEMO低22.8%，比LIMO低5.4。在序列03、04、07上，我们系统的精度低于Huang等人提出的方法，这是因为这三个序列中线特征较为丰富，而Huang等人提出的方法在此类场景下精度更高。我们的视觉里程计在Intel Core i7-11700 CPU上的运行时间为0.06秒/帧。相比之下，LIMO需要语义标签信息来识别运动物体并剔除异常值，因此需借助GPU来提升运算速度；同时，LIMO和Huang等人提出的方法都无法实时运行。而我们的方法、DEMO和DVL-SLAM则轻量得多，能够在仅搭载CPU的平台高效实时运行。我们的视觉里程计在序列11-21上的测试结果也已在官方网站上公布，其名称缩写为“SD-DEVO”。值得一提的是，DEMO正是V-LOAM中所采用的视觉模块，因此与DEMO的比较结果也能体现出我们的视觉里程计相较于V-LOAM的优越性。 我们在KITTI-360和KITTI-CARLA数据集上验证了我们的深度增强型视觉里程计的有效性，结果如表I所示。我们并未在这两类数据集上评估那些最先进的深度增强型视觉里程计系统，原因在于它们要么没有发布源代码（DEMO [^12] 和 Huang等人提出的方法 [^15]），要么所提供的源代码（DVL-SLAM [^14] 和 LIMO [^13]）无法成功编译和运行。此外，这些方法也并未在这两类数据集上进行评估。 3) SDV-LOAM的性能：为了与最先进的方法进行公平比较，我们修改了几种开源的激光雷达里程计系统（即A-LOAM、LeGOLOAM [^9]、Fast-LOAM、ISC-LOAM [^11]、MULLS [^24]以及CT-ICP [^19]），在这些系统中，我们在激光雷达里程计前接入我们的视觉模块以提供运动先验估计。相应的修改后的视觉-激光雷达里程计分别被命名为A-LOAM+、LeGO-LOAM+、Fast-LOAM+、ISC-LOAM+、MULLS+和CT-ICP+。此外，我们还将我们的方法与最先进的紧耦合视觉-激光雷达系统V-LOAM [^16]进行了比较。 表VI中的结果显示，在KITTI基准测试、KITTI-360和KITTI-CARLA的几乎所有序列中，我们的SDV-LOAM的性能都优于A-LOAM+、LeGO-LOAM+、Fast-LOAM+、ISC-LOAM+和MULLS+，这证明了我们所提出系统的有效性。即便在最先进的视觉-激光雷达系统CT-ICP+上，我们的系统在KITTI 01和KITTI 03上仍能取得显著提升。尽管在KITTI数据集上我们的系统无法获得比V-LOAM更精确的位姿估计结果，但我们的系统能够提高输入的激光雷达点云频率（详情见第VII-B2节）。此外，我们将发布SDV-LOAM的源代码，而V-LOAM既未发布其最新版本的源代码，也未发表相关文献。 在测试集上，根据KITTI官方网站评估的结果，我们的平移漂移率仅为0.60%，这一成绩在KITTI里程计基准测试的测试集中排名第八（截至2023年01月），在所有仅使用激光雷达/单目视觉-激光雷达系统中排名第六，在所有开源系统中排名第二。虽然CT-ICP的相对平移误差为0.59%，排名高于我们，但他们发布的代码所实现的相对平移误差为0.62%，而我们的系统仅为0.60%。 ### B. 基于自研传感器系统的评估 为了评估我们的系统在实时环境中的性能，并验证我们所提出的扫描重建模块的有效性，我们搭建了自己的相机-激光雷达传感器系统（见图6）来采集视觉和激光雷达数据，并在一台笔记本电脑上离线运行这些数据。 --- ![Image](images/3bfb43920f938a13bb7fb17b8624b5be7ce49ca057ba3d8768c91f6d4773e10a.jpg)
Fig. 9. The local screenshots of trajectories of Fig. 7(c) in the form of points. (a)-(d) are the results without sweep reconstruction while (e)-(h) are the results with sweep reconstruction. The resolution of points on the trajectory reflects the frequency of the output pose from our LiDAR odometry. The comparison between (a)-(d) and (e)-(h) demonstrates that our sweep reconstruction block can effectively improve the frequency of LiDAR pose estimation.
我们的相机-激光雷达传感器由FL3-U3-13E4M-C工业相机和Velodyne VLP-16激光雷达组成。FLIR相机可输出分辨率为$1280 \times 1040$的灰度图像，帧率为60 Hz。VLP-16则通过16束激光产生相对稀疏的点云，频率为10 Hz。 为了使传感器系统达到最佳性能，我们对异构传感器进行了离线标定。具体来说，我们利用Autoware [^38]工具首先进行标定以获取相机的内部参数，随后再标定相机与激光雷达之间的相对变换关系。 1) 轨迹与地图的可视化：我们的SDV-LOAM在包含闭环场景的户外环境中通过了该传感器系统的验证。该传感器系统被安装在车辆上（如图6所示），在校园和市区中行驶。图7(a)和(c)展示了我们的系统在两条不同车辆路径上估计出的轨迹。为了便于更好地评估，我们将图7(a)和(c)中的轨迹与谷歌地图叠加，得到了图7(b)和(d)中的可视化结果。我们的完整系统中并未包含闭环模块。即便如此，图7(a)–(d)显示，在轨迹的闭环区域仅有极小的偏差，这证明了即便没有闭环功能，我们的系统在大规模场景中的位姿估计仍具有高精度。 我们的系统还能根据激光雷达里程计估算出的精确位姿来构建精确的地图。由该系统生成的图7(a)和(c)中的最终全局地图如图8(a)和(b)所示，该地图以激光雷达点云的形式呈现。 2) 扫描重建的可视化：为了展示扫描重建的有效性，我们展示了局部 ![Image](images/6e34a1f49dca467a5cd452c74038e89a1881992e472c1eeca139c9c0dd4b5d96.jpg)
Fig. 10. The comparative result of trajectory Fig. 7(c) with and without sweep reconstruction, which are almost coincide. This result demonstrates that our method can increase the output frequency without affecting the accuracy of pose estimation.
图9中以点云形式呈现的图7(c)轨迹截图。结果显示，未进行扫描重建的激光雷达里程计的输出频率（图9(a)–(d)）明显低于进行了扫描重建的情况（图9(e)–(h)），也就是说，更高的频率能够生成更密集的轨迹。 我们进一步在同一坐标系下绘制了采用/不采用扫描重建时由该系统估算出的轨迹（如图10所示）。结果表明，我们的扫描重建能够在不降低精度的前提下提高激光雷达里程计的输出频率。值得说明的是，我们的 --- ![Image](images/b39a49cb6ec536f1eec66ccc3835491b41cec076e658b5786286baa58813bc96.jpg)
(a)
![Image](images/0f43f348ad8a6855a2d5e430d220b17874bd79139960486e7361da0e5d0b84b8.jpg)
(b)
![Image](images/69a23193d4f4af1b41751ef60b576b805efa9e5f12f9acc7556cb88248050c1a.jpg)
(C)
Fig. 11. (a) The 10 Hz final pose output of our LiDAR module. (b) and (c) are the 60 Hz final pose output obtained by transform integration and our sweep reconstruction respectively.
理论上，SDV-LOAM能够以60 Hz的频率输出位姿信息。但由于所配备的计算资源有限，实际上我们的视觉模块和激光雷达模块的输出频率仅能达到约20 Hz。若使用高端计算设备或通过多线程加速，输出频率还可进一步提升。 3) 扫描重建的评估：我们在自行采集的数据集上评估了扫描重建技术的优势，该数据集中相机帧的采集频率为60 Hz，激光雷达扫描的采集频率为10 Hz。由于KITTI [^21]、KITTI-360 [^22]以及KITTI-CARLA [^23]中相机图像与激光雷达扫描的频率均为10 Hz，我们无法在这些数据集上进行此项消融实验。图11分别展示了基于我们的激光雷达模块得到的10 Hz最终位姿输出（图11(a)）、基于变换积分得到的60 Hz最终位姿输出（图11(b)）以及通过我们的扫描重建模块得到的输出（图11(c)）。尽管我们自行采集的数据集缺乏用于定量评估的真值，但图11(b)与图11(c)的对比仍能定性地表明：相较于变换积分方法，我们的扫描重建技术能更好地保持估计轨迹的局部一致性。 ### 八、结论 本文介绍了SDV-LOAM，这是一种精确且稳健的单目视觉-LiDAR里程计与建图方法。该方法在一个完整的系统中融合了半直接稀疏深度增强视觉里程计，以及采用自适应扫描到地图优化的LiDAR里程计。与现有采用基于特征或直接方法作为视觉模块的方案不同，我们的SDV-LOAM采用了半直接方法——它结合了基于特征方法的优势（通过重投影误差最小化来实现精确的位姿估计）以及直接方法的优点（在将LiDAR信息整合到视觉模块时，消除了因稀疏特征提取导致的深度关联误差）。针对大多数现有LiDAR里程计在垂直几何信息较弱的场景中位姿估计不精确的问题，我们提出了一种自适应扫描到地图优化模块，该模块能够根据当前扫描数据在垂直方向上的几何信息丰富程度，自动判断是否对3个垂直自由度（即横滚、俯仰和上下移动）进行优化。此外，与现有视觉-LiDAR系统通过将高频但较粗糙的相机位姿与低频但精确的LiDAR位姿叠加来得到最终输出位姿的做法不同，我们提出了一种扫描重建模块，该模块会根据相机图像的时间戳对扫描数据进行重叠和重建，从而提高点云的输入频率，进而理论上提升LiDAR里程计的输出频率。 所提出的方法在公开数据集以及我们自行搭建的硬件系统上均取得了令人满意的结果。我们的整套系统在KITTI训练集上的相对平移误差为0.47%，在KITTI里程计基准测试集上的误差为0.60%，在KITTI-360上的误差为0.46%，在KITTI-CARLA上的误差则为0.038%。同时，基于我们自行搭建的硬件平台在户外环境中所进行的实时评估，也证明了该系统在实际场景中的精确性和鲁棒性。未来的工作包括尝试将LiDAR深度信息作为视觉光束法平差中的软约束。 ## 参考文献 [^1]: 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. [^2]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular SLAM," in Proc. Eur. Conf. Comput. Vis., 2014, pp. 834–849. [^3]: J. Engel, J. Stückler, and D. Cremers, "Large-scale direct slam with stereo cameras," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2015, pp. 1935–1942. [^4]: C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in Proc. IEEE Int. Conf. Robot. Automat., 2014, pp. 15–22. [^5]: C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, "SVO: Semidirect visual odometry for monocular and multicamera systems," IEEE Trans. Robot., vol. 33, no. 2, pp. 249–265, Apr. 2017. [^6]: 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. [^7]: R. Wang, M. Schwörer, and D. Cremers, “Stereo dso: Large-scale direct sparse visual odometry with stereo cameras,” in Proc. IEEE Int. Conf. Comput. Vis., 2017, pp. 3923–3931. ## [^8]: J. Zhang and S. Singh, "LOAM: Lidar odometry and mapping in real-time," in Proc. Robot.: Sci. Syst., vol. 2, no. 9, 2014, pp. 1–9 --- [^9]: 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., 2018, pp. 4758–4765. [^10]: J. Behley and C. Stachniss, “Efficient surfel-based slam using 3D laser range data in urban environments,” in Proc. Robot.: Sci. Syst., vol. 2018, 2018, p. 59. [^11]: H. Wang, C. Wang, and L. Xie, “Intensity scan context: Coding intensity and geometry relations for loop closure detection,” in Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 2095–2101. [^12]: J. Zhang, M. Kaess, and S. Singh, “A real-time method for depth enhanced visual odometry,” Auton. Robots, vol. 41, no. 1, pp. 31–43, 2017. [^13]: J. Graeter, A. Wilczynski, and M. Lauer, “LIMO: Lidar-monocular visual odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018, pp. 7872–7879. [^14]: Y.-S. Shin, Y. S. Park, and A. Kim, “DVL-SLAM: Sparse depth enhanced direct visual-lidar slam,” Auton. Robots, vol. 44, no. 2, pp. 115–130, 2020. [^15]: 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. Automat., 2020, pp. 1091–1097. [^16]: J. Zhang and S. Singh, “Visual-lidar odometry and mapping: Low-drift, robust, and fast,” in Proc. IEEE Int. Conf. Robot. Automat., 2015, pp. 2174–2181. [^17]: J. Zhang and S. Singh, “Laser–visual–inertial odometry and mapping with high robustness and low drift,” J. Field Robot., vol. 35, no. 8, pp. 1242–1264, 2018. [^18]: Z. Yuan, K. Cheng, J. Tang, and X. Yang, “RGB-D DSO: Direct sparse odometry with RGB-D cameras for indoor scenes,” IEEE Trans. Multimedia, vol. 24, pp. 4092–4101, 2022. [^19]: P. Dellenbach, J.-E. Deschaud, B. Jacquet, and F. Goulette, "CT-ICP: Real-time elastic LiDAR odometry with loop closure," in Proc. Int. Conf. Robot. Automat., 2022, pp. 5580–5586. [^20]: J.-E. Deschaud, “IMLS-SLAM: Scan-to-model matching based on 3D data,” in Proc. IEEE Int. Conf. Robot. Automat., 2018, pp. 2480–2485. [^21]: A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving? the kitti vision benchmark suite,” in Proc. IEEE Conf. Comput. Vis. Pattern Recognit., 2012, pp. 3354–3361. [^22]: Y. Liao, J. Xie, and A. Geiger, "KITTI-360: A novel dataset and benchmarks for urban scene understanding in 2D and 3D," IEEE Trans. Pattern Anal. Mach. Intell., vol. 45, no. 3, pp. 3292–3310, Mar. 2023. [^23]: J.-E. Deschaud, "KITTI-CARLA: A kitti-like dataset generated by carla simulator," 2021, arXiv:2109.00892. [^24]: 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. Automat., 2021, pp. 11633–11640. [^25]: R. Mur-Artal, J. M. M. Montiel, and J. D. Tardós, “ORB-SLAM: A versatile and accurate monocular SLAM system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Oct. 2015. [^26]: C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. M. Montiel, and J. D. Tardós, “ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM,” IEEE Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021. [^27]: T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018. [^28]: C. Yu et al., “DS-SLAM: A semantic visual slam towards dynamic environments,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018, pp. 1168–1174. [^29]: C. Kerl, J. Sturm, and D. Cremers, “Dense visual SLAM for RGB-D cameras,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2013, pp. 2100–2106. [^30]: J. Zhang and S. Singh, “Low-drift and real-time lidar odometry and mapping,” Auton. Robots, vol. 41, no. 2, pp. 401–416, 2017. [^31]: D. Rozenberszki and A. L. Majdik, "Lol: Lidar-only odometry and localization in 3D point cloud maps," in Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 4379–4385. [^32]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIO-SAM: Tightly-coupled lidar inertial odometry via smoothing and mapping," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2020, pp. 5135–5142. [^33]: P. Smith, I. Reid, and A. Davison, “Real-time monocular slam with straight lines,” in Proc. Brit. Mach. Vis. Conf., 2006, pp. 17–26. [^34]: A. P. Gee and W. Mayol-Cuevas, “Real-time model-based SLAM using line segments,” in Proc. Adv. Vis. Comput.: 2nd Int. Symp., Tahoe, NV, USA, 2006, pp. 354–363. [^35]: G. Klein and D. Murray, “Improving the agility of keyframe-based SLAM,” in Proc. Eur. Conf. Comput. Vis., 2008, pp. 802–815. [^36]: M. Himmelsbach, F. V. Hundelshausen, and H.-J. Wuensche, “Fast segmentation of 3D point clouds for ground vehicles,” in Proc. IEEE Intell. Veh. Symp., 2010, pp. 560–565. [^37]: 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., 2016, pp. 163–169. [^38]: S. Kato et al., “Autoware on board: Enabling autonomous vehicles with embedded systems,” in Proc. IEEE/ACM 9th Int. Conf. Cyber-Phys. Syst., 2018, pp. 287–296. [^39]: A. Dosovitskiy, G. Ros, F. Codevilla, A. Lopez, and V. Koltun, “CARLA: An open urban driving simulator,” in Proc. 1st Annu. Conf. Robot Learn., 2017, pp. 1–16. ![Image](images/af9c159a66fab25d9160c0f654179fc52e3dd2430aee361ea4acb021a389ce73.jpg) 袁子康于2020年在中国武汉华中科技大学获得工学学士学位。他目前正于华中科技大学人工智能学院攻读博士学位。他已在ACM MM上发表两篇论文，在TMM上发表一篇论文。他的研究方向包括单目稠密建图、RGB-D同步定位与建图、视觉惯性状态估计以及视觉-LiDAR位姿估计。 ![Image](images/75cf072f930bfd1e12c042e84271763c6992bd56ad55e4cad7e52ad7b15b19a0.jpg) 王庆杰于2020年在中国武汉华中科技大学获得工学学士学位。她目前正于华中科技大学电子信息与通信学院攻读研究生学位。她的研究方向包括视觉-LiDAR里程计、物体重建以及基于深度学习的光流与深度估计。 ![Image](images/3ef4f065c6262bd68375119655af946e24062b782059c9cdb1e058c5c195bc14.jpg) 程肯于2019年在中国武汉华中科技大学获得工学学士学位。他目前正于华中科技大学电子信息与通信学院攻读研究生学位。他的研究方向包括RGB-D同步定位与建图以及视觉-LiDAR位姿估计。 ![Image](images/7c6c6b5333a3a56d03c866ce1c71a0ba5547e44b327efa45a6adb8025d0bb869.jpg) 郝天宇于2018年在中国桂林电子科技大学获得工学学士学位。他目前正于华中科技大学电子信息与通信学院攻读硕士学位。他已在CVPR2021上发表一篇论文，在IEEE Sensors上发表一篇论文。他的研究方向包括视觉同步定位与建图（V-SLAM）、硬件设计以及嵌入式设备开发。 ![Image](images/a15912257903fc0ecf1c7f96b24fa7050ec204408e4af8530f19a70e5ff1d1e5.jpg) 杨欣（IEEE会员）于2013年在加州大学圣巴巴拉分校获得博士学位。她曾于2013年至2014年间在UCSB的基于学习的多媒体实验室担任博士后研究员。她现任华中科技大学电子信息与通信学院教授。她的研究方向包括同步定位与地图构建、增强现实以及医学图像分析。她已发表90余篇技术论文，涵盖《IEEE模式分析与机器智能汇刊》、《国际计算机视觉杂志》、《IEEE医学成像汇刊》、MedIA、CVPR、ECCV、MM等期刊和会议；她还合著了两部著作，并拥有3项美国专利。她是ACM会员。