# 基于点特征和线特征的激光雷达-单目视觉里程计 黄世生 $^{1}$、马泽宇 $^{1}$、穆泰江 $^{1}$、付宏波 $^{2}$、胡世敏 $^{1}$ 摘要—我们提出了一种利用点特征和线特征的新型激光雷达-单目视觉里程计方法。与以往仅基于点的激光雷达-视觉里程计相比，该方法通过将点和线特征引入位姿估计，能够利用更多环境结构信息。我们提供了一种稳健的点和线深度提取方法，并将提取的深度作为点线束调整的先验因子。该方法大幅降低了特征的3D歧义性，从而提高了位姿估计的精度。此外，我们还提供了一种纯视觉的运动跟踪方法和一种新颖的尺度校正方案，最终构建出高精度、高效率的激光雷达-单目视觉里程计系统。在公开的KITTI里程计基准测试上的评估结果表明，我们的技术比现有最先进的方法能实现更精确的位姿估计，有时甚至优于那些利用语义信息的方法。 ## I. 引言 由于其在机器人、虚拟现实和自动驾驶等领域的广泛应用，激光雷达-视觉里程计一直是热门的研究课题。视觉传感器与激光雷达传感器的结合所形成的激光雷达-视觉里程计兼具两类传感器的优势，因此如今在计算机视觉、计算机图形学和机器人领域受到了越来越多的研究关注 [^1]。 近年来，像V-LOAM [^2] 这类紧耦合融合算法在视觉增强型激光雷达里程计方面展现出了出色的性能。这些算法遵循不采用束调整、回环闭合等SLAM技术的里程计框架。后续的DEMO [^3]、LIMO [^4] 和DVL-SLAM [^5]、[^6] 等工作则利用束调整技术来实现更高的运动估计精度。尽管这些方法的前端相机跟踪方式可能有所不同（DEMO [^3] 和LIMO [^4] 采用类似ORB-SLAM2 [^7] 的基于特征的视觉SLAM进行相机跟踪，而DVL-SLAM [^5] 则采用类似DSO [^8] 的基于直接的视觉SLAM进行相机跟踪），但它们都采用基于稀疏点的束调整作为后端，以精确校正相机跟踪结果。然而，这些仅基于点的系统的精度仍不尽如人意，其中一些系统如LIMO [^4] 还需要额外的语义信息作为输入，而获取这些语义信息的计算成本较高。 近期，利用运动恢复结构（[^9]、[^10]）进行3D重建的技术，以及视觉SLAM系统中精确的运动估计技术（[^11]、[^12]）表明：利用现实环境中的更多结构信息（如线特征），能够实现更精确的相机位姿估计。此外，线特征还具有一些突出的优势：它们对噪声（[^13]）、大视角范围（[^14]）和运动模糊（[^15]）等问题不敏感，而这些正是仅基于点的系统（如LIMO [^4]）的主要缺陷。这促使我们将点特征与线特征结合起来，以构建高精度的激光雷达-视觉里程计系统。然而，基于线的视觉SLAM系统仍存在一个未解决的问题：基于线的3D三角测量在相机跟踪过程中较为敏感（[^16]），从而导致视觉SLAM系统不稳定，且位姿估计精度不佳。尽管线特征在各种场景环境（尤其是城市环境中）中广泛存在，但要将线特征直接应用于激光雷达-视觉里程计却并非易事。 在本文中，我们提出了一种鲁棒且高效的激光雷达-单目视觉里程计方法，该方法以纯几何方式结合点特征与线特征，从而能从场景环境中提取比仅基于点的系统更多的结构信息。具体而言，我们的系统在相机跟踪过程中将点特征与线特征作为路标，并将基于点和基于线的路标的重投影误差作为后端捆绑调整的因素。在传感器融合阶段，我们采用了一种鲁棒的方法来从激光雷达数据中提取点和线的深度信息，并利用该深度先验来指导相机跟踪。通过这种方式，我们避免了仅依赖可能具有歧义的3D三角测量来创建3D路标，尤其是对于3D线而言。该深度先验还被作为先验因素纳入点-线捆绑调整中，以进一步提升位姿估计的精度。 此外，为了克服帧间里程计中的尺度漂移问题，我们在捆绑调整前通过尺度校正优化来恢复每个关键帧中的尺度。借此，我们实现了高效且无漂移的激光雷达-视觉里程计系统。得益于丰富的3D结构路标，与在KITTI里程计基准测试（[^3]）中评估的其他纯几何方法（如DEMO [^5]和DVL-SLAM [^17]）相比，我们的方法实现了更高的位姿估计精度；同时，其精度可与LIMO [^4]相媲美，却无需使用任何额外的语义信息。据我们所知，我们是首个以纯几何方式将点特征与线特征相结合的高效激光雷达-单目视觉里程计方法。 ### 二、方法论 预处理。给定单目图像序列和激光雷达序列，我们假设两种传感器的内参和外参均已标定，且两类传感器数据已时间对齐。我们设定 --- ![Image](images/f6d81ddebdfb316a045f9f03bd005795d3c6e31b2db1c1a8a58a162989e6e1f7.jpg)
Fig. 1. The system framework of our approach. Given the input monocular image sequence and lidar sequence, we extract the point and line features for each image, and track image frames using our frame-to-frame odometry with scale corrected by our scale correction optimization. For each keyframe, the depth priors for point and line landmarks are extracted and fed to the point-line bundle adjustment. Loop closure with the point and line features is used for further pose estimation correction. The graph structure for bundle adjustment is illustrated in the dashed box with different factors. The components highlighted in green are our main contribution in this work.
相机的局部坐标系为机体坐标系，世界坐标系则起始于机体坐标系。 概述。图1展示了我们系统的框架，该系统包含三个运行线程：运动跟踪线程（前端）、光束法平差线程（后端）以及回环闭合线程。前端首先提取每一帧中的点和线特征（第II-A节），接着估算每个关键帧中特征的深度先验（第II-B节），最后通过帧间里程计来估算相机位姿（第II-C节）。在帧间里程计之后，会执行尺度校正优化（第II-D节）以修正尺度漂移问题。后端利用点线约束因子进行点线光束法平差（第II-E节）。此外，还会基于词袋模型的回环闭合[^18]技术，结合点和线特征来进一步精修关键帧的位姿（第II-F节）。 ### A. 特征提取 点特征。各种点特征（如SIFT、SURF、ORB等）均可用作跟踪特征。为了追求效率，我们采用定向快速旋转BRIEF（ORB）特征作为点特征，这一做法与ORB-SLAM2中的处理方式一致[^7]。在特征检测过程中，要求ORB特征尽可能均匀分布在图像中。 线特征。对于每张图像，我们采用常用的线特征检测器——线段检测器（LSD）[^19]来检测线段，随后会提取线段带描述符（LBD）[^20]。 如图2（左）所示，LSD算法常常会将一条长线段拆分为多条短线段。此外，有些检测到的线段可能距离非常近，比如图2（右）中薄区域的边界边缘。这类检测到的线段往往会使得后续的线匹配任务变得复杂，进而增加不确定性， ![Image](images/f6a27edd408c98afbd641920c6c362a09907980a940fde49e8818c48cc9bd91c.jpg)
Fig. 2. We chain short line segments into a longer one (Left), or merge near line segments into a new one (Right) to enhance the quality of lines returned by LSD. The images come from KITTI dataset sequence 00.
的三维直线三角化。为了解决这些问题，我们建议通过合并此类"不良"线段来改进LSD算法的结果，如图2所示。更具体地，对于线段对$(l_i, l_j)$，如果$l_i$只有一个端点接近$l_j$的端点，我们将$l_i$和$l_j$连接起来形成一条更长的线段。如果$l_i$的两个端点都接近$l_j$的两个端点，并且$l_i$与$l_j$之间的距离低于给定阈值（在我们的实现中为10像素），我们将$l_i$和$l_j$合并为一条新的线段。新连接或合并后的线段的LBD线描述子也会被更新。 ### B. 点与线的深度提取 在本节中，我们将介绍一种从激光雷达数据中提取点和线深度的方法。此处，二维点特征的深度指的是其对应三维点的深度；而二维线特征的深度则是指其两个端点所对应的三维地标的深度。 --- ![Image](images/b24652476355b1962610ec0d4f6b5a51474712f393294da8f920400968cf870e.jpg)
Fig. 3. A simple illustration for point and line depth extraction. After the sparse lidar data (grey points) is aligned to the image plane, the point depth and line depth are extracted in the point neighbor patch and line neighbor patch separately.
如图3所示，对于每个检测到的二维点或线段，都会分别通过其邻域补丁来估算其深度先验。具体来说，属于点补丁的激光雷达点会先根据Tateno等人提出的方法[^21]被划分为前景点和背景点。随后利用前景点拟合出一个平面，特征点的深度即为该点光线与平面交点处的深度。对于属于图像中线段$P = \{p_i\}$的激光雷达点$l_j$，我们首先通过最小化下述能量函数来估算其对应的三维线$L_j^*$。 $$ L_{j}^{*}=\arg\min_{L_{j}}\sum_{i}d(p_{i},L_{j})+e(L_{j},l_{j}), $$ 其中，$d(p_i, L_j)$ 表示激光雷达点 $p_i$ 与 $L_j$ 之间的欧几里得距离，而 $e(L_j, l_j)$ 则是第II-E节中描述的线重投影误差。在初始化阶段，我们将属于该线段区域的激光雷达点拟合为一条三维直线 $L_j'$，以此作为 $L_j$ 的初始值。估算出 $L_j^*$ 之后，便可利用端点裁剪方法得到其两端点（如图5(左)所示），相关操作详见 [^10]、[^11]。此外，对于位于激光雷达数据视野之外的点和线段，我们采用点三角测量和线段裁剪方法来估算深度先验，具体可参考 [^10]、[^11]。 在运动跟踪过程中，我们采用Plücker坐标 [^9] 来表示三维直线，并以正交归一化表示形式 [^9] 作为最少参数描述方式。该优化问题可通过Levenberg-Marquardt算法 [^22] 高效求解。 ### C. 帧间里程计 我们采用纯视觉的帧间里程计来估算相邻帧的相机位姿，该方法比V-LOAM等基于ICP的里程计更高效 [^2]。对于新帧 $F_i$，我们利用前一帧以及经束调整优化后的三维（点和线）地标来计算其相机位姿 $T_i \in SE(3)$（详见第II-E节）。将 $F_i$ 中检测到的二维点特征（ORB）和线特征（LBD）分别与前一帧 $F_{i-1}$ 中的跟踪特征进行二维-二维点匹配和线匹配。随后提取估算出的三维点地标和三维线地标，通过求解透视-点/线问题来确定 $F_i$ 的相机位姿 [^23]。 ![Image](images/cc9147798bdc01c99075a0cd42dc5d7ce948e72b3b064b6256dce9088bd6caa2.jpg)
Fig. 4. The graph structure for scale correction optimization.
由于描述符距离对二维-二维线匹配的描述能力有限，实际应用中我们会采用多种几何准则来筛选掉“劣质”的线匹配对，以提高匹配精度。具体而言，一个合格的线匹配对 $(l_i, l_j)$ 需满足以下条件：（1）角度差小于给定阈值；（2）长度差也低于给定阈值；（3）$l_i$ 与 $l_j$ 的中点之间的距离应足够小。求解透视-点/线问题后，当前帧 $F_i$ 的相机位姿 $T_i$ 便会被更新。随后将三维点地标和线地标重新投影到 $F_i$ 的图像平面上，以判断三维-二维匹配是否为异常值；一旦检测到异常值便会将其剔除。关于线条投影、端点裁剪和线三角测量的详细内容，请参阅 [^10]、[^11]。 ### D. 尺度校正优化 由于估算出的尺度可能会偏离真实的物理尺度，因此我们提出通过尺度校正优化来修正该偏差。我们的核心思路是融合ICP对齐步骤中计算出的相对相机位姿，从而通过尺度校正优化来调整新估算出的关键帧的相机位姿及相关三维地标。 该问题被构建为基于图的优化问题，如图4所示。对于新选取的、具有估计相机位姿$T_i \in SE(3)$的关键帧，我们构建其相邻关键帧集合$\mathcal{T}_i = \{T_i, T_{i-1}, ..., T_{i-n}\}$、从该关键帧集合$\mathcal{P}_i$中可见的3D点地标集合$\mathcal{L}_i$以及3D线地标集合$\mathcal{T}_i$。我们的目标是调整该关键帧的相机位姿$T_i$、3D点地标$\mathcal{P}_i$以及3D线地标$\mathcal{L}_i$，同时保持其他关键帧的相机位姿$\mathcal{T}_i \setminus \{T_i\}$固定不变。具体来说，这一目标是通过最小化下述误差函数来实现的： $$ E^{i}=e_{i,i-1}^{p o s e}+\sum_{P_{j}\in\mathcal{P}_{i}}\sum_{T_{k}\in\mathcal{T}_{i}}e_{j,k}^{P}+\sum_{L_{j}\in\mathcal{L}_{i}}\sum_{T_{k}\in\mathcal{T}_{i}}e_{j,k}^{L}, $$ 其中，$e^{pose}$、$e^P$和$e^L$分别对应第II-E节所描述的相对位姿因子误差、点重投影因子误差以及线重投影因子误差。利用g2o库[^24]可以高效地求解这一优化问题。 借助我们的尺度校正方法，能够有效减少尺度漂移现象（详见第III节）。为提高效率，耗时的ICP对齐操作仅在关键帧之间执行。需要注意的是，由帧间里程计计算出的相对相机位姿 --- ![Image](images/f8d5f4874fe8e537d06a2c6ccbe8989e20c565693aee8bc0d2e956c5bb41e4eb.jpg)
Fig. 5. A 3D line landmark $L_{w}^{i}$ is re-projected onto the image plane yielding a 2D line $l_{i}$ matched with a line segments $l_{i}^{\prime}$(Left), with $P^{\prime}$ and $Q^{\prime}$ are the priors computed from the depth prior extracted in Section II-B. The error between the re-projected line $l_{i}$ and the matched line segment $l_{i}^{\prime}$ is defined by the distances from its two endpoints to the line $l_{i}$ (Right).
可被用作位姿初始化，从而加快ICP对齐的速度。 ### E. 点线束调整 我们借鉴ORB-SLAM2的方法[^7]，在后端对滑动邻域窗口内的关键帧进行点线束调整。具体而言，当新关键帧到来时，会在其相邻关键帧集合$\mathcal{T}$中执行束调整，涉及3D点地标集合$\mathcal{P}$和3D线地标$\mathcal{L}$。如图1所示，我们将束调整建模为基于图的优化问题，包含以下三个因子，旨在调整关键帧的相机位姿$\mathcal{T}$、3D点地标位置$\mathcal{P}$以及3D线地标位置$\mathcal{L}$。 相对位姿因子。对于一对相邻关键帧，其估计相机位姿分别为$\{T_i, T_j\} \subset \mathcal{T}$，而通过点云ICP对齐步骤得到的相对相机位姿为$T_{i,j}^{icp}$。我们将相对相机位姿$\xi_{i,j}$与$T_{i,j}^{pose} = T_i * T_j^{-1}$之间的差异向量定义为$T_{i,j}^{icp}$：$\xi_{i,j} = \text{Log}(T_{i,j}^{pose} * T_{i,({j}}^{{icp}})^{{}-1}) \in \mathbb{R}^6$，其中$\text{Log}(\cdot)$是$SE(3)$的对数映射[^25]。相对位姿误差的计算公式为$e_{i,j}^{pose} = \xi_{i,j}^T \sum_{i,j}^{pose} \xi_{i,j}$，这里$\sum_{i,j}^{pose}$是一个$6 \times 6$信息矩阵。在某些条件下，ICP对齐会失败，从而导致“不良”的相对相机位姿。为了惩罚这种“不良”的ICP对齐结果，我们会根据ICP对齐质量自适应地计算信息矩阵。设用于设定相对相机位姿$\hat{e}_{i,j}$的ICP误差为$T_{i,j}^{pose}$，对应$\bar{e}_{i,j}$的ICP误差为$T_{i,j}^{icp}$，此时我们计算一个缩放因子$\zeta_{i,j} = e^{(-2.0*(\frac{\bar{e}_{i,j}}{\bar{e}_{i,j}})^2)}$，并令$\sum_{i,j}^{pose} = \zeta_{i,j} I_{6 \times 6}$。 点重投影因子。假设世界坐标系中的一个三维点地物 $P_i \in \mathcal{P}$ 与关键帧 $p_i$ 中的二维点特征 $T_j \in \mathcal{T}$ 相匹配。通过将 $P_i$ 重投影到关键帧的图像平面上，我们可以得到 $P_i$ 的重投影位置 $p'_i = \pi(P_i, T_j, \mathbb{K})$，其中 $\pi(\cdot)$ 表示投影函数，$\mathbb{K}$ 是相机的内参矩阵。接着我们计算重投影误差 $\rho_{i,j} = (p_i - p'_i) \in \mathbb{R}^2$。如果该点特征存在深度先验，我们便按照 ORB-SLAM2 $\rho_{i,j}$ 中的描述，将包含深度先验的情况下的立体重投影误差表示为 [^7]。最终，点重投影误差被定义为 $e_{i,j}^P = \rho_{i,j}^T \Sigma_{i,j}^P \rho_{i,j}$，其中 $\Sigma^P$ 为协方差矩阵。 线重投影因子。世界坐标系中的一条三维线地物 $L^i \in \mathcal{L}$ 也可以被重投影到关键帧 $T_j \in \mathcal{T}$ 上，如第II-A节所述，在二维图像平面上形成线段 $l_i = [l_1^i, l_2^i, l_3^i]^T \in \mathbb{R}^3$。假设一条具有两个端点 $l_i'$ 和 $p_i'$ 的线段 $q_i'$ 与 $L^i$ 相匹配，如图5所示。我们可以把线重投影误差向量表示为：$\gamma_{i,j} = \left[ \frac{l_i^T p_i'}{l_1^2 + l_2^2} \quad \frac{l_i^T q_i'}{\sqrt{l_1^2 + l_2^2}} \right]^T \in \mathbb{R}^2$。如果该线特征存在深度先验，我们就分别计算端点先验 $P', Q'$ 到三维线地物的距离 $d(P')$、$d(Q')$，如图5（左）所示。随后线重投影误差向量被计算为：$\gamma_{i,j} = \left[ \frac{l_i^T p_i'}{\sqrt{l_1^2 + l_2^2}} \quad \frac{l_i^T q_i'}{\sqrt{l_1^2 + l_2^2}} \right]^T \in \mathbb{R}^3$。最后，线重投影误差可计算为 $e_{i,j}^L = \gamma_{i,j}^T \sum_{i,j}^L \gamma_{i,j}$，其中 $\sum^L L$ 为协方差矩阵。 综上所述，基于关键帧的捆绑调整优化旨在最小化下述代价函数。 $$ E=\sum_{\{T_{i},T_{j}\}\subset\mathcal{T}}e_{i,j}^{pose}+\sum_{P_{i}\in\mathcal{P}}\sum_{T_{j}\in\mathcal{T}}e_{i,j}^{P}+\sum_{L_{i}\in\mathcal{L}}\sum_{T_{j}\in\mathcal{T}}e_{i,j}^{L} $$ 在求解非线性优化问题时，为了提升系统的计算效率与数值稳定性，我们采用6维向量 $\boldsymbol{\xi} \in \mathbb{R}^6$ 作为相机位姿 $T(\boldsymbol{\xi}) \in \mathcal{T}$ 的最小参数，用3维向量 $\boldsymbol{\theta} \in \mathbb{R}^3$ 表示点地物 $P(\boldsymbol{\theta}) \in \mathcal{P}$，用4维向量 $\boldsymbol{\phi} \in \mathbb{R}^4$ 表示线地物 $L(\boldsymbol{\phi}) \in \mathcal{L}$ [^10]。我们利用 g2o 库 [^24] 高效地求解这种点线捆绑调整问题。 ### F. 闭环检测 闭环检测涉及运动估计过程中基于关键帧的闭环识别与校正。在闭环识别阶段，我们首先使用 DBoW [^18] 算法分别为点特征（ORB描述符）和线特征（LBD描述符）训练词汇表。随后，每个关键帧 $K_i$ 会被转换为点 BoW 向量 $v_i^P$ 和线 BoW 向量 $v_i^L$。在评估关键帧 $K_i$ 与 $K_j$ 之间的相似性时，我们将相似度得分定义为：$s(i, j) = e^{(1-s_p(v_i^P, v_j^L))^2} e^{(1-s_l(v_i^L, v_j^L))^2}$，其中 $s_p(v_i^P, v_j^P)$ 是点 BoW 向量的相似度得分，$s_l(v_i^L, v_j^L)$ 是线 BoW 向量的相似度得分。一旦为新选定的关键帧 $K_i$ 找到了相似的候选关键帧，便会利用全局捆绑调整算法 [^7] 进行闭环校正。 ### III. 实验 我们的系统基于ORB-SLAM2 [^7]实现，包含三个线程：帧间里程计计算、基于关键帧的束调整以及闭环检测。在进行ICP对齐时，我们采用PCL库 [^26] 中实现的正态分布变换（NDT）[^27] 来计算两个相邻点云之间的相对相机位姿。我们在公开的KITTI基准测试训练数据集上验证了该方法，该数据集包含带有真实轨迹的序列00-10，并在配备Intel Core i7-2600 @ 2.6GHz处理器及8GB内存的64位Linux操作系统计算机上分析了该方法的精度和时间效率。 --- ![Image](images/538883c18d7c8ed9a012df9ace6ff3818220a46c4bdd425551d7593df0a9437a.jpg)
Fig. 6. The motion tracking accuracy comparison between the point-only system (Left) and our system (Right), at the glance of frame 1380 of KITTI dataset sequence 00. The results by the point-only system have obvious drifts, while our results have drift free motion tracking.
![Image](images/267795a861c9f57784723174d4454c784e6a52209c8738ebc2fc871ca07e216a.jpg)
Fig. 7. Our motion tracking trajectories on KITTI training dataset sequences 00 (Left) and 05 (Right), with results by our full system (Ours(*)), the system without depth prior module (Ours(-)) and system without scale correction module (Ours(#)) comparing with the ground truth (GT) trajectory separately.
处理器为Intel Core i7-2600 @ 2.6GHz，内存为8GB，运行在64位Linux操作系统上。 参数设置：对于每张图像，我们提取1000个ORB特征；长度小于50像素的线段会被丢弃。在第II-C节进行线段匹配时，我们将角度阈值设为$2^{\circ}$，长度差异比设为0.1，距离阈值设为5像素。在束调整步骤中，点重投影因子的协方差矩阵被设为$\Sigma^{P} = 0.1 * I$，线段重投影因子的协方差矩阵则设为$\Sigma^{L} = 0.2 * I$。 仅使用点特征与结合点线特征的效果对比：我们首先评估该方法在特征跟踪性能上的表现。我们实现了仅使用点特征的基线版本，并将其与完整系统进行比较。如图6所示，我们的系统能够实现误差更小的运动跟踪，且精度更高，而仅使用点特征的方法则存在明显漂移现象。我们对整个训练数据集进行了定量评估，按照DVL-SLAM [^5]、[^6] 的做法，将每条轨迹按100米、200米……800米分段，汇总各段的平均旋转误差和平移误差。如表I所示，同时使用点和线段特征时，平移误差（0.94%）显著低于仅使用点特征的基线方法（2.16%）。 深度先验与尺度校正模块的评估：我们还通过将其精度与完整系统进行比较，来评估深度先验模块（第II-B节）和尺度校正模块（第II-D节）的性能。如表I所示，我们分别构建了不含深度先验模块的系统（Ours(-)）和不含尺度校正模块的系统（Ours(#)），并与完整系统（Ours(*)）在整体KITTI训练数据集上的平移误差进行对比。对比结果显示，深度先验模块大幅降低了平移误差（从3.64%降至0.94%）；尺度校正模块同样发挥了重要作用，使平移误差减少了约50%（从1.95%降至0.94%）。由此我们可以得出结论：深度先验模块和尺度校正模块（尤其是深度先验模块）能够显著降低我们系统中的位姿估计误差。图7展示了我们的系统在KITTI训练数据集某些序列上采用不同模块时的代表性运动跟踪轨迹，更多详细结果请参阅我们的视频附件。 与现有方法的比较。我们将我们的方法与DEMO [^3]、DVL-SLAM [^5]、[^6]以及LIMO [^4]进行比较，这些方法都拥有与我们类似的优化后端。而像V-LOAM [^2]这样的其他方法仅为视觉里程计系统，没有优化后端，因此未被选为比较对象。需要注意的是，LIMO [^4]需要额外的语义标签信息来识别汽车和行人等移动物体。要获取此类语义标签信息，通常需要诸如Nvidia TitanX Pascal这样的大型计算资源。相比之下，我们的方法、DEMO和DVL-SLAM无需此类语义标签信息，便能在仅搭载CPU的平台上高效运行。因此，我们的评估重点在于对比我们的方法、DEMO和DVL-SLAM。各比较方法的平均平移误差如表I所示。由于LIMO所需的语义标签数据仅对序列00、01、04可用，我们仅展示LIMO在这几个序列上的结果。
TABLE I
TRANSLATION ERROR [%] ON THE WHOLE KITTI TRAINING DATASET
Seq.DEMODVL-SLAMOurs (*)Ours (-)Ours (#)Point-OnlyLIMO
01.050.930.993.234.142.811.12
11.871.471.879.405.033.210.91
20.931.111.384.963.812.10-
30.990.920.651.060.671.67-
41.230.670.420.890.661.780.53
51.040.820.722.940.881.52-
60.960.920.613.280.851.30-
71.161.260.563.530.742.07-
81.241.321.273.711.833.61-
91.170.661.064.411.821.73-
101.140.700.832.630.991.92-
avg1.160.980.943.641.952.160.93
在整个KITTI训练数据集中，我们的平均平移误差为0.94%，旋转误差为0.0036$\frac{deg}{m}$，这一数值低于DEMO（论文中给出的平移误差为1.14%，旋转误差为0.0049$\frac{deg}{m}$）和DVL-SLAM（论文中给出的平移误差为0.98%，旋转误差约为0.0040$\frac{deg}{m}$）。根据原始的LIMO论文 [^4]记载，他们报告的平均平移误差为0.93%，旋转误差为0.0026$\frac{deg}{m}$。尽管我们的平移误差略高于LIMO，但如表I所示，我们在序列00和04上的平移误差更低。这表明 --- ![Image](images/f0398a9f79bdb95ea366c08362853ea09a7ab88c1b57ab866d117c300e049f69.jpg) ![Image](images/241fd8c03cac81b170aeb2ec3f21130da66ef74a03f867c41eeda796520ee755.jpg) ![Image](images/94c41dd78bc22d2ef11f320fc434f7eb01890098ddcfbd0660a586ed9ec6e5e0.jpg)
Fig. 8. The error analysis performed on the KITTI training dataset between DEMO, DVL-SLAM, LIMO and Ours. Note that LIMO only provided error curves according to vehicle speed (Right), so we only collect DEMO, DVL-SLAM and Ours error curves in the path length analysis (Left).
![Image](images/a38a026332192c56f699f7bb46d080f6130056a0e84bb4bec9787df24ed80d80.jpg) 作为一种纯几何方法，我们的方法能够达到与LIMO相当（有时甚至更优）的精度，而LIMO却需要额外的语义标签信息。 我们参照DVL-SLAM的做法，根据路径长度和车辆速度进行误差分析，DEMO、DVL-SLAM、LIMO以及我们的方法之间的对比曲线如图8所示。当车辆速度较快时，我们系统的精度会较低。这解释了为何我们的方法在序列01上的表现相对较差，该序列属于车辆速度极快的极具挑战性的场景。关于我们的方法在整个KITTI训练数据集上的更多详细结果，请参阅视频附件。 在NuScenes数据集上的评估。我们在NuScenes数据集 [^28]上评估我们的方法，该数据集是一个包含大量结构线特征的城市环境公开大规模数据集。为了在不考虑回环闭合影响的情况下评估我们的激光雷达-视觉里程计精度，我们使用了包含80个测试场景的NuScenes Part I子集数据集，每个场景仅有单一路径轨迹，不存在回环路径。由于每个测试场景的轨迹长度不长（< 200米），我们参照DVL-SLAM的做法将轨迹分段来计算平均旋转误差和平移误差，分段长度分别设为5米、10米、50米、100米、150米和200米。总体而言，我们的平均平移误差为2.4%，旋转误差为0.0007 $\frac{deg}{m}$。图9展示了NuScenes Part I数据集中两个示例测试场景的点-线特征检测中间结果（左）以及利用估计的相机位姿生成的点云重建结果（右）。
Fig. 9. The test results of scene0007(top row) and scene0024(bottom row) in NuScenes Part I dataset using our full system, including the point-line detection results (left) and final point cloud reconstruction results using the estimated camera poses (right).
讨论与局限性。在本文中，我们并未将点-线回环闭合视为主要贡献，因为 时间与复杂度。特征提取大约需要80毫秒，尺度校正优化则需约30毫秒。最耗时的部分在于ICP对齐步骤，每次耗时约300毫秒。由于ICP对齐仅在关键帧之间进行，且每3-4帧选取一个关键帧，因此我们的系统在仅使用CPU的平台上的平均处理速率约为5Hz，这一数值与依赖GPU平台的LIMO系统相当。 这种基于词袋模型的回环闭合技术通常用于DEMO、DVL-SLAM和LIMO等系统中。我们还可以将车辆里程计传感器数据融入系统：把车辆里程计测得的相对位姿作为点线束调整中的因子，从而构建更精确的激光雷达-视觉里程计系统。不过，我们目前的重点是将更多结构先验信息融入激光雷达-视觉SLAM解决方案，这部分内容将留待后续研究。此外，我们的系统尚未考虑校准参数误差以及激光雷达与摄像头传感器近似同步所带来的问题，这也是未来开发鲁棒性更强的激光雷达-摄像头SLAM系统时值得探讨的有趣课题。我们方法的缺点之一在于跟踪鲁棒性：当摄像头进行大幅旋转或高速运动时，跟踪系统可能会失效，进而导致运动估计精度下降。另一个缺点是面对运动物体时的处理能力有限，这超出了我们这类纯几何方法的能力范围。未来我们希望引入语义信息，以实现更精确的激光雷达-视觉里程计方案。 ### 四、结论 在本文中，我们提出了一种利用点和线特征的精确且高效的激光雷达-单目视觉里程计方法。通过利用更多的结构信息，我们证明该方法比现有的纯几何技术更精确，其精度可与采用额外语义信息的LIMO等系统相媲美。我们希望我们的工作能够启发后续研究，探索其他类型的结构先验信息，例如平面先验、平行关系、正交关系或共面规则，以构建更精确的激光雷达-摄像头传感器融合系统。 ### 五、致谢 感谢匿名审稿人对本文提出的详细意见。本研究得到了国家自然科学基金（项目编号：61521002、61863031、61902210）和中国博士后科学基金（项目编号：2019M660646）的支持。 --- [^1]: S. Yang, P $^{1}$ and S.-M. Hu, "H $^{1}$-sensors," [^2]: J. Zhang and S. Singh, “Visual-lidar odometry and mapping: low-drift, robust, and fast,” in IEEE International Conference on Robotics and Automation (ICRA), 2015, pp. 2174–2181. [^3]: 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. [^4]: J. Gräter, A. Wilczynski, and M. Lauer, “LIMO: lidar-monocular visual odometry,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018, pp. 7872–7879. [^5]: Y.-S. Shin, Y. S. Park, and A. Kim, “Dvl-slam: sparse depth enhanced direct visual-lidar slam,” Autonomous Robots, 2019. [^6]: Y. Shin, Y. S. Park, and A. Kim, "Direct visual SLAM using sparse depth for camera-lidar system," in 2018 IEEE International Conference on Robotics and Automation (ICRA), 2018, pp. 1–8. [^7]: R. Mur-Artal and J. D. Tardós, “ORB-SLAM2: an open-source SLAM system for monocular, stereo, and RGB-D cameras,” IEEE Trans. Robotics (TRO), vol. 33, no. 5, pp. 1255–1262, 2017. [^8]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Trans. Pattern Anal. Mach. Intell. (PAMI), vol. 40, no. 3, pp. 611–625, 2018. [^9]: A. Bartoli and P. F. Sturm, “The 3d line motion matrix and alignment of line reconstructions,” International Journal of Computer Vision (IJCV), vol. 57, no. 3, pp. 159–178, 2004. [^10]: A. Bartoli and P. Sturm, “Structure-from-motion using lines: Representation, triangulation, and bundle adjustment,” Computer Vision and Image Understanding (CVIU), vol. 100, no. 3, pp. 416–441, 2005. [^11]: G. Zhang, J. H. Lee, J. Lim, and I. H. Suh, “Building a 3-d line-based map using stereo SLAM,” IEEE Trans. Robotics (TRO), vol. 31, no. 6, pp. 1364–1377, 2015. [^12]: X. Zuo, X. Xie, Y. Liu, and G. Huang, “Robust visual SLAM with point and line features,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2017, pp. 1775–1782. [^13]: P. Smith, I. D. Reid, and A. J. Davison, “Real-time monocular SLAM with straight lines,” in Proceedings of the British Machine Vision Conference 2006 (BMVC), 2006, pp. 17–26. [^14]: A. P. Gee and W. W. Mayol-Cuevas, “Real-time model-based SLAM using line segments,” in Advances in Visual Computing, Second International Symposium, 2006, pp. 354–363. [^15]: G. Klein and D. W. Murray, “Improving the agility of keyframe-based SLAM,” in ECCV, 2008, pp. 802–815. [^16]: Y. Zhao and P. A. Vela, “Good line cutting: Towards accurate pose tracking of line-assisted VO/VSLAM,” in ECCV, 2018, pp. 527–543. [^17]: A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving? the kitti vision benchmark suite,” in Conference on Computer Vision and Pattern Recognition (CVPR), 2012. [^18]: D. Gálvez-López and J. D. Tardós, “Bags of binary words for fast place recognition in image sequences,” IEEE Transactions on Robotics (TRO), vol. 28, no. 5, pp. 1188–1197, October 2012. [^19]: R. G. von Gioi, J. Jakubowicz, J. Morel, and G. Randall, "LSD: A fast line segment detector with a false detection control," IEEE Trans. Pattern Anal. Mach. Intell. (PAMI), vol. 32, no. 4, pp. 722–732, 2010. [^20]: L. Zhang and R. Koch, “An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency,” J. Visual Communication and Image Representation, vol. 24, no. 7, pp. 794–805, 2013. [^21]: K. Tateno, F. Tombari, and N. Navab, “Real-time and scalable incremental segmentation on dense SLAM,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2015, pp. 4465–4472. [^22]: W. M. Häußler, “A local convergence analysis for the gauss-newton and levenberg-morrison-marquardt algorithms,” Computing, vol. 31, no. 3, pp. 231–244, 1983. [^23]: A. Vakhitov, J. Funke, and F. Moreno-Noguer, “Accurate and linear time pose estimation from points and lines,” in ECCV, 2016, pp. 583–599. [^24]: R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “G²o: A general framework for graph optimization,” in IEEE International Conference on Robotics and Automation (ICRA), 2011, pp. 3607–3613. [^25]: H. Strasdat, “Local accuracy and global consistency for efficient SLAM,” Ph.D. dissertation, Imperial College London, UK, 2012. [^26]: P. Biber and W. Straßer, “The normal distributions transform: a new approach to laser scan matching,” in 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2003, pp. 2743–2748. [^27]: P. org., “Pcl library,” . [^28]: H. Caesar, V. Bankiti, A. H. Lang, S. Vora, V. E. Liong, Q. Xu, A. Krishnan, Y. Pan, G. Baldan, and O. Beijbom, “nuscenes: A multimodal dataset for autonomous driving,” arXiv preprint arXiv:1903.11027, 2019.