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

最新推荐文章于 2023-09-20 21:49:40 发布

redfivehit

最新推荐文章于 2023-09-20 21:49:40 发布

阅读量3.3k

点赞数 2

分类专栏：视觉slam

视觉slam 专栏收录该内容

9 篇文章 0 订阅

订阅专栏

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

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

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

1. 去掉goodmatch太少的帧，最少的goodmatch定义为：
```
min_good_match=10
```
2. 去掉solvePnPRASNAC里，inlier较少的帧，同理定义为：
```
min_inliers=5
```
3. 去掉求出来的变换矩阵太大的情况。因为假设运动是连贯的，两帧之间不会隔的太远：
```
max_norm=0.3
```

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

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

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

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

　　累积误差是里程计中不可避免的，后续的相机姿态依赖着前面的姿态。想要保证地图的准确，必须要保证每次匹配都精确无误，而这是难以实现的。所以，我们希望用更好的方法来做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中，图 G 离全图很远，只有少部分的节点存在直接边的联系。这就是姿态图的稀疏性。

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

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

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

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

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

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

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

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