Visual-lidar Odometry and Mapping: Low-drift, Robust, and Fast

摘要
这里我们提供一个视觉雷达里程计的通用框架。这种方法表明它可以改善性能，提高鲁棒性。通过视觉估计自身位姿来修正高频点云的畸变，然后，基于扫描匹配的激光雷达测距仪同时改进运动估计和点云配准。

1.简介

最近在视觉测距法和激光雷达测距法中的独立结果是有前景的，因为它们可以提供6自由度状态估计，测绘甚至障碍物检测的解决方案。 然而，仅使用每个传感器存在缺点。 视觉测距方法需要适度的照明条件，并且如果清晰的视觉特征不充分可用则失败。 另一方面，通过移动激光雷达的运动估计涉及点云中的运动失真，因为在连续激光雷达运动期间在不同时间接收到距离测量。 因此，运动常常需要用大量的变量来解决。 在诸如由平面区域支配的退化场景中，扫描匹配也失败。
在这里，我们提出了一个方法来组合单目相机和3D激光雷达的自我运动估计。 我们希望准确估计六自由度运动以及环境的空间度量表示，实时并在机器人在未知环境中航行。 虽然照相机和激光雷达具有互补性优势和弱点，但将其结合到传统滤光片中并不是直接的。 我们的方法将两种模式紧密结合在一起，以便它可以处理包括平移和旋转在内的侵略性运动，以及缺乏光学纹理，如完全黑白或黑屏图像。 在非病理情况下，运动估计和环境重建的高精度是可能的。
我们提出的方法，即V-LOAM，探索了每个传感器的优点并补偿了另一个传感器的缺点，因此显示出相对于现有技术的性能的进一步改进。 该方法有两个顺序交错的过程。一是使用以高频运行的视觉测距作为图像帧速率（60Hz）来估计运动。 二是使用低频率（1 Hz）的激光雷达测距仪来细化运动估计并消除由视觉测距的漂移引起的点云中的失真。
V-LOAM的基本算法是足够通用的，它可以适用于使用不同类型的距离传感器，例如， 飞行时间相机。 如果先前的地图可用，该方法也可以被配置为仅提供本地化。
除了对KITTI odometry基准[1]的评估外，我们还进一步试验了广角相机和鱼眼相机。 我们的结论是鱼眼摄像机由于其更大的视野和更高的图像失真而带来更强的鲁棒性但精度更低。 但是，在扫描匹配细化之后，最终的运动估计达到相同的准确性水平。 我们的实验结果可以在公开的视频1中看到。

2.相关工作

基于视觉和激光雷达的方法在状态估计中很常见[2]。 利用立体相机[3]，[4]，基线为帮助确定运动的比例提供了参考。 然而，如果使用单眼相机[5] - [7]，运动的尺度一般是不能解决的，无需其他传感器或运动假设。 RGB-D相机的推出提供了将视觉图像与深度关联的有效方式。
RGB-D摄像机的运动估计[8]，[9]可以轻松进行。 还提出了许多RGB-D视觉测距法，显示出有希望的结果[10] - [12]。 但是，这些方法仅利用深度可用的成像区域，可能会浪费视觉图像中的大面积而没有深度覆盖。 在我们的系统中使用的视觉测距方法类似于[8] - [12]，因为它们都使用具有额外深度的视觉图像。 但是，我们的方法旨在利用激光雷达的稀疏深度信息。 它涉及具有和不具有解决运动深度的特征。
对于3D地图，典型的传感器是（2轴）3D激光雷达[13]。然而，由于激光雷达持续移动和移动，因此在点云中存在运动失真，因此难以使用这些激光雷达。消除失真的一种方法是结合其他传感器来恢复运动。例如，Scherer等人的导航系统[14]使用与IMU集成的立体视觉测距来估计微型飞行器的运动。激光雷达云由估计的运动记录。 Droeschel等人的方法[15]采用多摄像机视觉测距法，其次是基于多分辨率点云表示的扫描匹配方法。与[14]，[15]相比，我们的方法的不同之处在于它将相机和激光雷达紧密耦合，使得仅需要一个相机来进行运动恢复。我们的方法还考虑了由视觉测距的漂移引起的点云失真，即我们在短时间（1s）内将漂移建模为线性运动，并在扫描匹配期间用线性运动模型校正失真。
它还表明，状态估计只能用3D激光雷达。 例如，Tong等人。 通过堆叠来自2轴激光雷达的激光扫描产生的强度图像中的视觉特征来解决运动[16]。 运动用恒定速度和高斯过程建模。 然而，由于该方法从激光图像中提取视觉特征，因此需要密集点云。 另一种方法来自Bosse和Zlot [17]，[18]。 该方法匹配局部点簇的几何结构。 他们使用手持式绘图设备，由2D激光雷达和通过弹簧连接到手杆的IMU组成[17]。 他们还使用多个2轴激光雷达来绘制地下矿山[18]。 在这种方法中，通过批处理优化处理分段数据来恢复轨迹，其中分段数据具有连接在分段之间的边界约束。 该方法适用于离线调查，但不适合在线实时应用。
所提出的方法是基于我们在[19]，[20]中的工作，其中分别提出了视觉测距方法DEMO和激光雷达测距方法LOAM。 LOAM需要平滑运动并依靠IMU来补偿高频运动。 在本文中，对LOAM进行了修改，使得新方法V-LOAM将视觉测距输出视为运动先验，然后是激光雷达测距。 视觉测距中的相机型号也经过修改并与鱼眼相机兼容。 进行了一系列新的实验，结果表明V-LOAM具有较低的漂移。 结合高频视觉测距和鱼眼相机还可以使系统处理快速运动。

