视觉SLAM漫淡(二):图优化理论与g2o的使用

时间:2022-02-04 15:50:32

视觉SLAM漫谈(二):图优化理论与g2o的使用

1    前言以及回顾

  各位朋友,自从上一篇《视觉SLAM漫谈》写成以来已经有一段时间了。我收到几位热心读者的邮件。有的希望我介绍一下当前视觉SLAM程序的实用程度,更多的人希望了解一下前文提到的g2o优化库。因此我另写一篇小文章来专门介绍这个新玩意。

  在开始本篇文章正文以前,我们先来回顾一下图优化SLAM问题的提法。至于SLAM更基础的内容,例如SLAM是什么东西等等,请参见上一篇文章。我们直接进入较深层次的讨论。首先,关于我们要做的事情,你可以这样想:

  l   已知的东西:传感器数据(图像,点云,惯性测量设备等)。我们的传感器主要是一个Kinect,因此数据就是一个视频序列,说的再详细点就是一个RGB位图序列与一个深度图序列。至于惯性测量设备,可以有也可以没有。

  l   待求的东西:机器人的运动轨迹,地图的描述。运动轨迹,画出来应该就像是一条路径。而地图的描述,通常是点云的描述。但是点云描述是否可用于导航、规划等后续问题,还有待研究。

  这两个点之间还是有挺长的路要走的。如果我们使用图优化,往往会在整个视频序列中,定义若干个关键帧:

视觉SLAM漫淡(二):图优化理论与g2o的使用

  这个图着实画的有点丑,请大家不要吐槽……不管怎么说,它表达出我想表达的意思。在这张图中,我们有一个路标点(五角星),并在各个关键帧中都看到了这个点。于是,我们就能用PnP或ICP求解相邻关键点的运动方向。这些在上篇文章都介绍过了,包括特征选择,匹配及计算等等。那么,这个过程中有什么问题呢?

2    为什么要用全局优化

  你一定已经注意到,理想的计算总和实际有差距的。好比说理想的科研就是“看论文——产生想法——做实验——发文章”,那么现实的科研就是“看论文——产生想法——做实验——发现该想法在二十年前就有人做过了”,这样一个过程。实际当中,仅通过帧间运动(ego-motion)来计算机器人轨迹是远远不够的。如下图所示:

  视觉SLAM漫淡(二):图优化理论与g2o的使用

  

  如果你只用帧间匹配,那么每一帧的误差将对后面所有的运动轨迹都要产生影响。例如第二帧往右偏了0.1,那么后面第三、四、五帧都要往右偏0.1,还要加上它们自己的估算误差。所以结果就是:当程序跑上十几秒之后早就不知道飞到哪儿去了。这是经典的SLAM现象,在EKF实现中,也会发现,当机器人不断运动时,不确定性会不断增长。当然不是我们所希望的结果。

  那么怎么办才好呢?想象你到了一个陌生的城市,安全地走出了火车站,并在附近游荡了一会儿。当你走的越远,看到许多未知的建筑。你就越搞不清楚自己在什么地方。如果是你,你会怎么办?

  通常的做法是认准一个标志性建筑物,在它周围转上几圈,弄清楚附近的环境。然后再一点点儿扩大我们走过的范围。在这个过程中,我们会时常回到之前已经见过的场景,因此对它周围的景象就会很熟悉。

  机器人的情形也差不多,除了大多数时候是人在遥控它行走。因而我们希望,机器人不要仅和它上一个帧进行比较,而是和更多先前的帧比较,找出其中的相似之处。这就是所谓的回环检测(Loop closure detection)。用下面的示意图来说明:

视觉SLAM漫淡(二):图优化理论与g2o的使用

  

  没有回环时,由于误差对后续帧产生影响,机器人路径估计很不稳定。加上一些局部回环,几个相邻帧就多了一些约束,因而误差就减少了。你可以把它看成一个由弹簧连起来的链条(质点-弹簧模型)。当机器人经过若干时间,回到最初地方时,检测出了大回环时,整个环内的结构都会变得稳定很多。我们就可以籍此知道一个房间是方的还是圆的,面前这堵墙对应着以前哪一堵墙,等等。

  相信讲到这里,大家对回环检测都有了一个感性的认识。那么,这件事情具体是怎么建模,怎么计算,怎么编程呢?下面我们就一步步来介绍。

3    图优化的数学模型

  SLAM问题的优化模型可以有几种不同的建模方式。我们挑选其中较简单的一种进行介绍,即FrameSLAM,在2008年提出。它的特点是只用位姿约束而不用特征约束,减少了很多计算量,表达起来也比较直观。下面我们给出一种6*度的3D SLAM建模方法。

  符号:

视觉SLAM漫淡(二):图优化理论与g2o的使用

