Monocular Camera Localization in Prior LiDAR Maps with 2D-3DLine Correspondences

本文配置下：25FPS，线检测；总的13-15FPS，实时性用于XR还是实时性不够好。用于室内的慢速机器人应该够了。

摘要

在现有地图中进行轻量级相机定位对于基于视觉的导航至关重要。目前，视觉和视觉惯性测距（VO&VIO）技术在状态估计方面已经很成熟，但在闭环时会产生不可避免的漂移和姿态跳变。为了克服这些问题，我们提出了一种利用直接的2D-3D线对应关系在先前的LiDAR地图中进行高效的单目相机定位方法。为了处理LiDAR点云和图像之间的外观差异和模态差距，我们离线从LiDAR地图中提取了几何3D线，同时在线从视频序列中提取了稳健的2D线。利用VIO的姿态预测，我们可以高效地获得粗略的2D-3D线对应关系。然后，通过最小化对应关系的投影误差并拒绝异常值，对相机姿态和2D-3D对应关系进行迭代优化。在EurocMav数据集和我们收集的数据集上的实验结果表明，所提出的方法可以在结构化环境中高效地估计相机姿态，而不会积累漂移或姿态跳变。

I. 引言

在城市环境中准确的机器人定位对于自动驾驶汽车至关重要。由于GPS定位在没有与卫星的直接视线接触时不稳定，因此通常使用基于LiDAR的定位模块，因为它具有准确的距离测量。此外，城市场景具有相对不变的几何结构（例如建筑物），因此可以使用一次性的3D地图构建来进行长期定位。然而，LiDAR传感器的昂贵成本和沉重重量限制了其广泛应用。相机和IMU是低成本、轻量级和普遍可用的传感器，当前的基于视觉惯性的姿态估计和映射方法已经针对各种机器人系统进行了充分的发展。然而，仅使用图像特征的状态估计方法容易因环境中的光照或纹理变化而失败。因此，如果相机定位模块可以与先前的3D地图关联起来，即将视觉信息与距离测量融合，就有很大的潜力可以使用这些轻量级和小型的相机模块在城市环境中实现准确的定位，而无需LiDAR传感器。

然而，由于外观差异和模态差距，图像数据与3D点云的融合具有挑战性。目前的方法通常将3D数据转换为2D空间或从2D图像重建3D点云，以对齐数据进行姿态估计。基于城市环境的特点，我们的直觉在于，主要的几何结构，如线条和平面，无论外观差异和模态差距如何，都可以在3D地图和2D图像中都被捕获到。直接的2D-3D几何共现对应关系比领域转换数据的关联更为稳健和精确。因此，我们的目的是直接估计2D-3D几何线对应关系，以实现准确且长期的相机定位。

在这项工作中，我们提出了一种实时轻量级单目相机在先前的3D LiDAR地图中使用直接2D-3D几何线对应关系进行定位的方法。我们假设给定了一个粗略的姿态初始化，并侧重于地图中的姿态跟踪，这遵循了相关工作的方法。为了进行几何并发特征提取，我们离线从LiDAR地图中检测3D线段，同时在线从视频序列中提取稳健的2D线段。通过利用VIO的六自由度姿态预测，可见区域内的局部可见3D线段被提取出来，并直接与2D线特征匹配以获得粗略的2D-3D线对应关系。最后，通过最小化对应关系的投影误差和拒绝异常值，对相机姿态和2D-3D匹配进行迭代优化。

这项工作的主要贡献在于估计几何2D-3D线对应关系以进行相机定位，从而将每个关键帧与先前的LiDAR地图关联起来。几何线对应关系对外观变化具有鲁棒性，并且适用于城市环境中的相机定位。图1显示了LiDAR地图中相机图像与2D-3D线对应关系以及估计的相机姿态。

II. 相关工作

地图中基于视觉的定位旨在建立2D和3D模态之间的对应关系，以提高定位的鲁棒性和准确性。为了克服模态差距和外观差异，通常采用的方法是使用“中间产品”将匹配转换为相同的空间，即在2D空间中[9]或在3D空间中[6]，[10]。

第一种相机在3D地图中的定位方法是在2D图像空间中进行光度匹配。对于大多数视觉同时定位与地图构建（SLAM）方法[1]，[2]，使用光度（或视觉）描述符重建稀疏点云地图，然后通过将这些描述符进行匹配以减少漂移来进行闭环检测。在基于图像的定位方法中[9]，[4]，使用视觉特征使用SFM进行大规模地图重建，这些特征被高效地匹配以产生2D-3D对应关系以进行相机定位。然而，外观和视觉特征对照明变化和光照条件敏感，这使得对长期相机定位而言，对应关系不稳定。此外，为了处理没有关联视觉特征的LiDAR地图，通常使用LiDAR外观合成图像通过归一化互信息（NMI）[11]或归一化信息距离（NID）[12]直接匹配相机图像。此外，最近的方法利用着色点云投影[13]和合成的深度图像[14]与实时图像匹配进行相机定位。这些定位方法都试图将2D-3D匹配问题转化为2D-2D匹配问题。

