【翻译】visual-lidar odometry and mapping: low- drift, robust, and fast

---

作者：Ji Zhang and Sanjiv Singh
时间：2015/05

---

摘要 —— 在这里，我们提出了一个通用的框架，结合视觉里程计和激光雷达里程计在一个基本和第一原则的方法。该方法显示了在性能上的改进，特别是对aggressive motion的鲁棒性和temporary lack of visual features。所提出的在线方法从视觉里程计算开始，利用扫描激光雷达在高频率（high-frequency）、低保真度（low fidelity）下进行自运动估计（ego-motion）和点云记录。然后，基于扫描匹配的激光雷达测距法同时改进运动估计（motion estimation）和点云记录（point cloud registration）。我们展示了在我们自己的实验中收集的数据集以及使用KITTI odometry基准测试的结果。我们提出的方法在平均平移和旋转误差方面在基准上排名第一，相对位置漂移为0.75％。除了运动估计精度的比较之外，我们还评估了当传感器套件高速移动并且受到显着环境光照变化影响时方法的稳健性。

1. 引言

最近在视觉测距和激光雷达测距中的单独结果是有希望的，因为它们可以提供6DOF（前后、上下、左右/航向、俯仰、横滚）状态估计，映射甚至障碍物检测的解决方案。然而，仅使用每个传感器都存在缺点。**（VIO和DIO的缺陷）**视觉里程计需要适度的照明条件，并且如果不能获得不同的视觉特征则会失败。另一方面，通过移动激光雷达的运动估计涉及点云中的运动失真，因为在连续激光雷达运动期间的不同时间接收范围测量。因此，运动通常必须用大量变量来解决。在诸如由平面区域支配的退化场景中，扫描匹配也失败。

在此，我们提出了一种结合单目相机（monocular camera）和三维激光雷达（3D lidar）进行自我运动（ego-motion）估计的基本方法和第一原理方法。我们想要准确地估计实时和机载机器人导航在未知的环境的6自由度的运动以及环境的空间、度量表示。虽然相机和雷达有互补的优点和缺点，但将它们结合在一个传统的过滤器中并不简单。我们的方法将这两种模式紧密地耦合在一起，这样它既能处理包括平移和旋转在内的攻击性运动，又能处理像完全白化或黑屏图像那样缺乏光学纹理的情况。在非病理情况下，运动估计和环境重建的高精度是可能的。

我们提出的方法，即V-LOAM，探索了每个传感器的优点并补偿了另一个传感器的缺点，因此显示出相对于现有技术的性能的进一步改进。该方法具有两个顺序交错的过程。第一种是使用以高频率运行的视觉里程计作为图像帧速率（60Hz）来估计运动。第二种是在低频（1 Hz）下使用激光雷达测距法来重新估算运动，并消除由于视觉里程计的漂移而导致的点云失真。无失真点云被匹配并记录以逐步构建地图。结果是视觉里程计处理快速运动，并且激光雷达里程计保证在不期望的照明条件下的低漂移和鲁棒性。我们的发现是，地图通常是准确的，无需后处理。尽管回路检测可以进一步改善地图，但我们有意选择不这样做，因为这项工作的重点是推动精确里程计估计的极限。

V-LOAM的基本算法是足够通用的，它可以适用于使用不同类型的range传感器，例如，a time-of-fly camera。如果先前的地图可用，则该方法还可以被配置为仅提供本地化。

除了评估KITTI测距基准测试[1]外，我们还进一步尝试了广角相机和超薄相机。我们的结论是，由于其更大的视野和更高的图像失真，因此摄像机具有更强的鲁棒性，但精度更低。然而，在扫描匹配改进之后，最终的运动估计达到了相同的精度水平。我们的实验结果可以在公开的视频中看到。

2. 相关工作

基于视觉和激光雷达的方法常用于状态估计[2]。对于立体相机[3]，[4]，基线提供了一个参考以帮助确定运动的规模。然而，如果使用单目相机[5] - [7]，在没有其他传感器辅助或运动假设情况下，运动尺度通常是无法解决的。RGB-D相机的引入提供了一种将视觉图像与深度相关联的有效方式。使用RGB-D相机进行运动估计[8]，[9]可以很容易地进行规模化。还提出了许多RGB-D视觉测距方法里程计方法，显示出可行的结果[10] - [12]。然而，这些方法仅利用可获得深度的成像区域，可能在没有深度覆盖的视觉图像中浪费大面积区域。我们系统中使用的视觉里程计方法类似于[8]-[12]在所有意义上都使用了具有额外提供深度的视觉图像。但是，我们的方法旨在利用激光雷达的稀疏深度信息，它涉及有和没有深度解决运动的特征。

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

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

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

