基于先验LiDAR点云地图的单目VIO定位（IROS 2020）

作者：杨凌昊
首发：公众号【3D视觉工坊】
注1：文末附有【视觉SLAM、激光SLAM】交流群加入方式
注2：自动驾驶系统课程：https://mp.weixin.qq.com/s/0-mb6M6HvKU3Uau0a4taTw
注3：彻底搞懂视觉-惯性SLAM：基于VINS-Fusion
注4：彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
注5：彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM+LIO-SAM)
注6：激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解

Yu H , Zhen W , Yang W , et al. Monocular Camera Localization in Prior LiDAR Maps with 2D-3D Line Correspondences[J]. 2020. IROS 2020

武汉大学、卡内基梅隆大学

针对问题：

如何使用先验LiDAR地图约束单目VO位姿漂移？

提出方法：

首先使用LiDAR构建先验LiDAR点云地图并离线提取点云地图中的三维直线信息；

通过将场景中的三维线段投影回图像平面与图像中检测出的二维线段进行匹配，最小化线段之间的距离来实现对当前相机位姿的约束效果。

达到效果：

在Euroc室内数据集和通过FARO scanner focus3D构建的数据集上进行了测试，显著抑制了VIO系统的位姿误差累计问题。

存在问题：

方法主要适用于存在较多结构化信息的场景；

方法针对静态场景设计，对于动态场景中结构变化大不能有效应对。

Abstract

在先验地图中实现轻量级的相机定位对于基于视觉的导航至关重要。目前，VO和VIO技术在状态估计方面已经发展得很好，但在闭环时不可避免地存在位姿累积漂移和跳动的问题。为了克服这些问题，我们提出了一种高效的单目相机定位方法，在先验LiDAR地图中直接使用2D-3D线段匹配构建数据关联，由此构建先验LiDAR地图提供的绝对约束。为了处理LiDAR地图之间的外观差异以及LiDAR点云和图像不同模态信息之间的信息差异，几何三维线从先验LiDAR地图中离线提取，而稳健的二维线则从视频序列中在线提取。通过VIO的位姿估计信息，我们可以有效地获得粗略的2D-3D线段的对应关系。然后通过对匹配上的线段最小化重投影误差对当前相机位姿进行不断地迭代优化，对没有匹配上的外点进行剔除。在EuRoC MAV数据集和我们采集的数据集上的实验结果表明，所提出的方法能够有效地在结构化环境中估计相机的位姿，而不会出现累积漂移或位姿跳跃情况。

Main Contribution

这项工作的主要贡献是利用2D-3D线段在几何上的对应关系实现了单目相机在先验地图中的定位。它有效地将每个关键帧与先验LiDAR地图进行数据关联，并且几何线的对应关系对外观变化是稳健且对外观变化具有鲁棒性，适用于城市环境中的相机定位。

Proposed Method

在这项工作中，我们提出了一种在先验三维LiDAR地图中直接使用2D-3D几何线对应的关系的实时轻量级单目相机定位的方法。地图中，使用直接的2D-3D几何线对应进行实时的轻量级单目相机定位。我们假设给定一个粗略的姿态初始化，并将重点放在地图中的姿态跟踪上，着重于地图中的姿态跟踪。在几何线段的提取方法上，三维线段通过从LiDAR地图中离线提取，而稳健的二维线段则是从视频序列中在线提取。通过采用来自VIO系统的6-DOF位姿估计预测，在视场（FoV）中的局部可见3D线被提取出来，并直接与2D线段特征相匹配以获得粗略的2D-3D点断对应。最后对相机姿态迭代优化，使关联上的2D-3D线段投影误差最小化实现对初始位姿的精优化结果。

1.2D 3D线段提取

3D线段：