第二种策略是在3D空间中匹配几何来估计相机姿态。通过使用局部BA，[6]提出了将稀疏重建的局部3D点云与给定的3D LiDAR地图进行匹配，从而解决了单目VO系统的尺度估计问题，并实现了6-DoF相机姿态的在线估计。类似地，在[15]中，使用3D结构描述符将LiDAR地图与稀疏视觉重建的点云进行匹配。在[10]，[7]中，从立体相机重建出密集的局部点云以与LiDAR地图匹配，然后将匹配结果松散或紧密地耦合到VO和VIO系统中，以优化相机姿态。这些通过3D配准的定位方法与仅基于视觉的方法相比获得了可行的结果。然而，定位的准确性高度依赖于局部重建的性能。SFM存在尺度问题，重建的稀疏点可能在地图中没有对应点。立体重建提供了密集的局部点云，但多数情况下耗时且不适合长距离深度估计。

与局部点云匹配方法相比，我们的目标是直接从单目相机中提取2D线条，与LiDAR地图中的3D线条进行匹配以进行相机定位，不依赖于SFM或立体重建模块。全局的2D-3D定位被称为被绑架的机器人问题，因为特征描述之间存在差异且非凸[16]。RANSAC和分支定界策略通常用于最大化全局定位的2D-3D内点对应关系，而不需要姿态先验[16]。通过GPS或VO（VIO）提供的相机姿态预测，我们可以获得用于局部2D-3D匹配的姿态预测。SoftPosit [17]和BlindPnP [18]是提供了姿态初始化的传统的同时2D-3D对应关系和姿态估计方法。对于相机与LiDAR传感器的校准方法[19]，[20]，通常通过匹配图像和点云之间的几何特征来估计两个传感器之间的外部转换。最近，一些基于学习的方法提出了直接将图像注册到LiDAR地图的方法，并通过GPS姿态初始化[21]，[22]。这些方法要么计算复杂度高，要么输出不稳定，仍然需要探索用于在3D LiDAR地图中进行实时相机定位的方法。

III. 提出的方法

提出的方法在LiDAR地图中同时估计6自由度相机姿态和2D-3D线对应关系。这些对应关系被用来通过最小化3D线投影误差来优化相机姿态，而精确的相机姿态则有助于拒绝异常对应关系。作为在线2D-3D对应关系估计的初步步骤，从大规模3D LiDAR地图中离线提取3D线特征。与此同时，通过PnP求解器[23]对手动标记的2D-3D点对应关系为第一帧提供粗略的姿态初始化。然后利用VINS-Mono [2]来预测相邻关键帧之间的相机运动。利用预测的姿态，在相机视野（FoV）内提取局部3D线，并直接与从图像序列中在线提取的2D线进行匹配。最后，相机姿态和2D-3D对应关系进行迭代更新。流程如图2所示。

3.1  2D和3D线提取

在城市环境中，几何结构通常由线段和平面表示。我们使用基于分割的3D线检测方法[24]从LiDAR地图中提取3D线。其一般思想是将点云聚类成平面区域，并使用轮廓线拟合来获得3D线段。这种方法对大规模的非组织点云具有高效和稳健性。尽管处理数百万个点需要时间，但所有地图的3D线只在跟踪开始前提取一次。

对于2D线提取，我们希望提取与3D线一致且对噪声具有鲁棒性的主要几何2D线。在城市场景中，这是具有挑战性的，因为大量的纹理噪声会导致片段化的2D线段，并且一些几何边缘在彩色均匀结构（例如白墙）上在2D图像中是不可见的。 计算机视觉领域已经提出了许多最先进的线段检测（LSD）方法[25]，[26]，传统的手工方法对于在CPU上在线运行非常有效。然而，检测到的线段是片段化和嘈杂的，如图3(a)所示的示例。这些片段化和嘈杂的特征可能会产生大量的2D-3D匹配异常值。考虑到线段的完整性和对噪声的鲁棒性，我们最终采用了基于学习的LSD方法[26]，该方法使用吸引场地图（AFM）将LSD问题转换为区域着色问题。对于图像中的每个像素p，模型首先从像素到最近线段上的最近点p0学习一个2D向量a(p)。

其中p = (x, y) 是像素p的坐标。然后，通过将像素编码到线段关联来生成AFM {a(p), p ∈ I}。在训练步骤中，将地面真值的2D线段转换为AFM表示，然后网络直接学习模型以使输出与真实AFM之间的相似性最小化。对于测试，首先从图像中生成AFM表示，然后使用挤压模块[25]迭代地将属于同一线的v(p) = p + a(p)分组以拟合线段。图3(b)显示了示例图像的检测到的线段。它与几何3D结构显示出良好的一致性，并且对纹理噪声具有鲁棒性。

3.2 2D-3D line matching

