2015 Visual-lidar Odometry and Mapping：Low-drift，Robust，and Fast

2015 Visual-lidar Odometry and Mapping：Low-drift，Robust，and Fast

**摘要：**本文从基本原理出发，提出了一个视觉里程计和激光里程计相结合的一般框架，该方法与现有技术相比在性能上有所提升，尤其是在有挑战性的运动和缺乏视觉特征下的鲁棒性，所提出在线方法从视觉里程计估计自身位姿，以高频率低精度方式进行激光雷达的点云配准，然后通过scan-match的激光里程计来优化运动估计和点云匹配，平均平移误差和旋转误差在Kitti数据排名第二，相对位姿漂移为0.75%，除了比较运动估计的精度外，还评估了在传感器高速运动和环境光照变化较大的情况下该方法的鲁棒性

I. INTRODUCTION

视觉里程计缺点：

• moderate lighting conditions 光照条件
• distinct visual features 突出的特征

激光里程计缺点

• motion distortion 运动畸变
• degenerate scenes 退化场景（平面）

两个步骤：

1. 视觉里程计以图像帧率为60Hz的高频率进行运动估计
2. 激光里程计以低频率(1Hz)优化运动估计和移除由于视觉里程计漂移导致的点云畸变，并根据去畸变的点云进行匹配和配准来增量构建地图

我们的发现是，这些地图往往是准确的，不需要后处理。虽然闭合环路可以进一步提高地图的质量，但由于这项工作的重点是突破精确里程估计的极限，所以没有回环检测

做了鱼眼相机和广角相机下的实验，由于鱼眼相机的视场大、图像失真严重，使得视觉里程计更加鲁棒，但精度较低，经过scan-match后会达到一样的精度水平

III. COORDINATE SYSTEMS AND TASK

一种相机模型[21],使得系统既可以使用普通相机，也可以使用鱼眼相机

假设相机的内参已知、激光和相机之间的外参以标定好，使得两者在同一坐标系下sensor coordinate system

为了简化计算，将sensor coordinate system选定为相机坐标系，并将所有点云投影到相机坐标系中

• sensor coordinate system {S} 的原点与相机光心重合，x轴向左，y轴向上，z轴与光轴重合
• 世界坐标系{W}与传感器坐标系{S}的起始位置重合

里程计和建图的问题可描述为：在接收到{S}下的图像和点云，得到{S}相对于{W}的位姿，建立在{W}下的环境地图

IV. SYSTEM OVERVIEW

Visual Odometry:

• 输入图像，获取特征
• 深度地图配准，利用点云数据来辅助视觉特征的深度估计
• 利用视觉特征进行帧间运动估计

Q问题：1s中输入60张图片和1帧scan，是怎么进行点云和视觉特征进行匹配的？？ 【看代码，前一帧的地图？】

Lidar odometry

慢轴连续旋转和来回旋转生成的点云形状不同

• sweep to sweeprefinement 优化运动估计、去除点云畸变
• the sweep to map registration 匹配并配准当前构建的地图上的点云，输出1Hz的map和相对于地图的传感器姿势
• integration of the transforms 两个位姿集成得到高频率的位姿输出

V. VISUAL ODOMETRY

本部分总结了视觉测距方法。 利用激光雷达的点云和视觉里程计得到的位姿对齐和维护了一个深度图。
在计算运动时，根据深度来源，它涉及三种类型的视觉特征：

• 深度图的深度
• 使用先前估计的位姿进行三角化得到的深度
• 以及不可用的深度

运动估计公式：

归一化坐标：${}^S{\overline{X}}_i^k=\left[{}^S{\overline{x}}_i^{k-1}{,}^S{\overline{y}}_i^{k-1}{,}^S{\overline{z}}_i^{k-1}\right]$

k时刻距离未知，两个方程：消去未知深度${}^S{d}_i^k$

k、k-1时刻距离都未知，一个方程，消去未知深度${}^S{d}_i^k$，${}^S{d}_i^{k-1}$公式中的第一个矩阵需要转置

运动估计采用一个鲁棒的拟合框架来解决特征跟踪误差，并根据上面公式的残差给每个特征赋权？？？

残差较大的要素被赋予较小的权重，而残差大于阈值的要素被视为异常值并被赋予零权重

维护深度图的时候，只添加相机前边的雷达点，那些在一定时间前的点就被丢弃。深度图会进行降采样保证常数的密度，还要把这些点投影到已经得到位姿的k-1帧，使用距离和两个角度在球面坐标上表示深度图上的点。这些点基于两个角坐标存储在2D KD树中，当深度图中的点关联到特征点的时候，在深度图中刚找到3个和特征点最近的深度点组成一个平面的patch，利用相机中心到平面的射线进行插值得到特征点的深度。如果多次跟踪特征点在深度图中始终得不到深度，就用三角化得到深度。