3. 坐标系和任务

本文所要解决的问题是**（目标）估计相机和激光雷达系统的运动，并用估计的运动建立一个遍历环境的地图。**我们假设相机是由一个通用的中央相机模型[21]建模的。有了这样一个相机模型，我们的系统可以同时使用普通相机和鱼眼相机(见实验部分)。我们假设相机的固有参数是已知的。标定了相机与激光雷达之间的外部参数。这使得我们可以为*两个传感器使用一个坐标系统，即传感器坐标系统（sensor coordinate system）。为了计算简单，我们选择传感器坐标系与相机坐标系重合，所有激光点在接收时投影到摄像机坐标系中。*作为本文的约定，我们使用左大写的标法来表示坐标系统。在下面，让我们来定义：

• 传感器坐标系{S}起源于摄像机光学中心。x轴指向左侧，y轴指向上方，z轴指向前方，与相机主轴一致；

• 世界坐标系{W}是在起始位置与{S}重合的坐标系。

在定义了假设和坐标系后，我们的里程计和映射问题表示为：

Problem: Given visual images and lidar clouds perceived in {S}, determine poses of {S} with respect to {W} and build a map of the traversed environment in {W}.

问题：给定在{S}中感知到的视觉图像和激光雷达云，确定{S}相对于{W}的姿态，并在{W}中构建已遍历环境的映射。

4. 系统概述

图2为软件系统框图。整个系统分为两个部分。视觉里程计部分利用激光里程计辅助获得的视觉图像，以图像帧速率估计传感器的帧与帧之间的运动。在本节中，特征跟踪块The feature tracking block提取并匹配连续图像之间的视觉特征。深度映射注册块The depth map registration block在本地深度映射上注册激光雷达点云，并将深度与视觉特性关联起来。帧对帧运动估计块The frame to frame motion estimation block采用视觉特征来计算运动估计。

为了总结激光雷达里程计部分，让我们定义一个扫描，当三维激光雷达完成一次完整的扫描覆盖。如果激光雷达的慢轴连续旋转，扫描通常是一个全球面旋转。但是，如果慢轴来回旋转，则扫频是顺时针或逆时针方向朝相同方向旋转。在我们的系统中，扫频持续1秒。激光雷达里程计部分每次扫描执行一次，处理在整个扫描中感知到的点云。首先，扫描到扫描细化块the sweep to sweep refinement block匹配连续扫描之间的点云，以细化运动估计并消除点云中的失真。然后，扫描到地图注册块the sweep to map registration匹配并注册当前构建的地图上的点云，并发布与地图相关的传感器姿态。该传感器的位姿输出是两个部分的变换的积分，在高频图像帧率。

5. 视觉里程计

本节总结了视觉测程法。使用激光雷达点云，该方法注册和维护一个深度图使用估计运动的视觉里程计。在计算运动时，涉及到深度来源的三种可视化特征：**深度图的深度、利用先前估计的运动进行三角测量的深度和不可用深度。**然而，要使用超过180视野的鱼眼相机，让我们从现在开始使用“distance距离”这个术语(特征的深度是它的距离在sz方向上的投影)。



上一帧深度已知：


上一帧深度未知：

上述过程说明，一个已知距离的特征提供两个方程为(2)-(3)，一个未知距离的特征提供一个方程为(4)。在求解运动时，我们将所有方程叠加起来，用六个未知数表示6自由度运动的运动估计问题。用**Levenberg-Marquardt方法求解。运动估计采用鲁棒拟合框架来处理特征跟踪误差。**根据(2)-(3)或(4)中的残差（额外约束）为每个特征分配一个权重。残差较大的特征分配较小的权重，残差大于阈值的特征被认为是异常值，权重为零。如果发现收敛性或满足最大迭代次数，则优化终止。

在维护深度图时，从激光雷达云接收到新的点后，会添加到深度图中。只保留相机前的点，而在一定时间之前接收到的点则被遗忘。将深度图缩小到一个恒定的点密度，投影到最后一个图像帧，该图像帧对前一帧的变换已经建立，即帧k1。我们用距离和两个角在球坐标中表示深度图上的点。这些点基于两个角坐标存储在二维kd树中。当关联到特征的距离时，我们从每个特征中找到深度图上最近的三个点。这三个点构成一个局部的平面补片，通过将相机中心的光线投射到平面补片上来插值这三个点的距离。

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

6. 雷达里程计

利用视觉里程计估计的帧与帧之间的运动，用激光雷达测程法进一步细化。激光雷达测程包含对点云进行粗到细处理的两个主要步骤：