这个过程描述了单帧中进行2D-3D线匹配的主要步骤：

1. 初始相机姿态预测（VINS-Mono）：使用VINS-Mono，一个视觉惯性测距系统，获得当前帧的姿态估计。

2. 可见3D线收集：提取视野范围内的局部3D线，以提高效率。这一步骤通过专注于有限的一组3D线，而不是考虑整个3D地图中的所有3D线，来减少计算负载。在视野范围内保留所有的3D线，包括可能被遮挡的线。

3. 单个2D-3D线对应关系估计：估计图像中的2D线与场景中的3D线之间的对应关系。这涉及将图像特征与场景中的3D线进行匹配。

4. 使用2D-3D对应关系更新姿态：使用估计的2D-3D对应关系更新姿态。基于上一帧的预测姿态和从VINS-Mono获得的相机运动T，计算更新的姿态。

5. 3D线的可见性检查：为了确定3D点（P）是否在视野范围内，使用估计的姿态（P^t​）和相机内参数矩阵（K），计算其在图像平面上的投影点（pt​）。如果 �>0w>0 并且投影点位于图像尺寸内，则将该3D点视为在视野范围内可见。

                pt​=⎝⎛​xyw​⎠⎞​=K⋅P^t​⋅P

        使用预测的相机姿态 P^t​，将图像中的2D线 {lt​=(ps​,pe​)} 直接与视野范围内的局部3D线进行匹配，其中ps​ 和pe​ 分别是2D线的起点和终点。对于每一个可能的2D-3D对应关系，我们使用一个3D向量来衡量相似度，包括2D角度距离θ，两个3D端点在对应无限2D线上的投影距离 d，以及有限2D线与3D线投影的重叠长度lenoverlap​。               

 将3D线 L=(Ps​,Pe​) 在图像平面上的投影lu​=(psu​,peu​) （公式3），与检测到的2D线 lt​ 进行匹配。2D线的标准化方向记为v=(pe​−ps​)/∣∣pe​−ps​∣∣。然后，可以通过以下方式计算2D角度距离 θ：

 

 

 3.3 Pose optimization and correspondence refinement

对于单帧，相机姿态可以通过最小化两个3D端点投影到相应的2D线距离的点到无限线距离来优化。估计的相机姿态 Pt​ 的李代数表示为 ξt​。无限2D线的系数向量可以表示为 H=[A,B,C]T。目标函数是最小化所有2D-3D对应关系之间的投影误差：

 

其中，Li​ 包含两个端点，M 表示对应关系的数量。这被构建成一个非线性最小二乘问题。我们将相机姿态初始化为从VINS-Mono得到的预测姿态 P^t​。利用李代数，经典的L-M算法可以找到最优的相机姿态 Pt​。然而，单帧2D-3D对应观测并不足以保证在线相机定位的稳健性。当视野范围内的3D线有限或者在3D空间中相互平行时，2D-3D对应关系无法约束6自由度姿态。此外，即使对于姿态估计来说，对应关系足够多，2D和3D线的几何定位噪声也会使估计值在真实姿态周围抖动。为了解决这些问题，我们使用一个滑动窗口来添加更多先前的对应观测以优化当前的姿态 Pt​。假设从VINS-Mono估计的相邻关键帧的相机运动是准确的，这是合理的，因为VINS-Mono在相邻关键帧上的漂移很小。T1t​,T2t​,⋅⋅⋅,Tnt​ 被表示为相邻关键帧之间的相机运动（包括旋转矩阵 �R 和平移向量 t 的4x4矩阵）。先前第 n 个关键帧的相机姿态 Pt−n​ 可以通过当前的估计 Pt​ 和相机运动得到。然后，滑动窗口中的所有先前的2D-3D对应关系可以在当前帧中“可见”。在方程9中，先前的相机姿态 Pt−n​ 与当前姿态 Pt​ 相关联，而(T1t​T2t​⋅⋅⋅Tnt​)−1 是一个常数姿态变换，其李代数表示为 Δξnt​。因此，姿态优化函数为：

其中，N 是滑动窗口中先前帧的数量。因此，更多的观测使估计对异常值更加稳健，而运动约束使姿态轨迹更加平滑。在使用滑动窗口时，由于每帧的2D-3D对应关系数量不同，会出现不平衡的问题。为了均衡每帧对应关系的贡献并提高效率，设置了最大对应关系数量的阈值，以丢弃具有较短重叠距离 lenoverlap​ 的2D-3D对应关系。我们使用Ceres Solver [27] 实现优化。在获得优化的相机姿态后，可以通过更新的姿态重新估计2D-3D对应关系以排除异常值。然后，根据更严格的阈值（例如，0=0.8×θ0​=0.8×θ ， 0=0.8×d0​=0.8×d0​ ），按照第III-B节中的2D-3D线匹配重新估计2D-3D对应关系。然后，可以使用新的对应关系更新更准确的相机姿态。经过几次迭代，可以优化相机姿态和2D-3D对应关系。