《Visual-lidar Odometry and Mapping: Low-drift, Robust, and Fast》论文阅读

【摘要】

        在这里，我们提出了一个通用的框架，以结合视觉里程计和激光雷达里程计在一个基本和第一原理的方法。该方法在性能上有所改进，特别是在对剧烈性运动的鲁棒性和暂时缺乏视觉特征方面。所提出的在线方法从视觉里程计法开始，以估计自我运动，并在高频率但低精度度的扫描激光雷达上配准点云。然后，基于scan匹配的激光雷达里程计法同时精练了运动估计和点云配准。我们用在我们自己的实验中收集的数据集以及使用KITTI里程计基准来显示结果。我们提出的方法在平均平移和旋转误差方面在基准测试集上排名第1，相对位置漂移率为0.75%。除了对比运动估计的精度外，我们还评估了传感器套件高速移动并受显著环境照明变化时的鲁棒性。

【I. INTRODUCTION】

        最近在视觉里程计和激光雷达里程计方面的独立结果很有希望，因为它们可以为6-DOF状态估计、建图，甚至障碍检测提供解决方案。然而，单独使用每个传感器都存在缺点。视觉里程计方法需要适度的光照条件，如果没有明显的视觉特征，则会失败。另一方面，通过移动中激光雷达的运动估计涉及到点云的运动失真，因为在连续的激光雷达运动过程中，在不同的时间接收到距离测量。因此，运动通常需要用大量的变量来解决。scan匹配在退化场景，如平面区域也失败。在此，我们提出了一种结合单眼照相机和三维激光雷达的自我运动估计的基本和第一原理方法。我们希望准确地估计6-DOF运动以及环境的空间度量表示，实时和在机器人在未知环境中导航。虽然相机和激光雷达有互补的优缺点，但将它们结合在传统的滤波器中并不简单。我们的方法紧密结合了这两种模式，这样它既可以处理包括平移和旋转在内的剧烈运动，也可以处理在像完全白色或遮光图像那样缺乏光学纹理的场景。在非病态的场景中，运动估计和环境重建的高精度是可能的，

        我们提出的方法，即称为V-LOAM，探索了每个传感器的优点，并弥补了其他传感器的缺点，因此显示了性能的进一步改进。该方法有两个顺序交错的过程。首先使用视觉里程计以一个较高的频率作为图像帧速率(60Hz)来估计运动。第二种方法使用低频(1Hz)的激光雷达里程计来改进运动估计，并消除由视觉里程计漂移引起的点云失真。对无失真的点云进行匹配和配准，以逐步构建地图。结果是，视觉里程计处理快速运动，激光雷达里程计在不理想的照明条件下保证低漂移和鲁棒性。我们的发现是，这些地图通常是准确的，而不需要后处理。虽然闭环可以进一步改进建图，但我们有意地选择不这样做，因为本论文的这项工作的重点是推高精确的里程计估计的极限。

        V-LOAM的基本算法足够通用，它可以适用于使用不同类型的距离传感器，如飞行时间TOF摄像机。该方法还可以配置为仅在事先建好的地图可用时提供定位。除了评估KITTI里程计基准[1]外，我们还进一步用广角相机和鱼眼相机进行了实验。我们的结论是，鱼眼相机由于其更大的视场FOV和更高的图像变形，具有更强的鲁棒性，但精度较低。然而，经过scan匹配精炼后，最终的运动估计达到了相同的精度水平。我们的实验结果可以在一个公开的视频中看到。

