slam学习（5）-vo （visual odometry）视觉里程计和图优化工具g2o

http://www.cnblogs.com/gaoxiang12/p/4754948.html

什么是视觉里程计呢？简而言之，就是把新来的数据与上一帧进行匹配，估计其运动，然后再把运动累加起来的东西。

• FRAME readFrame( int index, ParameterReader& pd ) 是读取帧数据的函数。告诉它我要读第几帧的数据，它就会乖乖的把数据给找出来，返回一个FRAME结构体。
• 在得到匹配之后，我们判断了匹配是否成功，并把失败的数据丢弃。为什么这样做呢？因为之前的算法，对于任意两张图像都能做出一个结果。对于无关的图像，就明显是不对的。所以要去除匹配失败的情形。
• 如何检测匹配失败呢？我们采用了三个方法：

1. 1. 去掉goodmatch太少的帧，最少的goodmatch定义为：

      min_good_match=10

   2. 去掉solvePnPRASNAC里，inlier较少的帧，同理定义为：

      min_inliers=5

   3. 去掉求出来的变换矩阵太大的情况。因为假设运动是连贯的，两帧之间不会隔的太远：

      max_norm=0.3

　　如何知道两帧之间不隔太远呢？我们计算了一个度量运动大小的值：∥Δt∥+min(2π−∥r∥,∥r∥)∥Δt∥+min(2π−∥r∥,∥r∥)。它可以看成是位移与旋转的范数加和。当这个数大于阈值max_norm时，我们就认为匹配出错了。

　　经过这三道工序处理后，vo的结果基本能保持正确啦。

那么，这个里程计有什么不足呢？

1.  一旦出现了错误匹配，整个程序就会跑飞。
2. 误差会累积。常见的现象是：相机转过去的过程能够做对，但转回来之后则出现明显的偏差。
3. 效率方面不尽如人意。在线的点云显示比较费时。

　　累积误差是里程计中不可避免的，后续的相机姿态依赖着前面的姿态。想要保证地图的准确，必须要保证每次匹配都精确无误，而这是难以实现的。所以，我们希望用更好的方法来做slam。不仅仅考虑两帧的信息，而要把所有整的信息都考虑进来，成为一个全slam问题（full slam）。下图为累积误差的一个例子。右侧是原有扫过的地图，左侧是新扫的，可以看到出现了明显的不重合。

这个问题也称为Bundle Adjustment(BA)，我们通常使用LM方法优化这个非线性平方误差函数。

　　BA方法是近年来视觉slam里用的很多的方法（所以很多研究者吐槽slam和sfm(structure from motion)越来越像了）。早些年间（2005以前），人们还认为用BA求解slam非常困难，因为计算量太大。不过06年之后，人们注意到slam构建的ba问题的稀疏性质，所以用稀疏的BA算法（sparse BA）求解这个图，才使BA在slam里广泛地应用起来。

　　为什么说slam里的BA问题稀疏呢？因为同样的场景很少出现在许多位置中。这导致上面的pose graph中，图GG离全图很远，只有少部分的节点存在直接边的联系。这就是姿态图的稀疏性。

　　求解BA的软件包有很多，感兴趣的读者可以去看wiki: https://en.wikipedia.org/wiki/Bundle_adjustment。我们这里介绍的g2o（Generalized Graph Optimizer），就是近年很流行的一个图优化求解软件包。下面我们通过实例代码，帮助大家入门g2o。

使用g2o图优化的简要步骤：第一步，构建一个求解器：globalOptimizer

然后，在求解器内添加点和边：然后，在求解器内添加点和边：

回环检测程序，利用g2o提升slam轨迹与地图的质量。有了他，将得到一个完整的slam程序，可以跑通大量的数据

前面的程序离完整的slam还有哪些距离。主要说来有两点：

1. 关键帧的提取。把每一帧都拼到地图是去是不明智的。因为帧与帧之间距离很近，导致地图需要频繁更新，浪费时间与空间。所以，我们希望，当机器人的运动超过一定间隔，就增加一个“关键帧”。最后只需把关键帧拼到地图里就行了。
2. 回环的检测。回环的本质是识别曾经到过的地方。最简单的回环检测策略，就是把新来的关键帧与之前所有的关键帧进行比较，不过这样会导致越往后，需要比较的帧越多。所以，稍微快速一点的方法是在过去的帧里随机挑选一些，与之进行比较。更进一步的，也可以用图像处理/模式识别的方法计算图像间的相似性，对相似的图像进行检测。

　　把这两者合在一起，就得到了我们slam程序的基本流程。以下为伪码：

1. 初始化关键帧序列：FF，并将第一帧f0f0放入FF。
2. 对于新来的一帧II，计算FF中最后一帧与II的运动，并估计该运动的大小ee。有以下几种可能性：
   • 若e>Eerrore>Eerror，说明运动太大，可能是计算错误，丢弃该帧； 
   • 若没有匹配上（match太少），说明该帧图像质量不高，丢弃； 
   • 若e<Ekeye<Ekey，说明离前一个关键帧很近，同样丢弃；
   • 剩下的情况，只有是特征匹配成功，运动估计正确，同时又离上一个关键帧有一定距离，则把II作为新的关键帧，进入回环检测程序：
3. 近距离回环：匹配II与FF末尾mm个关键帧。匹配成功时，在图里增加一条边。
4. 随机回环：随机在FF里取nn个帧，与II进行匹配。若匹配上，在图里增加一条边。
5. 将II放入FF末尾。若有新的数据，则回2； 若无，则进行优化与地图拼接。

　　slam流程大体是这样，也可以作一些更改。例如在线跑的话呢，可以定时进行一次优化与拼图。或者，在成功检测到回环时，同时检测这两个帧附近的帧，那样得到的边就更多啦。再有呢，如果要做实用的程序，还要考虑机器人如何运动，如果跟丢了怎么进行恢复等一些实际的问题呢。