本文使用Lu X , Liu Y , Li K . Fast 3D Line Segment Detection From Unorganized Point Cloud[J]. 2019 (https://github.com/xiaohulugo/3DLineDetection) 该3D直线检测方法在先验LiDAR点云地图中进行3D直线提取。该方法的思想是将点云集中到点云聚为平面区域，并使用等高线拟合来获得三维线段。这种方法对于大规模的无组织的点云来说是高效且稳健的。虽然它需要时间来处理数以百万计的点云，但所有地图的三维线段在我们开始追踪之前只需提取一次。

2D线段：

本文使用Xue N, Bai S, Wang F, et al. Learning attraction field representation for robust line segment detection[C]//Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2019 (https://github.com/cherubicXN/afm_cvpr2019)该方法使用AFM将LSD问题转变成一个区域着色问题，能够有效地应对场景中的纹理信息噪声带来的外点影响。

2.2D-3D线段匹配

局部3D线段筛选：

对于当前帧图像输入，获取2D-3D直线关联主要包含以下几个步骤：初始相机位姿估计(由VIO获取)；当前帧可视3D直线获取；各个2D-3D线对应关系估计。在提取FoV下的3D线段上，论文中保留了FoV中所有的3D直线，而不丢弃被遮挡的线条。

1）如果两个端点都在FoV内,我们就保留整个三维线段作为局部可见特征

2）当只有一个端点在FoV中时，我们迭代地从三维线上的新的三维点中抽取新的三维点，以线长的0.1比例抽出，并检查新采样点的可见性。所生成的具有最长FoV长度的3D线段被存储为本地可见特征

3）当两个端点都不在FoV范围内，但有一个子集在FoV范围内时的时候，我们也可以通过采样点来提取一个子集。然而,大部分不可见的地图线在这种情况下为了提高效率，我们放弃了有两个端点在FoV之外的所有3D线段。

这里分别给出各个距离度量量的计算方法：

至此，我们可以得到2D-3D线段之间的关联关系，可以将其用于后续的位姿优化中。

3.Pose optimization and correspondence refinement

对于每一帧，相机的位姿可以通过以下方式进行优化：使两个三维端点投影的点到最终线的距离最小化；端点投影与相应的二维线距离的最小化。由此得到最小化所有2D-3D对应关系之间的投影误差的目标函数为：

这里Li表示两个端点，K表示内参矩阵，M表示找到的关联线段的数量，ξ表示相机位姿的李代数形式，H表示2D线段的系数表示，它被表述为一个非线性最小二乘法问题，通过VINS-Mono提供的初始位姿进行迭代优化，得到先验地图约束优化后的相机位姿。然而，单帧的2D-3D对应观测对于在线相机定位来说是不够鲁棒的。当FoV中的三维线条在三维空间中受到限制或相互平行时，2D-3D对应关系不能约束6DoF姿态。此外，即使对应关系足以进行姿势估计，二维和三维线条的几何定位噪声也会使估算值在真实姿态周围出现抖动。为了解决这些问题，我们利用滑动窗口来增加更多以前的对应的观测值来优化当前的位姿

其中N是滑动窗口中以前的帧数。因此，更多的观测值使估计对离群值更加稳健，而运动约束则能够平滑轨迹。当使用滑动窗口时，由于二维直线的数量不同，会出现不平衡的问题。问题会出现，因为每一帧的2D-3D对应的数量不同。为了平衡每一帧对应关系的贡献，提高工作效率，我们设定了一个最大对应关系数的阈值，以舍弃重叠距离较短的2D-3D对应关系。

Experimental Results

本文在EuRoC MAV数据集和通过FARO scanner focus3D S激光雷达扫描仪采集的真实数据中进行了实验验证。

TABLE I: ATE RMSE

很明显，与仅有里程计的系统相比，2D-3D对应关系提高了位姿估计的准确性。V2房间有更多的噪音使得2D-3D对应关系在姿势优化方面有时不稳定。这就是为什么V2房间的改进不如 V2房间的改进不如V1房间的结果显著。最坏的情况是，没有稳定的2D-3D对应，最终的估计结果为仅有里程计时的轨迹。此外，我们的方法显示了与VINS-Mono(Loop)相比，我们的方法显示了有竞争力的结果。虽然闭环能够优化回环中过去的位姿，并为当前的姿势产生一个位姿跳转。但对于实时定位的目的来说，优化过去位姿是没有意义的且位姿跳转对导航有副作用。我们的方法总是在滑动窗口中估计当前的位姿，这大大减少了漂移也没有位姿跳动的问题。

TABLE II: RPE RMSE

平均相对姿势误差显示在上表和上图中，用于显示位置误差随轨迹长度的增长，位置误差随着轨迹长度的增加而增加。VINS-mono(odom)的累积漂移随着行程长度的增加而增加。相比之下，我们的方法的误差保持较小且稳定，这与二维和三维线路定位的准确性有关。

对于VINS-Mono(odm)来说，累计误差漂移比较严重。而我们的方法在稳定的2D-3D线段约束的帮助下，大大提高了定位精度。在整个轨迹上，定位误差保持较小。

Conclusion

在本文中，我们提出了一种新颖的单目相机定位方法，在先验的结构化环境的LiDAR地图中。利用LiDAR地图中的三维几何线段和强大的在线二维线条检测，我们的方法在VINS-Mono的相机运动预测的基础上有效地获得了粗略的2D-3D线段关联。使用2D-3D线段关系的位姿优化大大减少了VIO系统的位姿估计漂移，而无需使用视觉闭环检测。真实世界数据集的定性和定量结果表明，我们的方法可以有效地获得可靠的2D-3D对应关系和LiDAR地图中准确的相机位姿。作为未来的工作，我们打算加强2D-3D对应关系对不准确的位姿预测的鲁棒性性，例如直接使用最后一帧的位姿作为预测。

欢迎加入【3D视觉工坊】交流群，方向涉及3D视觉、计算机视觉、深度学习、vSLAM、激光SLAM、立体视觉、自动驾驶、点云处理、三维重建、多视图几何、结构光、多传感器融合、VR/AR、学术交流、求职交流等。工坊致力于干货输出，为3D领域贡献自己的力量！欢迎大家一起交流成长~ 欢迎大家投稿，有稿费哦。

添加小助手微信：CV_LAB，备注学校/公司+姓名+研究方向即可加入工坊一起学习进步。