II. RELATED WORK

        

         对于三维建图，一个典型的传感器是一个（2轴）3D激光雷达[13]。然而，要使用这些激光雷达是困难的，因为点云存在运动失真，因为激光雷达不断的范围和移动。消除失真的一种方法是结合其他传感器来恢复运动。例如，Scherer等人的导航系统[14]使用与IMU集成的立体视觉里程计来估计微型飞行器的运动。激光雷达点云是通过估计的运动来进行配准的。Droeschel等[15]的方法采用多摄像机视觉里程计，然随后采用基于多分辨率点云表示的扫描配准方法。与[14]，[15]相比，我们的方法的不同之处在于，它将一个相机和一个激光雷达紧密结合，因此只需要一个相机来进行运动恢复。我们的方法还考虑了由视觉里程计漂移引起的点云失真，即我们在短时间(1s)内将漂移建模为线性运动，并在扫描匹配过程中使用线性运动模型来校正失真。 

        结果还表明，只能用三维激光雷达进行状态估计。例如，Tong等人通过从2轴激光雷达上堆叠激光扫描来创建的强度图像来匹配视觉特征，以求解运动[16]。运动用恒定速度和高斯过程建模。然而，由于这种方法从激光图像中提取视觉特征，因此需要密集的点云。另一种方法来自Bosse和Zlot[17][18]。该方法匹配局部点簇的几何结构。他们使用一个由二维激光雷达和IMU组成的手持测绘设备，通过弹簧[17]连接到一个手杆上。他们还使用多个双轴激光雷达来绘制一个地下矿井的[18]地图。该方法通过连接边界约束的分段数据，对轨迹进行batch优化处理来恢复。该方法适用于离线使用，但不适合在线实时应用。 

        本文所提出的方法是基于我们在[19][20]中的工作，其中分别提出了视觉里程计方法DEMO和激光雷达里程计方法LOAM。LOAM需要平滑的运动，并依赖于一个IMU来补偿高频运动。本文对LOAM进行了改进，使新方法V-LOAM将视觉里程计输出作为运动先验，然后是得到激光雷达里程计。在视觉里程计中，它的相机模型也被修改，并与鱼眼相机兼容。进行了一组新的实验，结果表明V-LOAM具有较低的漂移能力。结合高频视觉里程计和广角的鱼眼相机也使该系统能够处理快速运动。 

III. COORDINATE SYSTEMS AND TASK

        本文所解决的问题是估计一个照相机和激光雷达系统的运动，并与估计的运动建立一个可通行的环境的地图。我们假设该相机是由一个一般的中央相机模型[21]建模的。有了这样的相机模型，我们的系统能够同时使用常规相机和鱼眼相机（见实验部分）。我们假设照相机的内参是已知的。对照相机和激光雷达之间的外部参数也进行了校准。这使得我们可以为两个传感器使用一个单一的坐标系，即传感器坐标系。为了便于计算，我们选择了与相机坐标系相一致的传感器坐标系——所有的激光点在接收后都被投影到相机坐标系中。作为本文的一个惯例，我们使用左大写符号来表示坐标系。在下面，让我们来定义 

（1）传感器坐标系起源于相机的光学中心。x轴指向左边，y轴指向上，z轴指向前，与相机主轴一致。

（2）世界坐标系是与传感器坐标系起始位置相重合的坐标系。

在定义了假设和坐标系统后，我们的里程计和建图问题被表示为

Problem:

        给定在传感器坐标系S中感知到的视觉图像和激光雷达点云，确定S相对于W中的姿态，即为里程计，并在W中构建一个可通行环境的地图。