3. 坐标系和任务

本文讨论的问题是估计摄像机和激光雷达系统的运动，并用估计的运动建立遍历环境的地图。 我们假设相机是由一个通用的中心相机模型[21]建模的。 使用这种相机型号，我们的系统可以使用常规和鱼眼相机（请参见实验部分）。 我们假设摄像机内在参数是已知的。 相机和激光雷达之间的外部参数也被校准。 这允许我们对两个传感器使用单个坐标系，即传感器坐标系。 为了简化计算，我们选择传感器坐标系与摄像机坐标系一致 - 所有激光点在接收时投影到摄像机坐标系中。 作为本文的一个约定，我们使用左大写字母来指示坐标系。 在下面，我们来定义：
传感器坐标系{S}原点在相机光心，x轴指向左侧，y轴指向上侧，z轴指向前侧与相机主轴一致。世界坐标系{W}由{S}的初始位置得到。
通过定义假设和坐标系统，我们的测距和映射问题被表述为：
问题：给定{S}中获得的视觉图像和激光雷达云，根据{W}确定{S}的姿势，并在{W}中构建遍历环境的地图。

4. 系统总览

图2显示了算法系统。 整个系统分为两部分。 视觉测距部分使用在激光雷达云帮助下的视觉图像来估计图像帧速率下的传感器的帧到帧运动。 在本节中，特征跟踪块将提取并匹配连续图像之间的视觉特征。 深度图模块修正激光雷达云模块，并将深度与视觉特征相关联。 帧到帧运动估计模块采用视觉特征来计算运动估计。

为了总结激光雷达测距部分，让我们定义一个扫描，因为3D激光雷达完成了一次全扫描覆盖。 如果激光雷达的慢轴连续旋转，则扫描通常是全球旋转。 但是，如果慢轴来回旋转，则扫描是朝向相同方向的顺时针或逆时针旋转。 在我们的系统中，扫描持续1秒。 激光雷达测距部分每扫描一次执行一次，处理在整个扫描期间感知的点云。 首先，扫描到扫描细化块匹配连续扫描之间的点云，以细化运动估计并消除点云中的失真。 然后，映射修正块的扫描匹配并在当前构建的地图上修正点云，并发布相对于地图的传感器姿态。 传感器姿态输出是来自两个部分的变换的整合，以高频图像帧速率。

5. 视觉里程计

本部分总结了视觉测距方法。 利用激光雷达云，该方法使用视觉测距的估计运动来记录和维护深度图。 在计算运动时，根据深度来源，它涉及三种类型的视觉特征：深度图的深度，使用先前估计的运动的三角测量的深度，以及不可用的深度。 但是，要使用超过180°视野的鱼眼摄像机，让我们使用从现在开始的术语“距离”（特征的深度是其在^Sz方向上的距离的投影）。让我们用右上角k，k∈Z⁺表示图像帧，I表示视觉特征集。对于特征i，i∈I，与距离相关，其坐标在{S^k}中记为^SXi^k=[^Sxi^k,^Syi^k,^Szi^k]^T。对于具有未知距离的特征，我们使用其归一化坐标来代替。我们将传感器运动建模为刚体变换， 令R和T为3×3旋转矩阵和描述帧到帧运动的3×1平移向量。 我们定义运动为

公式1

在特征距离可用的情况下，我们可以将距离与 ^SXi ^k-1关联。然而， ^SXi ^k的距离总是未知的。 由于在这个阶段没有计算帧k-1和k之间的运动，我们无法从深度图或三角测量中检索 ^SXi ^k的距离。设 ^Sdi ^k为 ^SXi ^k未知的距离，虽然距离位置，可以利用公式1获得线性方程，消去d可以获得式(2)(3)，如图：

求距离

上述过程说明具有已知距离的特征提供了两个等式作为(2)-(3)，而具有未知距离的特征提供了一个等式作为(4)。 在求解运动时，我们叠加所有方程，并用六个未知量表示运动估计问题，表示他的六自由度运动。 问题由Levenberg-Marquardt方法解决。 运动估计适应于强大的拟合框架来处理特征跟踪错误。 根据(2)-(3)或（4）中的残差分配权重。 具有较大残差的特征被赋予较小的权重，而具有大于阈值的残差的特征被视为异常值并被赋予零权重。 如果找到收敛或满足最大迭代次数，则优化终止。