视觉SLAM漫淡(二):图优化理论与g2o的使用

  

  注意到这里的建模与前文有所不同,是一个简化版的模型。因为我们假设帧间匹配时得到了相邻帧的变换矩阵,而不是把所有特征也放到优化问题里面来。所以这个模型看上去相对简单。但是它很实用,因为不用引入特征,所以结点和边的数量大大减少,要知道在图像里提特征动辄成百上千的。

4    g2o是什么

  g2o,就是对上述问题的一个求解器。它原理上是一个通用的求解器,并不限定于某些SLAM问题。你可以用它来求SLAM,也可以用ICP, PnP以及其他你能想到的可以用图来表达的优化问题。它的代码很规范,就是有一个缺点:文档太少。唯一的说明文档还有点太装叉(个人感觉)了,有点摆弄作者数学水平的意思,反正那篇文档很难懂就是了。话说程序文档不应该是告诉我怎么用才对么……

  言归正传。如果你想用g2o,请去它的github上面下载:https://github.com/RainerKuemmerle/g2o

  它的API在:http://www.rock-robotics.org/stable/api/slam/g2o/classg2o_1_1HyperGraph.html

4.1     安装

  g2o是一个用cmake管理的C++工程,我是用Linux编译的,所以不要问我怎么在win下面用g2o,因为我也不会……不管怎么说,你下载了它的zip包或者用git拷下来之后,里面有一个README文件。告诉你它的依赖项。在ubuntu下,直接键入命令:

  sudo apt-get install cmake libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-qt4-dev

  我个人感觉还要 libcsparse-dev和freeglut3这两个库,反正多装了也无所谓。注意libqglviewer-qt4-dev只在ubuntu 12.04库里有,14.04 里换成另一个库了。g2o的可视化工具g2o_viewer是依赖这个库的,所以,如果你在14.04下面编,要么是去把12.04那个deb(以及它的依赖项)找出来装好,要么用ccmake,把build apps一项给去掉,这样就不编译这个工具了。否则编译过不去。

  解开zip后,新建一个build文件夹,然后就是:

  cmake ..

  make

  sudo make install

  这样g2o就装到了你的/usr/local/lib和/usr/local/include下面。你可以到这两个地方去看它的库文件与头文件。

4.2     学习g2o的使用

  因为g2o的文档真的很装叉(不能忍),所以建议你直接看它的源代码,耐心看,应该比文档好懂些。它的example文档夹下有一些示例代码,其中有一个tutorial_slam2d文件夹下有2d slam仿真的一个程序。值得仔细阅读。

  使用g2o来实现图优化还是比较容易的。它帮你把节点和边的类型都定义好了,基本上只需使用它内置的类型而不需自己重新定义。要构造一个图,要做以下几件事:

  l   定义一个SparseOptimizer. 编写方式参见tutorial_slam2d的声明方式。你还要写明它使用的算法。通常是Gauss-Newton或LM算法。个人觉得后者更好一些。

  l   定义你要用到的边、节点的类型。例如我们实现一个3D SLAM。那么就要看它的g2o/types/slam3d下面的头文件。节点头文件都以vertex_开头,而边则以edge_开头。在我们上面的模型中,可以选择vertex_se3作为节点,edge_se3作为边。这两个类型的节点和边的数据都可以直接来自于Eigen::Isometry,即上面讲到过的变换矩阵T。

  l   编写一个帧间匹配程序,通过两张图像算出变换矩阵。这个用opencv, pcl都可以做。

  l   把你得到的关键帧作为节点,变换矩阵作为边,加入到optimizer中。同时设定节点的估计值(如果没有惯性测量就设成零)与边的约束(变换矩阵)。此外,每条边还需设定一个信息矩阵(协方差矩阵之逆)作为不确定性的度量。例如你觉得帧间匹配精度在0.1m,那么把信息矩阵设成100的对角阵即可。

  l   在程序运行过程中不断作帧间检测,维护你的图。

  l   程序结束时调用optimizer.optimize( steps )进行优化。优化完毕后读取每个节点的估计值,此时就是优化后的机器人轨迹。

  代码这种东西展开来说会变得像字典一样枯燥,所以具体的东西需要大家自己去看,自己去体会。这里有我自己写的一个程序,可以供大家参考。不过这个程序需要带着数据集才能跑,学习g2o的同学只需参考里面代码的写法即可:https://github.com/gaoxiang12/slam3d_gx

5    效果

  最近我跑了几个公开数据集(http://vision.in.tum.de/data/datasets/rgbd-dataset)上的例子(fr1_desk, fr2_slam)(,感觉效果还不错。有些数据集还是挺难的。最后一张图是g2o_viewer,可以看到那些关键路径点与边的样子。

视觉SLAM漫淡(二):图优化理论与g2o的使用

视觉SLAM漫淡(二):图优化理论与g2o的使用

视觉SLAM漫淡(二):图优化理论与g2o的使用

  以上,如有什么问题,欢迎与我交流:gaoxiang12@mails.tsinghua.edu.cn