IV. SYSTEM OVERVIEW

          图2为系统的软件示意图。整个系统被分为两部分。视觉里程计部分利用激光雷达云的辅助下的视觉图像(雷达提供深度信息)，以图像帧速率估计传感器的帧到帧的运动。在本节中，特征跟踪模块提取并匹配连续图像之间的视觉特征。深度图配准块在局部深度图上配准激光雷达云，并将深度与视觉特征相关联(这样就能提供视觉特征的深度信息)。从帧到帧的运动估计模块利用视觉特征来计算运动估计。 

        为了总结激光雷达的里程计部分，让我们定义一个sweep为三维激光雷达完成一次全扫描覆盖。如果激光雷达的慢轴连续旋转，一个sweep通常是一个全球形旋转。然而，如果慢轴来回旋转，sweep是指向同一方向的顺时针或逆时针旋转。在我们的系统中，一次sweep会持续1秒。激光雷达里程计就是每次sweep执行一次，处理在整个扫描中感知到的点云。首先，sweep到sweep精炼模块匹配(matches)连续sweep之间的点云，以提纯运动估计和消除点云中的失真。然后，sweep到map配准模块，并在当前构建的地图上匹配matches和配准点云，并发布相对于该地图坐标系的传感器位姿(发布里程计）。传感器位姿输出是以高频图像的帧率，将两个部分的位姿变换进整合得到的。    

V. VISUAL ODOMETRY

         本节总结了视觉里程计的方法。使用激光雷达点云，该方法使用视觉里程计的估计运动来配准并维护一个深度图。当计算运动时，从深度信息的来源它涉及到三种类型的视觉特征：深度来源于深度图，深度来自于先前估计的运动的三角法得到测量深度，以及得到的深度。然而，要使用超过180°FOV的鱼眼相机，让我们从现在开始使用术语“深度距离”(一个特征的深度是它在传感器z轴方向上的距离的投影)。我们使用右下角下标表示图像的帧数，用表示视觉特征的集合。对于集合中的一个特征点i，，其坐标在中的表示为，那么对于一个未知深度的特征点，我们用其法线坐标代替此处，.我们将传感器的运动建模为刚体的变换，令R和T分别描述刚体的旋转和平移运动，那么运动就能够建模为：

                                                              

         如果在某些情况下的特征的距离是可用的情况下，我们可以将该距离关联到上，然而的深度距离总是未知的，由于k-1帧和k帧之间的运动在这个阶段没有计算，我们无法从深度图或者三角法得到的距离。我们将未知的深度距离记作，，然后在上式的公式中用来代替 ，然后结合第一行，第二行，第三行，我们能够消去，这样就能获得以下两个方程：

                                     

                                     

在这里和,,表示R和T的第i行.

        针对一个没有深度距离的特征点，两个距离和均是未知的，所以我们用和代替式（1）中的两项。结合三行就能消除，，我们就能得到

                                                

        上述过程表明，一个已知距离的特征提供了两个方程为（2）-（3），而一个距离未知的特征提供了一个方程为（4）。当求解运动时，我们堆叠所有方程，并用6个未知数表示6 DOF的运动估计问题。用列文伯格马夸特法解决了这个问题。该运动估计适用于一个鲁棒拟合框架来处理特征跟踪误差。根据其在（2）-（3）或（4）中的残差为每个特征分配一个权重。残差较大的特征赋值较小，残差大于阈值的特征被认为是异常值，赋值为零。如果找到收敛性或满足最大迭代次数，则优化终止。

        在维护深度图时，在从激光雷达云接收到数据后，新的点会被添加到深度图中。只保留相机前方的点，而在一段时间前收到的点则被遗弃。缩小深度图以保持恒定的点密度，并投影到建立了变换到前一帧的最后一个图像帧，即帧k−1。我们用一个距离和两个角度用球坐标系来表示深度图上的点。这些点存储在基于两个角坐标系的2D-kd树中。当将距离与特征相关联时，我们可以从每个特征中找到深度图上的离它三个最近的点。这三个点形成一个局部平面小面片，通过从相机中心投射到平面小面片来插值得到到三个点的距离。

        此外，如果某些特征的深度图中没有距离，但它们被跟踪的距离超过一定距离，我们使用跟踪特征的图像序列对它们进行三角化。图3为图1对应的重构特征示例（左图）。绿点是与深度图的距离相关联的特征，蓝点是三角化的特征（图1中的红点表示距离未知）。

        本文的方法的目的是利用单目相机和三维激光雷达进行运动估计和建图。一个视觉里程计估计高频率但低保真度的运动。然后，采用激光雷达里程计对低频点云进行匹配，以细化运动估计并逐步建立地图。激光雷达里程计还消除了由视觉里程计的漂移引起的点云失真。这两个传感器的结合使该方法能够准确地建图，即使在快速运动和在不理想的照明条件下。

        图1中左侧图像对应的深度图和重建的视觉特征的一个例子。彩色的点代表深度图，其中颜色编码的高度。绿点是与深度图之间距离一致的特征，蓝点是通过运动结构得到的（图1中的红点表示距离未知）。

VI. LIDAR ODOMETRY

        利用激光雷达里程计进一步细化了由视觉里程计估计的帧到帧的运动。激光雷达里程计包含点云粗到细处理的两个主要步骤： sweep到sweep细化步骤匹配连续sweep之间的点云来精细化运动估计和一个sweep到map步骤匹配和配准点云在地图上。

        图4说明了sweep to sweep重新步骤的功能。橙色的曲线表示由视觉里程计估计的传感器的非线性运动。视觉里程计的漂移通常被认为是慢速运动。我们在一个扫描（持续1秒）内用接触速度来对漂移建模，用蓝线表示。当使用由视觉里程计恢复的运动来配准激光雷达云时，漂移会导致激光雷达云的畸变。sweep到sweep的提纯步骤在激光雷达云匹配中加入了一个线性运动模型来消除失真。

        让我们用右上标m，m∈Z+表示sweep的序号，用表示第m次sweep时感知到的激光雷达点云。对于每个，我们通过计算局部scan中的曲率，提取点云中几何尖锐边缘点，即边缘点和局部平面的表面点，即平面点的几何特征。我们避免选择选择相邻点的点，以及遮挡区域的边界或局部表面的点(loam中的瑕点)。这些点很可能包含较大的噪声或随着时间的推移而改变位置。图5给出了当传感器在建筑物前导航时，通过一个sweep检测到的边缘点和平面点的一个例子。

         从一次sweep中检测到的边缘点（蓝色）和平面点（绿色）的示例。在数据收集过程中，传感器指向一个建筑物。

        我们假设和为从中提取的边点和平面点的集合。我们将和与之前扫描的激光雷达点云相匹配，。在这里，请注意，在完成m−1次sweep后，中的失真被校正。因此，我们只需要对当前的扫描应用线性运动模型即可。我们将6X1维向量定义为描述扫描第m的sweep期间视觉里程计的漂移，并将定义为m次sweep的开始时间。对于一个点i，，其接收的时间为，那么对应的漂移从到就是

                                               

        对于每一个在中的点，我们在中找到两个最近邻的角点来组成边线段；对于在中的每一个点，我们找到三个最近邻点作为局部的平面片。这一个过程使用两个3D的kd-trees，一个数来存储中的角点，一个树用来存储中的面点。找到对应的角点和面点之后，一个等式有：

                                                         

这里的是点i的坐标系，i为角点和面点集合中的点，其坐标系为坐标系中，结合上面两式，我们能够得到一个关于的函数。解决就是通过多个角点和面点联立，最小化他们总的距离。非线性优化采用适应于鲁棒拟合的LM方法，如若计算完成，我们就能去除点云的畸变。

        最后，scan to map配准步骤匹配并配准当前构建的地图上的无失真激光雷达点云。定义为在第m次sweep结尾处的点云，如图所示，这一步匹配和,并整合这两个点云，然后合并为新的地图点云。同样的角点和面点都从中提取，考虑到地图点云的稠密性质，通过检查在中的局部点簇的分布来确定的特征点的对应关系，也是通过计算特征值和特征向量（与LOAM相同）。具体地说，一个大特征值和两个小特征值表示一个边线段，两个大特征值和一个小特征值表示一个局部平面斑块。扫描匹配涉及一个最近邻迭代方法ICP[22]，类似于没有运动模型的扫描到扫描精炼步骤。

         等将配准到地图上之后，传感器在地图上的位姿变换也发布出来，数值为基于世界坐标系。由于这些变换每次sweep只计算一次，我们将它们与视觉里程计的高频帧到帧运动姿态变换结合起来。如图7所示，其结果是在图像帧速率下的高频整合后的姿态输出。

 

         变换整合的说明。蓝色的部分表示由激光雷达里程计在低频下发布的变换(低频高精度的位姿变换)，它是关于世界坐标系{W}中的传感器姿态。橙色的部分表示由视觉里程计在包含帧到帧运动的高频下发布的变换。集成了这两种变换，以图像帧率产生高频传感器姿态输出。

VII. EXPERIMENTS

        本文的研究在两个传感器系统上进行了验证，一个使用如下图所示的定制照相机和激光雷达传感器，另一个使用KITTI基准数据集[1]的配置。通过本文，我们使用了从定制的传感器中收集的数据来说明本文提出的方法。该相机是一个uEye单色相机配置为60hz的帧率。3D激光雷达是基于HokuyoUTM-30LX的激光扫描仪。该激光扫描仪具有180°的视场角和0.25°的分辨率，扫描速率为40帧/秒。一个电机驱动激光扫描仪进行旋转运动，实现三维scan。（这里可以看到类似于loam的雷达系统，利用单线雷达+摆动电机搭建一个3D的激光扫描器）电机被控制为以180°/s的角速度，在−90°和90°之间来回旋转，激光扫描仪的水平方向为零。一个编码器以0.25度的角分辨率测量电机的旋转角。

        该软件程序处理作者收集的数据，运行在Linux中具有2.5GHz四核的笔记本电脑上。该方法消耗大约2.5核：视觉里程计需要两个核，激光雷达里程计需半个核，因为每次sweep只执行一次。该方法使用卡纳德卢卡斯托马西(KLT)方法[23]最大跟踪300个Harris角点。为了均匀地分布视觉特征，一幅图像被分成5×6个相同大小的子区域，而每个子区域提供多达10个特征。

        当评估KITTI测程基准[1]时，该方法使用来自单个相机和一个Velodyne激光雷达的数据。无论传感器是什么，本文的算法都优于其他方法，包括激光雷达方法LOAM[20]。这主要是因为V-LOAM使用图像的扫描匹配来计算运动先验，而LOAM只处理激光数据。我们对这两种方法的研究结果都是公开的。

A. Accuracy Tests

        我们首先使用两种相机设置进行精度测试，一个使用广角镜头（76°的水平FOV），另一个使用鱼眼镜头（185°水平FOV）。为了同时获取两幅图像，在上图的中原相机下方安装另一台相机，也就是说这两个相机除分辨率略有不同外，配置相同。原来的相机是752×480像素，而第二个相机是640×480像素。这是因为鱼眼镜头提供了一个圆形区域的像素信息(见图9(a)中的示例)，而进一步扩展相机的水平分辨率只扩大了图像的黑色区域。 

        图9和图10显示了在室内和室外环境中的精度测试结果。在这两个测试中，传感器都由一个以0.7米/秒的速度行走的人持有。图9-10(a)显示了来自测试的样本图像。在图9(a)中，第一行来自广角相机，第二行是来自鱼眼相机的对应图像。在图10(a)中，由于空间有限，我们只显示了来自广角相机的图像。图9-10(b)显示了运动估计的结果。我们比较了四种轨迹：两种分别来自广角相机和鱼眼相机的视觉里程计，另外两种由激光雷达里程计改进。由于鱼眼相机更重的图像失真，我们看到鱼眼相机（绿色曲线）比广角相机（红色曲线）产生更快的漂移。然而，他们两种视觉里程计经过激光雷达里程计改进后的轨迹（蓝色和黑色曲线）并没有太大差异，表明激光雷达里程计能够修正视觉里程计的漂移而无论漂移量大与小。图9-10(c)为与图9-10(b).中蓝色曲线对应的图图9-10(a)中标记有数字1-4的图像分别拍摄在图9-10(c).中的位置1-4处。

        此外，我们还进行了一项测试，包括室内和室外环境。如图11所示，路径从建筑物前面开始，穿过建筑物，并向外出口穿过两个楼梯，沿着一个小楼梯走，道路在行驶538米后返回到起始位置。由于空间问题，我们消除了轨迹，并只显示所建立的地图。图11(a)中的图像是在图11(b).中相应的位置1-6处拍摄的。

        表一比较了三个测试的运动估计精度。该精度是基于三维坐标来计算的。对于测试1，该路径包含两个闭环。我们测量在环闭合处轨迹上的间隙，以确定相对位置误差作为沿环移动的距离的分数。对于测试2，激光雷达在路径的开始和结束时感知到相同的物体。我们手动提取并关联激光雷达云中的15个点来计算位置误差。对于测试3，将测量起始位置和结束位置之间的位置误差。从表一中，我们得出结论，尽管鱼眼相机的视觉里程计精度不如广角相机，但激光雷达的里程计精度能够将精度提高到相同的水平。

B. Robustness Tests

        我们进一步进行了实验，以检验该方法对快速运动的鲁棒性。我们首先选择一个如下图所示的楼梯环境，其中包括7个180°转弯。在楼梯上行走会使传感器发生连续旋转。其次，我们选择了一个如图13所示的走廊环境。沿着走廊行走会带来持续的移动。在每个环境中，一个人拿着传感器，沿着同一路径走两次，一个是慢动作，另一个是快速动作。在图12-13(a)中，我们显示了估计的轨迹。红色和绿色的曲线分别来自慢动作试验，分别使用广角相机和鱼眼相机。

                                                        图12 楼梯环境 

 

                                                图13

        蓝色曲线是快速动作试验。在这两个测试中，当使用广角相机进行快速运动时，我们会遇到视觉特征在快速转弯时松散跟踪的问题，导致运动估计失败，轨迹被移除。在图12-13(b)中，我们展示了绿色曲线对应的图，在图12-13(c)中，我们显示了蓝色曲线对应的图。仔细比较，发现图12(c)的点云模糊，图13(c)的墙壁由于快速运动而弯曲。

        角速度和线速度的分布分别如图12-13(d)和图12-13(e)所示。角速度采用空间旋转法计算，线性速度基于三维平移法计算。我们可以看到慢速试验和快速试验在速度上有显著差异。在图12(d)中，快速试验的角速度覆盖范围高达170°/s，而在图13(e)中，平均线性速度约为2.6m/s。表二比较了相对位置误差。对于测试4，地面真值是人工计算的，假设不同楼层的墙壁是平坦的。我们能够测量墙壁的弯曲程度，从而确定轨迹末端的位置误差。对于测试5，将使用回路闭合时的间隙来计算误差。从这些结果中，我们得出结论，使用鱼眼相机相比，广角相机略有损失的精度，但在快速运动中获得了更多的鲁棒性。

         最后，我们实验了该方法对光照急剧变化的鲁棒性。如图14(a)所示，关灯4次。在位置1-2处，传感器在房间内导航，而在位置3-4处，传感器沿着走廊移动。当光熄灭时，视觉里程计停止工作，而使用恒速预测。每次扫描一次，都可以用激光雷达里程计来校正漂移。图14.(b)为所构建的地图。图14(c)-(d)显示了激光雷达里程计应用的修正量。这四个峰值代表了图14(a)中红色部分对应的大修正，这是由于恒速预测造成的漂移比视觉里程计更快。结果表明，该方法处理暂时的光照丢失是可行的(然而，对于连续的黑暗，本文所提出的方法是不合适的，建议读者使用我们的激光雷达方法，LOAM[20])

 

VIII. CONCLUSION

         我们提出了一种利用照相机和三维激光雷达相结合的实时里程计和建图方法。这是通过一种视觉里程计方法来估计高频自我运动和一种改进运动估计和修正低频漂移的激光雷达里程计方法。这两个组件的整合可以实现精确和鲁棒的运动估计，即视觉里程计处理快速运动，而激光雷达里程计保证低漂移。该方法在室内和室外进行了测试，使用了我们自己的实验中收集的数据集，使用了广角相机和鱼眼相机。该方法在KITTI测程基准上进一步评估，平均相对位置漂移为0.75%。我们的实验结果也表明，当传感器在高速移动和受到显著的光照变化时，该方法的鲁棒性。