（每日一读2019.10.23）低漂移、鲁棒和快速的视觉-激光里程计和建图（VLoam）

参考：https://www.jianshu.com/p/cb7098567711
论文：pdf

摘要

本文开发了一个低成本的立体视觉惯性定位系统，该系统利用有效的基于多状态约束卡尔曼滤波（MSCKF）的视觉惯性里程计（VIO），同时利用先验激光雷达图提供有界误差的三维导航。除了VIO中使用的标准稀疏视觉特征测量外，在紧密耦合的MSCKF更新中，还利用视觉半稠密点云到先前激光雷达地图中进行全局校准，从而校正累积漂移。特别讨论了视觉点云和激光雷达点云之间的交叉模态约束。通过montecarlo模拟和实际实验验证了该方法的有效性，结果表明，通过不同的探测方式建立的云之间的lidar映射约束极大地提高了标准vio，并提供了有界误差性能。

引用

最近在视觉里程计和激光雷达里程计方面取得的独立成果很有希望，因为它们可以为六自由度状态估计、建图甚至障碍物检测提供解决方案。单独使用每一个传感器都存在缺点。

• 视觉的缺点：视觉里程表方法需要适度的照明条件，如果没有足够的视觉特征，则会失败。
• 激光的缺点：通过移动激光雷达的运动估计涉及点云中的运动失真，因为在连续激光雷达运动的不同时间接收距离测量。因此，运动常常需要用大量的变量来求解。在退化的场景中，例如那些由平面区域控制的场景，扫描匹配也会失败。

在此，我们提出了一种单目相机和三维激光雷达相结合的自我运动估计的基本原理和第一原理方法。我们希望实时准确地估计6自由度运动以及环境的空间、度量表示，并在未知环境中导航机器人。虽然相机和激光雷达具有互补的优势和劣势，但使用传统的滤波方法结合将他们比较难。我们的方法是紧耦合了这两种模式，因此它既能处理包括平移和旋转在内的快速运动，又能处理完全白化或黑化图像中缺少光学纹理的情况。在非病态条件下，高精度的运动估计和环境重建是可能的。

我们提出的方法，即V-LOAM，探索了每种传感器的优点，并弥补了另一种传感器的缺点，因此在性能上比现有技术有了进一步的改进。该方法有两个顺序交错的过程。第一种方法使用高频视觉里程计作为图像帧速率（60hz）来估计运动。第二种方法使用低频（1hz）的激光雷达里程计来改进运动估计，并消除由视觉里程计漂移引起的点云失真。无失真点云被匹配并注册为增量构建地图。其结果是，视觉里程计处理快速运动，激光雷达里程计保证在不理想的照明条件的低漂移和鲁棒性。我们发现这些地图通常是准确的，无需进行后期处理。虽然环路闭合可以进一步改善地图，但我们有意选择不这样做，因为这项工作的重点是推动精确的里程估计的极限。

V-LOAM的基本算法具有一定的通用性，可以适用于不同类型的距离传感器，如飞行时间摄像机。如果先前的映射可用，也可以将该方法配置为仅提供本地化。

除了对Kitti里程计基准的评估[1]，我们还进一步试验了广角摄像机和鱼眼摄像机。我们的结论是，鱼眼相机具有更大的视场和更高的图像失真，因而具有更强的鲁棒性，但精度较低。然而，经过扫描匹配细化后，最终的运动估计达到了相同的精度水平。我们的实验结果可以在公开的视频中看到。

II 相关工作