• A sweep to sweep refinement step匹配连续扫描之间的点云以细化运动估计；

• A sweep to map registration step匹配并在地图上注册registration点云。

图4说明了从扫描到扫描重新调整步骤sweep to sweep refinement step的功能。橙色曲线表示用视觉里程计估计的传感器的非线性运动。视觉里程计的漂移通常被认为是慢动作。我们在扫描范围内(持续1秒)用蓝线表示接触速度下的漂移模型。利用视觉里程计恢复的运动记录激光雷达点云时，漂移引起了激光雷达点云的畸变。The sweep to sweep refinement step在激光雷达云匹配中加入了线性运动模型，消除了畸变。

让我们用正确的上标m，m属于z^+表示扫描，和Pm表示扫描m期间激光雷达点云。对于每个Pm，我们梳理点锐利的边缘提取的几何特性，即边角点，并指出在平面表面，即平面点，通过计算曲率的本地扫描。我们避免选择相邻点的点，以及与激光束大致平行的闭塞区域或局部表面边界上的点。这些点很可能包含较大的噪音或随着时间的推移位置的变化。图5给出了传感器在建筑物前导航时从扫描中检测到的边缘点和平面点的示例。

对于Em中的每一个点，我们在pm-1中找到了两个最近的边缘点，它们构成了一个边缘线段edge line segment。对于Hm中的每一个点，我们找到三个最近的平面点构成一个局部平面补片local planar patch。该过程使用了两棵3D KD树，一棵存储边缘点，另一棵存储pm-1中的平面点。利用边缘点与平面点的对应关系，导出了描述点与对应点距离的方程：

其中，S X m是点i ，i ∈ E m ∪ H m在{S m}中的坐标，di是到对应点的距离。结合(5)(6)得到一个关于T’的函数。求解T’的过程是将各边缘点和平面点的函数叠加，然后使总距离最小。非线性优化采用了适合于鲁棒拟合框架的Levenberg- Marquardt方法。通过计算T’，消除了Pm的失真。

最后，the sweep to map registration step匹配并注册当前构建的映射上的无失真激光雷达点云。将Qm定义为扫描m结束时的地图点云，如图6所示，这一步将Pm与Qm-1匹配，并将两个点云合并，构建一个新的地图云Qm。从Pm中提取了相同类型的边缘点和平面点。考虑到图云的密度性质，通过计算特征值和特征向量，考察Qm-1中局部点簇的分布情况，提取特征点之间的对应关系。一个大的和两个小的特征值表示一个边缘线段，两个大的和一个小的特征值表示一个局部平面patch。扫描匹配采用迭代最接近点方法[22]，类似于没有运动模型的扫描到扫描的细化步骤。

将Pm在地图上进行配准后，还在世界坐标系{W}中对地图上的传感器姿态进行了变换。由于这些变换每次扫描只计算一次，我们将它们与高频帧相结合，从视觉里程计到帧运动变换。如图7所示，得到的结果是在图像帧率下的高频综合位姿输出。

7. 实验

本文的研究在两个传感器系统上进行了验证，一个使用定制相机和激光雷达传感器，如图8所示，另一个使用KITTI基准数据集[1]的配置。通过对自定义传感器采集的数据进行分析，说明了该方法的可行性。该相机是配置在60Hz帧率uEye单目相机。三维激光雷达是基于北越UTM-30LX激光扫描仪。激光扫描仪有180个视野和0:25的分辨率，40行/秒的扫描速度。电机驱动激光扫描仪旋转运动，实现三维扫描。控制电机在90~ 90之间以180度/s角速度旋转，激光扫描仪水平方向为零。编码器以0:25分辨率测量电机旋转角度。

处理作者收集的数据的软件程序运行在Linux中具有2.5GHz四核处理器的笔记本电脑上。该方法大约消耗两个半核：视觉里程计取两个核，激光雷达里程计取半个核，因为每次扫描只执行一次。该方法使用Kanade Lucas Tomasi (KLT)方法最多跟踪300个Harris corners。为了均匀分布图像的视觉特征，将图像划分为5 - 6个相同的子区域，每个子区域提供最多10个特征。

在KITTI测程基准[1]上进行评价时，该方法使用单个摄像机和Velodyne激光雷达的数据。无论传感方式如何，它都优于其他方法，包括仅使用激光雷达的LOAM[20]方法。这主要是因为V-LOAM使用图像来计算扫描匹配的运动先验，而LOAM只处理激光数据。我们两种方法的结果都是公开的。

A．精度测试

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