图1的左图中，绿色点来自深度图关联得到的，蓝色点来自三角化，红色点是未知的

VI. LIDAR ODOMETRY

sweep to sweeprefinement

橙色线表示视觉里程计估计的非线性运动，蓝色线表示视觉里程计的漂移，假设漂移在一次扫描（1s）内是恒速运动的，当采用视觉里程计来配准点云时候，由于漂移会导致运动失真，所以在sweep to sweep refinement部分根据线性模型进行去畸变。

边点edge points和平面点planar points的提取

点的提取注意：

• 选取点周围的点就不在选取
• 遮挡区域的边界点
• 与激光束大致平行的局部表面

漂移线性插值

根据上一次sweep得到的边点和面点$P^{k-1}$，对当前sweep的点进行点到线、点到面的距离求解，最小化距离来求解当前sweep的位姿$T_i^{{}^{\prime }}$,采用LM方法，求解得到位姿后，可以进行运动畸变去除。

sweep to map registration

根据当前点云$P^m$与上一时刻地图$Q^{m-1}$匹配生成新的地图$Q^m$

根据特征值和特征向量来判断边点和面点，一个大特征值和两个小特征值表示边缘线段，两个大特征值和一个小特征值表示局部平面片

transform integration

Q问题：是如何集成$S^m$,$S^k$得到高频率的输出位姿的？？【线性插值？还是类似回环检测一样优化把位姿平均误差均摊到每一时刻上】

Q问题：如何处理初始时刻的两者的关系

VII. EXPERIMENTS

采用Harris角点，为了均匀分布视觉特征，将一幅图像分成5×6个相同的子区域，每个子区域最多提供10个特征

A. Accuracy Tests

室内试验

第一行来自广角摄像机，第二行对应于鱼眼摄像机的图像

黑点是起点。虽然使用鱼眼相机比使用广角相机(红色)会产生更大的漂移(绿色)，但激光雷达里程计(蓝色和黑色)的精细化轨迹差别不大

室外实验

Test1：测量在两个回环处的轨迹上的间隙来确定相对位置误差

Test2：在初始时刻和末尾测量同一个物体，手工提取15个点来计算位置误差

Test3：位置误差是在起始位置和结束位置之间测量的？？

结论是，即使鱼雷相机的视觉里程计比广角相机的精度要低，激光雷达里程计也能将精度提高到同样的水平

B. Robustness Tests

B.1 运动对鲁棒性的影响

楼梯，连续旋转环境：对于同一个路径，快速运动和慢速运动实验

在快速运动中使用广角摄像机时，由于视觉特征在快速运动过程中跟踪丢失，导致运动估计失败

（b）（c）对应于(a)中绿色和蓝色轨迹的地图，©中的点云由于快速运动的影响而变得模糊

(d)-(e)给出了两次试验的角速度和线速度分布,角速度使用空间旋转计算，线速度基于3D平移

走廊，连续平移环境

结果

test4：假设不同楼层的墙完全平整且对齐，地面实况是手动计算的。我们可以测量墙壁弯曲的程度，从而确定轨迹末端的位置误差

test5：闭环处的间隙计算误差

结论:与广角摄像机相比，使用鱼雷摄像机的精度略有下降，但在快速运动时获得了更好的鲁棒性。

B.2 光照对鲁棒性的影响

光照关闭了四次，1-2是在房间内运动，3-4是在走廊移动，当灯熄灭时，视觉里程计停止工作，取而代之的是恒速预测，然后通过激光里程计来修复漂移

结论：对于短暂的视觉里程计失效是可用的，对于连续的黑暗，所提出的方法是不合适的，建议读者使用我们的仅限激光雷达的方法（LOAM）

Ⅷ 结论

B.2 光照对鲁棒性的影响

光照关闭了四次，1-2是在房间内运动，3-4是在走廊移动，当灯熄灭时，视觉里程计停止工作，取而代之的是恒速预测，然后通过激光里程计来修复漂移

结论：对于短暂的视觉里程计失效是可用的，对于连续的黑暗，所提出的方法是不合适的，建议读者使用我们的仅限激光雷达的方法（LOAM）

Ⅷ 结论

采用相机和3D激光雷达来进行里程计和建图，视觉里程计快速估计位姿，雷达里程计低频率优化运动估计和纠正漂移，两者的结合可以实现准确且鲁棒的运动估计（视觉里程计处理快速运动，激光里程计保证低漂移），该方法在Kitti数据集的结果为相对位姿漂移为0.75%，我们的实验也表明了该方法在传感器高速运动和环境光照变化较大的情况下该方法的鲁棒性。