基于视觉和激光雷达的方法通常用于状态估计[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）将漂移建模为线性运动，并在扫描匹配过程中用线性运动模型进行校正。

典型的去除运动畸变的方法是在激光帧之间进行插值（比如视觉，轮速计，IMU等）

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

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

III 坐标系和规定

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

• 传感器坐标系 {S} 为摄像机光学中心。X轴指向左边，Y轴指向上，Z轴点与摄像机主轴一致。
• 世界坐标系 {W} 是在起始位置与{S}同向的坐标系。

在定义了假设和坐标系的情况下，我们的里程测量和地图绘制问题被描述为:
在给定视觉图像和激光雷达云图的情况下，确定{S}相对于{W}的姿态，建立{W}中环境的地图。

IV 系统总结

图2示出了软件系统的示意图。整个系统分为两部分。视觉里程计主要估计帧间运动，频率为图像的帧率。特征跟踪模块提取并匹配连续图像之间的视觉特征。深度地图注册块在局部深度地图上注册激光雷达云，并将深度与视觉特征相关联。帧到帧运动估计块利用视觉特征计算运动估计。

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

V 视觉里程计

本节总结了视觉里程计的方法。对于激光雷达云，该方法使用视觉里程计的估计运动来更新和维护深度地图。
在计算运动时，根据深度的来源，它涉及三种视觉特征：

• depthmap中的深度值
• 基于先前估计的运动状态进行三角化得到的深度值
• 无用的深度值

然而，要使用视野超过180°的鱼眼相机，我们从现在开始使用术语“距离”（特征的深度是其距离在$SZ$方向上的投影）。 让我们用右上标K,

表示图像帧，用$i$表示视觉特征集。\vec
对于与距离相关联的特征$i、i属于I$，其在{$S^k$}中的坐标表示为$S{X}_i^k=[S{x}_i^k；S{y}_i^k；S{z}_i^k{]}^T$。对于距离未知的特征，我们使用其标准化坐标$S{X}_i^k=[S{x}_i^k；S{y}_i^k；S{z}_i^k{]}^T$，其中$S{X}_i^k$的欧式距离是1。我们将传感器的运动建模为刚体变换。设$R$和$T$是一个$3\times 3$的旋转矩阵和一个$3\times 1$的平移向量，用于描述帧到帧的运动。我们把运动表述为



上述步骤表明，距离已知的特征提供两个方程（2）-（3），距离未知的特征提供一个方程（4）。在求解运动方程时，我们将所有方程组叠加起来，用6个未知量来表示6自由度运动，从而得到运动估计问题。这个问题用levenbergmarquardt方法解决。运动估计采用稳健的拟合框架来处理特征跟踪误差。根据每个特征在（2）-（3）或（4）中的残差为其分配权重。残差较大的特征被赋予较小的权重，而残差大于阈值的特征被认为是离群值并被赋予零权重。如果找到收敛或达到最大迭代次数，则优化终止。

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

此外，如果某些特征距离depthmap不可用，但它们被跟踪的距离超过了一定距离，则我们使用跟踪特征的图像序列对它们进行三角化。图3示出了对应于图1（左图像）的重构特征的示例。绿点是其距离与深度映射相关联的特征，蓝点是通过三角剖分（图1中的红点具有未知距离）。

VI 激光雷达里程计

由视觉里程计估计的帧到帧运动通过激光雷达里程计方法进一步细化。lidar里程计包含了点云粗到精处理的两个主要步骤：

• 1.sweep 到sweep 细化步骤在连续扫描之间匹配点云以细化运动估计
• 2sweep 到地图注册步骤匹配并在地图上注册点云。
  
  图4示出了sweep 到sweep 细化步骤的功能。橙色曲线表示由视觉里程计估计的传感器的非线性运动。视觉里程计的漂移通常被认为是慢动作。我们用蓝线表示的扫描（持续1s）内的接触速度来模拟漂移。当使用由视觉里程计恢复的运动来记录激光雷达点云时，漂移会导致激光雷达云的失真。sweep 到sweep 细化步骤在激光雷达云匹配中加入线性运动模型以消除失真。

我们用右上标$m，m含于Z^{+}$表示sweep ，用$p^m$表示sweep m时所感知到的lidar点云。对于每个$p^m$，我们通过计算局部sweep中的曲率来提取尖锐边缘上的合并点（即边缘点，平面上的点，平面点的几何特征）。我们避免选择相邻点的点以及与激光束大致平行的被遮挡区域或局部表面边界上的点。这些点可能包含较大的噪声或随着时间的推移而改变位置。图5给出了当传感器在建筑物前面导航时从扫描中检测到的边缘点和平面点的示例。

设$E^m$和$H^m$为从$p^m$中提取的边缘点和平面点的集合。我们将$E^m$和$H^m$与上次扫描的激光雷达云$p^{m-1}$匹配。这里，注意在扫描m-1完成后，$p^{m-1}$中的失真被修正。因此，我们只需要对当前扫描应用线性运动模型。将$T^{‘}$定义为在扫描m期间视觉里程漂移的6×1向量，并将$t^m$定义为该扫描的开始时间。对于点$i，i属于E^m并H^m，$在时间$t_i$时获取数据，$t^m$和$t_i$之间的对应漂移线性插值为

对于$E^m$中的每个点，我们找到$p^{m-1}$中形成边缘线段的两个最近的边缘点。对于$H^m$中的每个点，我们找到三个最近的平面点，它们形成一个局部平面片。该过程使用两个3D kd树，一个存储边缘点，另一个存储$p^{m-1}$中的平面点。根据边缘点与平面点的对应关系，导出了一个描述点与其对应关系之间距离的方程:

其中$S{X}_i^m$是点$i$的坐标，$i属于E^m和H^m，$在{$S$}中，$d_i$是与其对应关系的距离。结合（5）和（6），我们得到了$T^{‘}$的函数。$T^{‘}$的求解过程是将每个边点和平面点的函数叠加，然后使总距离最小。非线性优化采用levenbergmarquardt方法。通过计算$T^{‘}$，我们消除了$P^m$中的失真。

最后，sweep数据在地图中匹配并在当前构建的地图上加入无失真的激光雷达云。将$q^m$定义为扫描$m$结束时的地图云。如图6所示，此步骤将$P^m$与$q^{m-1}$匹配，并合并两个点云以构建新的地图云$q^m$。从$P^m$中提取相同类型的边缘点和平面点。考虑到地图云的密度特性，通过计算特征值和特征向量，通过检验$q^{m-1}$中局部点簇的分布来确定特征点的对应关系。
具体地说，一个大特征值和两个小特征值表示边缘线段，两个大特征值和一个小特征值表示局部平面片。扫描匹配涉及迭代最近点方法[22]，类似于没有运动模型的扫描到扫描细化步骤。

图6 解释了激光数据加入地图的步骤。

当$P^m$在地图上注册后，在世界坐标系{W}中，还发布了一个关于地图上传感器姿态的变换。由于这些变换每次扫描只计算一次，因此我们将它们与来自视觉里程计的高频帧到帧运动变换相结合。如图7所示，结果是以图像帧速率的高频集成姿势输出。

图7 蓝色部分表示激光雷达发布的关于世界坐标系{W}中传感器姿态的低频变换。橙色部分表示视觉里程计在包含帧到帧运动的高频下发布的变换。这两种变换被集成在一起，以产生图像帧速率下的高频传感器姿态输出。

VII实验

作者使用的是自制的传感器系统，一个180度的鱼眼相机和一个单线2D激光雷达UTM-30LX。雷达安装在一个-90到+90，角度分辨率为0.25的电机转轴上来模拟3D激光雷达。
电脑配置为2.5GHz，四核的linux PC
该方法使用Kanade Lucas Tomasi（KLT）方法（23）最大跟踪300个Harris角点。为了均匀地分布视觉特征，图像被分成5×6个相同的子区域，而每个子区域最多提供10个特征。

图9和图10是室内和室外的效果。这些都是由人手持进行测试的。

VIII 总结

我们提出了一种使用相机和三维激光雷达相结合的实时里程测量和地图绘制方法。这是通过一种视觉里程表方法估计自我运动在高频和激光雷达里程表方法，细化运动估计和纠正漂移在低频。这两个部分的配合可以实现精确和稳健的运动估计，即视觉里程计处理快速运动，而激光雷达里程计保证低漂移。该方法在室内和室外使用我们自己的实验收集的数据集，用广角相机和鱼眼相机进行测试。该方法在Kitti里程计基准上进行了进一步的评估，平均相对位置漂移为0.75%。实验结果还表明了该方法在传感器高速运动和光照变化较大时的鲁棒性。