图9和图10显示了在室内和室外环境下的精度测试结果。在两项测试中，传感器都由一个以0.7米/秒速度行走的人手持。图9-10(a)为实验样本图像。图9(a)中，第一行为广角相机拍摄的图像，第二行为鱼眼相机拍摄的相应图像。在图10(a)中，由于空间有限，我们只显示广角相机的图像。图9-10(b)为运动估计结果。我们比较了四种轨迹：两种分别来自广角相机和鱼眼相机的视觉测程，另两种则来自激光雷达测程。我们看到鱼眼相机的漂移速度(绿色曲线)比广角相机(红色曲线)更快，这是因为图像失真更严重。但是，激光雷达测程法测得的轨迹(蓝、黑曲线)相差不大，说明**激光雷达测程法无论测程偏移量大小，都能校正视觉测程偏移。**图9-10©为与图9-10(b)蓝色曲线对应的地图。图9-10(a)中编号为1-4的图像分别拍摄于图9-10©中编号为1-4的位置。

此外，我们进行了一项测试，包括室内和室外环境。如图11所示，路径从一栋建筑前面开始，穿过建筑，出口到外面，穿过两个楼梯，经过538米的行程，沿着一条小路回到起点。由于空间问题，我们删除了轨迹，只显示生成的地图。图11(a)中的图像是在图11(b)中的对应位置1-6拍摄的。

表I比较了三种测试的运动估计精度。精度是基于三维坐标计算的。对于测试1，路径包含两个循环。我们在闭环闭合处测量轨迹上的间隙，以确定相对位置误差作为沿环路行驶距离的一部分。对于测试2，激光雷达在路径的开始和结束处感知相同的对象。通过对激光雷达云团中15个点的人工提取和关联，计算出位置误差。对于测试3，位置误差是测量起点和终点之间的位置。由表I可知，虽然鱼眼相机的视觉测程不如广角相机精确，但是激光雷达测程可以将精度提高到相同的水平。

B. 鲁棒性测试（快速运动）

通过实验验证了该方法对快速运动的鲁棒性。我们首先选择如图12所示的楼梯环境，包括7个180度转弯。在楼梯上行走会给传感器带来连续的旋转。其次，我们选择一个廊道环境，如图13所示。沿着走廊走会带来连续的翻译。在每个环境中，一个人拿着传感器，沿着相同的路径走两次，一次是慢动作，另一次是快动作。在图12-13(a)中，我们显示了估计的轨迹。红色和绿色曲线分别来自于广角相机和鱼眼相机的慢动作试验。蓝色曲线为(a) (b) © (d) (e)图13。测试5：鲁棒性w.r.t.快速翻译。与试验4(图12)相同，它包含两个试验，一个是慢动作试验，另一个是快动作试验。(b)和©中的映射对应于(a)中的绿色和蓝色曲线。我们发现©中的墙壁由于快速运动而弯曲。从快速运动试验中。在这两种测试中，**当使用广角相机进行快速运动时，我们都遇到了一些问题，即在快速转弯过程中视觉跟踪不准确，导致运动估计失败。**轨迹被移除。在图12-13(b)中，我们给出了与绿色曲线对应的图，在图12-13©中，我们给出了与蓝色曲线对应的图。仔细比较，可以发现图12©中的点云是模糊的，图13©中的壁面由于快速运动而弯曲。

角速度和线速度的分布分别如图12-13(d)和图12-13(e)所示。**角速度采用空间旋转计算，线速度采用三维平移计算。**我们可以看到慢速和快速试验的速度有显著差异。在图12(d)中，快速试验时角速度可达170 =s，在图13(e)中，平均线速度约为2.6m/s。表二比较了相对位置误差。对于测试4，假设不同楼层的墙壁完全是平的且对齐的，则手动计算地面真实值。我们能够测量墙体弯曲的程度，从而确定轨迹末端的位置误差。对于测试5，使用环路闭合时的间隙计算误差。结果表明，与广角相机相比，鱼眼相机的精度略有下降，但在快速运动中具有较强的鲁棒性。

C．鲁棒性测试（光照剧烈变化）

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

8. 总结

提出了一种利用摄像机与三维激光雷达相结合的实时测程测图方法。这是**通过一种视觉里程方法在高频率估计自我运动，并通过激光雷达里程计在低频率细化运动估计和纠正漂移。**两部分的协同工作实现了精确鲁棒的运动估计，即视觉里程计处理快速运动，激光雷达里程计保证低漂移。该方法是在室内和室外使用我们自己的实验中收集的数据集，用广角相机和鱼眼相机进行测试。在KITTI测程基准上进一步评价了该方法，平均相对位置漂移为0.75%。实验结果还表明，该方法在传感器高速运动时具有较强的鲁棒性，并且在光照变化较大时具有较强的鲁棒性。