Visual-lidar Odometry and Mapping: Low-drift, Robust, and Fast. V-LOAM论文. ICRA2015

4.系统概略

整个系统被分为两部分：

视觉里程计

视觉里程计以图像帧率估计帧间运动，使用的是带有激光雷达辅助的视觉图像。

在这个部分中，特征提取模块提取并且匹配相邻帧的特征点。

深度地图注册depth map registration模块将激光点云和local depthmap关联到一起，为视觉特征提供深度。

帧间运动估计模块使用视觉特征估计运动。

激光雷达里程计

一次sweep扫描定义为一帧，本系统一次扫描持续1s，每完成一次扫描执行一次激光雷达里程计。

帧（sweep）间refinement模块匹配相邻帧的点云来refine估计出来的运动并且消除点云畸变。

帧-地图注册模块匹配点云和已经建好的地图，并发布相对于地图的位姿。

发布出去的位姿是整合了两个里程计的结果，以图像频率发布。

大概就是60hz帧的速度做视觉里程计，1hz的帧率做激光里程计。

当有激光的时候发布refine结果？没有激光的时候等于还是视觉里程计输出的位姿？

5.视觉里程计

通过激光点云的帮助，视觉里程计注册并且维护了一个深度地图depth map，用以估计运动。

计算运动的时候，三种视觉特征会被考虑进去，以深度的来源区分为：1. 深度地图提供深度 2. 根据之前运动估计三角化得到的深度 3. 无深度。

对于一个特征点，它可以表示成，第k帧的第i个特征点X。（原文还有个左上标S表示Sensor坐标系）

对于一个不知道深度的特征点，使用标准化坐标表示方式,，其中。

使用刚体运动方程建模帧间运动，R为旋转矩阵T为位移向量，得到：
（再省略一个特征下标i）

当k帧特征点深度存在的时候，我们能直接把这距离跟上一帧的点关联。

什么情况特征点的深度是知道的？

然而的深度总是未知，因为k-1到k帧的运动还未知，我们没办法从深度地图或者通过运动T三角化获得k帧深度。

通过运动T来三角化特征点以获得深度？一般是我们是通过两帧的特征点对来三角化特征点并且估算T吧？

定义，用取代(1)中的得到，第一第三行抵消d，第二第三行抵消d，得到：。

整理成标记成(2)和(3)。

当特征点深度不存在的时候，前一帧后一帧的深度都不知道。以相似的方法用和分别替换掉（1）中的部分。

结合三行消除d，得到：标记为(4).

咋消的d?

上面处理说明，当一个特征具备深度的时候可以给出2-3式，而不具备深度信息的时候只能给出4式。

我们把所有的点提供的等式摞在一起，把运动T用6自由度表示，用LM方法求解。每个点的残差用来提供权重，以减少错误匹配的影响。

当维护深度地图的时候，新收到的激光雷达点云会被加入到地图中。只有摄像头前面的点，并且不是太久的点会被考虑进去。

深度地图会被downsized（how?）以保持固定的点密度，并且投影到最近的相机帧（一旦最近帧和上一帧的转换关系知道）。

深度地图中的点用球坐标系表示（两个角度一个距离），并且以2D KD-tree存储（两个角度为轴）。

当查找特征点的深度的时候，我们找到该点最近的三个深度地图点，构成一个平面，相机光心到该平面的距离就是深度。（跟后来某paper一样做法）

另外，如何有些特征点在地图中找不到深度，但是已经被追踪了一段距离，我们会将这些点用连续图像帧三角化。

下图绿色特征点深度来自深度图，蓝色特征点深度来自三角化

还是不清楚具体流程，盲猜是

求RT

当相机观测到特征点，这时候k和k-1的运动还不知道，

所以通过图像中的特征点匹配关系，找到k-1的对应特征点，如果k-1的点知道深度（上图绿色蓝色），构建2-3式，如果不知道k-1特征点深度（上图红色），构建4式。

摞好所有特征点的公式，带上权重，利用LM求最优RT。

维护深度地图

这时候已经知道k到k-1的运动了，我们把激光雷达的点云投影到深度地图中，用KD tree存储。

当k+1帧来的时候，要在k帧里面找对应特征点的深度（蓝色绿色），这个深度信息就来自于这张更新到了k帧的深度地图。

激光雷达里程计

激光雷达和后面的部分好理解的多。。。有空再写 忙读其他论文去了。