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

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


1    前言以及回顾

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

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

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

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

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

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

2   
为什么要用全局优化

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

  

  

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

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

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

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

  

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

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

3   
图优化的数学模型

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

  符号:

  

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

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,可以看到那些关键路径点与边的样子。

  以上,如有什么问题,欢迎与我交流:[email protected]

视觉SLAM漫淡(二):图优化理论与g2o的使用,布布扣,bubuko.com

时间: 2024-10-12 19:29:01

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

浅读《视觉SLAM十四讲:从理论到实践》--操作1--初识SLAM

下载<视觉SLAM十四讲:从理论到实践>源码:https://github.com/gaoxiang12/slambook 第二讲:初识SLAM 2.4.2 Hello SLAM(书本P27) 1.从github上下载源码,并解压 Ubuntu上,解压zip,先找到zip文件所在位置,然后运行下面代码,进行解压. unzip slambook-master.zip 解压后,找到ch2文件夹,在文件夹中找到helloSLAM.cpp文件 运行cpp文件 g++ helloSLAM.cpp 如未安

初识视觉SLAM:用相机解决定位和建图问题

引言:视觉SLAM 是指用相机解决定位和建图问题.本文以一个小机器人为例形象地介绍了视觉SLAM的功能及特点.本文选自<视觉SLAM十四讲:从理论到实践>. SLAM 是Simultaneous Localization and Mapping 的缩写,中文译作"同时定位与地图构建".它是指搭载特定传感器的主体,在没有环境先验信息的情况下,于运动过程中建立环境的模型,同时估计自己的运动.如果这里的传感器主要为相机,那就称为"视觉SLAM". 假设我们组装

CV学习资料《卷积神经网络与视觉计算》+《深度学习实践计算机视觉》+《视觉SLAM十四讲从理论到实践》电子资料代码分析

视觉和图形学真是一家,基础都一样! 如果学习图像识别,计算机视觉,推荐电子书<视觉SLAM十四讲:从理论到实践>,系统介绍了视觉SLAM(同时定位与地图构建)所需的基本知识与核心算法,既包括数学理论基础,如三维空间的刚体运动.非线性优化,又包括计算机视觉的算法实现,例如多视图几何.回环检测等. 一个周读完了,代码很清晰!Particle Filtering,KF,EKF, Batch Optimization, Lie Group,ICP,LK光流... 尤其惊喜的是文末作者看好的IMU-SL

深入理解图优化与g2o:图优化篇

前言 本节我们将深入介绍视觉slam中的主流优化方法——图优化(graph-based optimization).下一节中,介绍一下非常流行的图优化库:g2o. 关于g2o,我13年写过一个文档,然而随着自己理解的加深,越发感觉不满意.本着对读者更负责任的精神,本文给大家重新讲一遍图优化和g2o.除了这篇文档,读者还可以找到一篇关于图优化的博客: http://blog.csdn.net/heyijia0327 那篇文章有作者介绍的一个简单案例,而本文则更注重对图优化和g2o的理解与评注. 本

经典视觉SLAM框架

引言:通过前面的推送我们已经对SLAM有了个大体的认识.(初识视觉SLAM)下面来看经典的视觉SLAM框架,了解一下视觉SLAM究竟由哪几个模块组成.本文选自<视觉SLAM十四讲:从理论到实践>. 整体视觉SLAM流程图. 整个视觉SLAM流程包括以下步骤. 传感器信息读取.在视觉SLAM中主要为相机图像信息的读取和预处理.如果是在机器人中,还可能有码盘.惯性传感器等信息的读取和同步. 视觉里程计(Visual Odometry,VO).视觉里程计的任务是估算相邻图像间相机的运动,以及局部地图

图优化在slam中的应用——柏拉图启示录

南山学长在纸上写下了一个公式: e=z-h(x) 小K:这是什么?南山学长:这是一个蕴含了宇宙的终极奥秘的公式.小K:怎么可能?太离谱了,这只是一个普通的公式!南山学长:那好你用尺子测量一下纸上的这条直线有多长.小K(一脸疑惑的):3.55cm南山学长:你何以如此确信这条直线就是3.55cm,我是说它可能介于3.55和3.56之间,如果你再把图像放大,也许就能精确到小数点后三位.小K(想了想):恩,是这样的,放的越大得到的数据就越精确.南山学长:那么问题来了,这条直线必然存在一个精确的长度,而且

数据库优化理论整理之思维导图

经过整整一个月的整理,最新版的数据库优化理论梳理之思维导图版本已经出炉.这里先放出部分截图给大家欣赏.

入门实战《深度学习技术图像处理入门》+《视觉SLAM十四讲从理论到实践》

学习图像识别处理,想在数据分析竞赛中取得较高的排名,看了<深度学习技术图像处理入门>电子书,一边看电子书一边做标记,对配套的代码也做了测试,收获颇多. 从机器学习.图像处理的基本概念入手,逐步阐述深度学习图像处理技术的基本原理以及简单的实现. 学习理论后做实验,使用卷积神经网络进行端到端学习,构建深度卷积神经网络,使用循环神经网络改进模型,评估模型,测试模型.最关键的是可以将模型运用于实战之中,将深度学习模型导入到工程中,数据类型转换函数,实施CAM可视化,这是我最需要的. 视觉和图形学真是一

视觉SLAM漫谈 (三): 研究点介绍

1. 前言 读者朋友们大家好!(很久很久)之前,我们为大家介绍了SLAM的基本概念和方法.相信大家对SLAM,应该有了基本的认识.在忙完一堆写论文.博士开题的事情之后,我准备回来继续填坑:为大家介绍SLAM研究的方方面面.如果前两篇文章算是"初识",接下来几篇就是"渐入佳境"了.在第三篇中,我们要谈谈SLAM中的各个研究点,为研究生们(应该是博客的多数读者吧)作一个提纲挈领的摘要.然后,我们再就各个小问题,讲讲经典的算法与分类.我有耐心讲,你是否有耐心听呢? 在&l