V-Loam论文解读
激光视觉融合的Ｖ-LOAM论文解读
论文题目：《Visual-lidar Odometry and Mapping: Low-drift, Robust, and Fast》
发表在2015年的ICRA上，是一篇经典的视觉激光融合的SLAM系统框架，但是作者未开源代码。
摘要
本文提出了一个视觉激光融合的框架，提升了系统在缺乏视觉特征和在有挑战的运动情况下的鲁棒性。这个方法利用视觉里程计估计机器的运动，高速率低精度的对准点云。然后基于激光雷达的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的配准模块匹配和对齐当前点云和地图，并发布相对于地图的传感器的位姿。
最后位姿的输出是集成了这两部分，然后按照图像的帧率进行输出。
视觉里程计
利用激光雷达的点云和视觉里程计得到的位姿对齐和维护了一个深度图。计算运动的时候，按照深度来源划分可以得到三种特征点：
- 来自深度图的深度
- 利用先前的位姿三角化得到的深度
- 没有深度的特征点
记号
-
distance表示特征在{S}系下的z值
-
右上标k表示图象帧
-
用I表示{ S k S^k Sk}图像特征的集合 ->S系下第k帧已知深度的特征i表示为： S X i k = [ S x i k , S y i k , S z i k ] T ^SX^k_i=[^Sx^k_i,^Sy^k_i,^Sz^k_i]^T SXik=[Sxik,Syik,Szik]T，S系下第k帧未知深度的特征i表示为： S X ˉ i k = [ S x ˉ i k , S y ˉ i k , S z ˉ i k ] T ^S\bar X^k_i=[^S\bar x^k_i,^S\bar y^k_i,^S\bar z^k_i]^T SXˉik=[Sxˉik,Syˉik,Szˉik]T，其中 ∣ ∣ S X ˉ i k ∣ ∣ ||^S\bar X^k_i|| ∣∣SXˉik∣∣=1
最后的运动模型可以定义为:
如果第k帧特征的深度可用，我们可以直接把这个深度关联到K-1帧的特征深度。由于这里还没有计算k-1到k的位姿，所有我们还不能通过深度图或者三角化得到第k帧的特征深度。定义 S d i k ^Sd^k_i Sdik是不知道深度的 S X i k ^SX^k_i SXik特征点的深度，且 S d i k = ∣ ∣ S X ˉ i k ∣ ∣ ^Sd^k_i=||^S\bar X^k_i|| Sdik=∣∣SXˉik∣∣。对于k-1帧有深度，k帧没深度的特征点，用第三行来融入一二行:：第三行 x S x ˉ i k ^S\bar x_i^k Sxˉik，然后用第一行 x S z ˉ i k ^S\bar z_i^k Szˉik减；第三行 x S y ˉ i k ^S\bar y_i^k Syˉik，然后用第二行减，这样就可以求出来k帧的z：
其中 R l R_l Rl和 T l T_l Tl， l ∈ { 1 , 2 , 3 } l \isin \{1,2,3\} l∈{ 1,2,3}表示R和T的第l行。
对于k帧和k-1帧都没有深度的点:
所以一个点如果有深度另外一个没有就用公式2，两个点都没有深度就用公式3。在求解运动时利于所有的方程用六个未知数来表示运动估计问题。这个问题由LM解决。运动估计采用一个鲁棒的拟合框架来解决特征跟踪误差。利用上边两个公式得到每个点的权重，残差越大权重越小，当残差大于某个阈值认为是外点需要剔除。设置其权重为0；如果找到收敛或满足最大迭代次数，则优化将终止。
维护深度图的时候，只添加相机前边的雷达点，那些在一定时间前的点就被丢弃。深度图会进行降采样保证常数的密度，还要把这些点投影到已经得到位姿的k-1帧。使用距离和两个角度在球面坐标上表示深度图上的点。 这些点基于两个角坐标存储在2D KD树中。当深度图中的点关联到特征点的时候，在深度图中刚找到3个和特征点最近的深度点组成一个平面的patch，利用相机中心到平面的视线插值得到特征点的深度。如果多次跟踪特征点在深度图中始终得不到深度，就用三角化得到深度。
雷达里程计
从视觉得到的帧到帧的运动估计可以利用激光里程计做优化。从粗到细的处理点云有两个重要的步骤:
- 匹配连续的sweep的点云来优化位姿估计
- sweep to map配准来对准点云到地图上
上图表示的是sweep-to-sweep的优化过程。橘色的线表示视觉里程计估计的传感器的非线性的运动。漂移经常被看作慢运动，所以我们在一个sweep把模型构建为恒速的(蓝色的线表示)。当利用视觉恢复的运动来对齐点云，漂移会导致点云畸变。sweep到sweep的优化过程代表一个线性的运动模型来匹配点云，去除畸变。
用右上标m表示第m个sweep扫描的点云 P m P^m Pm，在该帧点云中通过曲率检测边和平面特征。我们要避免选择那些邻域点已经被选择为特征点、或者是在互相遮挡的区域(occluded)、或者是平面的朝向和雷达线平行的情况选择特征点。
把线特征和平面特征存在两个容器中，利用m-1帧的特征和m帧的特征匹配，匹配之后m-1帧点云的畸变就被去除。所以对于当前的sweep我们只需要用线性的运动模型。定义T’表示第m个sweep的运动漂移，对于当前帧点i(线特征或者面特征)的运动漂移，可以定义为:
T i ′ = T ′ ( t i − t m ) / ( t m + 1 − t m ) T_i^{'} = T^{'}(t_i-t^m)/(t^{m+1}-t^m) Ti′=T′(ti−tm)/(tm+1−tm)
在m-1帧的线特征找两个和当前帧线特征最近的点，在m-1帧的平面特征中找三个和当前特征最近的点，这里用两个3D的KD-tree，一个存储线特征点一个存储平面特征点。我们利用下式来计算特征点的距离:
f ( S X i m , T i ′ ) = d i f(^SX^m_i,T^{'}_i) = d_i f(SXim,Ti′)=di
其中 S X i m ^SX^m_i SXim是世界坐标系下点i的对应点， d i d_i di是对应的距离，融合上边两个公式,我们可以关于 T ′ T^{'} T′的方程，目标是最小化距离来得到 T ′ T^{'} T′，利用每个线和面特征构建上述方程，利用带有合适鲁棒核函数的LM算法来优化。计算出来了 T ′ T^{'} T′以后我们就可以去除m帧种点云的畸变了。
最后，sweep-to-map的对齐步骤配准无畸变的点云。记录 Q m Q^m Qm是第m个sweep之后的点云。对齐 P m P^{m} Pm和 Q m − 1 Q^{m-1} Qm−1，然后融合这两个点云生成 Q m Q^m Qm。具体的融合过程是：在 Q m − 1 Q^{m-1} Qm−1利用特征值和特征向量检测特征，一个特征值远远大于另外两个的时候，这个点云簇就是一个线特征；两个特征值差不多大小，远远大于第三个的时候就是一个面特征(参考PCA理解，LOAM论文中对这部分阐述更为详细)。扫描匹配的过程包含迭代的寻找最近点，和没有运动模型的sweep-to-sweep的优化过程类似。
把 P m P^{m} Pm和地图对准后，在世界坐标系中表示传感器位姿。尽管这些位姿只是在sweep后计算，我们把他和帧到帧的视觉里程计得到的位姿混合，最后里程计发布的频率和图像的帧率一样。
参考文献
[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.