本文转载自「计算机视觉工坊」，该公众号重点在于介绍计算机视觉方向的知识点，主要包括3D算法、2D算法、自动驾驶、光学介绍、算法与编程等。
论文题目：《Visual-lidar Odometry and Mapping: Low-drift, Robust, and Fast》 发表在2015年的ICRA上，是一篇经典的视觉激光融合的SLAM系统框架，但是作者未开源代码。在公众号「3D视觉工坊」后台，回复「V-LOAM」，即可获得原文。
摘要
本文提出了一个视觉激光融合的框架，提升了系统在缺乏视觉特征和在有挑战的运动情况下的鲁棒性。这个方法利用视觉里程计估计机器的运动，高速率低精度的对准点云。然后基于激光雷达的scan-match来优化运动估和点云对准，在KITTI的01序列达到了0.75%的位置漂移，系统在高速运动和照明变化的场景中也有很高的鲁棒性。
介绍
视觉里程计需要在光线渐变、特征比较多的场景中运行；激光在测量点云的时候存在运动畸变(激光采集一帧点云需要0.1s，所以一帧的点并不是在同一时刻采集的)，因此激光雷达的运动往往需要大量的变量来求解。不仅如此，在场景中平面较多的退化场景中，scan-match也很容易失败。这个系统包括两个交替的过程：
-
第一个是视觉里程计以60hz的速度来估计运动。
-
激光里程计利用低频率1hz来优化运动估计，去除由于里程的漂移造成的点云的运动畸变。
对无畸变的点云进行匹配并配准以逐步构建地图。结果是视觉里程计解决快速的运动，激光雷达可确保在不良照明条件下的低漂移和鲁棒性。实验结果表明地图很准确的无需后期处理。尽管闭环可以进一步改善地图精度但本文选择不这样做，因为这项工作的重点是实现更精准的里程计。
V-LOAM可以利用不同类型的测距传感器，在有先验地图的时候本系统也可以只定位。本文实验得到利用鱼眼相机得到更好的鲁棒性，但是由于大视角和严重的畸变精度比较差，经过激光雷达优化后精度可以得到提升。
本文的方法采用相机和激光雷达紧耦合的方式，从而仅需一个摄像机即可恢复运动。同时考虑了由视觉里程计的漂移引起的点云畸变，即我们将漂移建模为短时间（1s）内的线性运动，并在扫描匹配期间使用线性运动模型校正畸变。
坐标系统和任务
本文的任务是估计相机和雷达系统的运动，并利用估计的位姿构建环境的地图。假设相机是通过中央相机模型来建模的参考文档。利用这个模型系统可以利用常规的或者是鱼眼相机来定位。假设相机内参已知、相机和激光雷达外参通过标定已得到，这就允许我们利用一个坐标系来表示两个传感器。为了简化计算，我们选择相机坐标系作为机体坐标–所有激光点都投影到相机坐标系中。
- {S}表示相坐标系：x->left、 y ->upwards、z -> forward
- 世界坐标系{W}和{S}在0时刻是对齐的。
本文的问题可以表述：利用在{S}中得到的相机获得的图像和激光雷达得到的点云，确定{S}和{W}的位姿变换关系，在{W}系中构建地图。
系统概述
系被分为两部分：
- 视觉里程计：按图像的帧率利用图像和激光点云估计系统的运动。在这个部分特征跟踪模块在连续的图像帧中检测和匹配视觉特征点。深度图配准模块把局部的深度图和点云对齐并得到视觉特征点的深度。帧到帧的运动估计模块利用特征点计算机体运动。
- 激光里程计：激光雷达的一次完整的扫描定义为sweep。在这个系统中,一个sweep是1s。激光里程计一个sweep执行一次来处理在sweep中接收到的点云。首先sweep到sweep的refinement模块匹配连续帧的点云来优化运动估计并消除点云中的畸变。然后sweep到map的配准模块匹配和对齐当前点云和地图，并发布相对于地图的传感器的位姿。
最后位姿的输出是集成了这两部分，然后按照图像的帧率进行输出。
视觉里程计
利用激光雷达的点云和视觉里程计得到的位姿对齐和维护了一个深度图。计算运动的时候，按照深度来源划分可以得到三种特征点：
- 来自深度图的深度
- 利用先前的位姿三角化得到的深度
- 没有深度的特征点
记号
最后的运动模型可以定义为:
对于k帧和k-1帧都没有深度的点:
所以一个点如果有深度另外一个没有就用公式2，两个点都没有深度就用公式3。在求解运动时利于所有的方程用六个未知数来表示运动估计问题。这个问题由LM解决。运动估计采用一个鲁棒的拟合框架来解决特征跟踪误差。利用上边两个公式得到每个点的权重，残差越大权重越小，当残差大于某个阈值认为是外点需要剔除。设置其权重为0；如果找到收敛或满足最大迭代次数，则优化将终止。维护深度图的时候，只添加相机前边的雷达点，那些在一定时间前的点就被丢弃。深度图会进行降采样保证常数的密度，还要把这些点投影到已经得到位姿的k-1帧。使用距离和两个角度在球面坐标上表示深度图上的点。这些点基于两个角坐标存储在2D KD树中。当深度图中的点关联到特征点的时候，在深度图中刚找到3个和特征点最近的深度点组成一个平面的patch，利用相机中心到平面的视线插值得到特征点的深度。如果多次跟踪特征点在深度图中始终得不到深度，就用三角化得到深度。
雷达里程计
从视觉得到的帧到帧的运动估计可以利用激光里程计做优化。从粗到细的处理点云有两个重要的步骤:
- 匹配连续的sweep的点云来优化位姿估计
- sweep to map配准来对准点云到地图上
上图表示的是sweep-to-sweep的优化过程。橘色的线表示视觉里程计估计的传感器的非线性的运动。漂移经常被看作慢运动，所以我们在一个sweep把模型构建为恒速的(蓝色的线表示)。当利用视觉恢复的运动来对齐点云，漂移会导致点云畸变。sweep到sweep的优化过程代表一个线性的运动模型来匹配点云，去除畸变。
把线特征和平面特征存在两个容器中，利用m-1帧的特征和m帧的特征匹配，匹配之后m-1帧点云的畸变就被去除。所以对于当前的sweep我们只需要用线性的运动模型。定义T'表示第m个sweep的运动漂移，对于当前帧点i(线特征或者面特征)的运动漂移，可以定义为:
在m-1帧的线特征找两个和当前帧线特征最近的点，在m-1帧的平面特征中找三个和当前特征最近的点，这里用两个3D的KD-tree，一个存储线特征点一个存储平面特征点。我们利用下式来计算特征点的距离:
参考文献
[1] Zhang J , Singh S . Visual-lidar odometry and mapping: low-drift, robust, and fast[C]// IEEE International Conference on Robotics & Automation. IEEE, 2015.
[2] Zhang J , Singh S . LOAM: Lidar Odometry and Mapping in Real-time[C]// Robotics: Science and Systems Conference. 2014.
本文仅做学术分享，如有侵权，请联系删文。下载1在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。
下载2在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计汇总等。
下载3在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。