从零开始一起学习SLAM理解图优化，一步步带你看懂g2o代码

从零开始⼀起学习SLAM理解图优化，⼀步步带你看懂g2o代码菜单栏点击“知识星球”查看「从零开始学习SLAM」⼀起学习交流

本⽂⼲货较多，建议收藏，仔细阅读

⼩⽩：师兄师兄，最近我在看SLAM的优化算法，有种⽅法叫“图优化”，以前学习算法的时候还有⼀个优化⽅法叫“凸优化”，这两个不是⼀个东西吧？

师兄：哈哈，这个问题有意思，虽然它们中⽂发⾳⼀样，但是意思差别⼤着呢！我们来看看英⽂表达吧，图优化的英⽂是 graph optimization 或者 graph-based optimization，你看，它的“图”其实是数据结构中的graph。⽽凸优化的英⽂是convex optimization，这⾥的“凸”其实是凸函数的意思，所以单从英⽂就能区分开它们。

⼩⽩：原来是这样，我看SLAM中图优化⽤的很多啊，我看了⼀下⾼博的书，还是迷迷糊糊的，求科普啊师兄

师兄：图优化真的蛮重要的，概念其实不负责，主要是编程稍微有点复杂。。

⼩⽩：不能同意更多。。，那个代码看的我⼀脸懵逼

图优化有什么优势？

师兄：按照惯例，我还是先说说图优化的背景吧。SLAM的后端⼀般分为两种处理⽅法，⼀种是以扩展卡尔曼滤波（EKF）为代表的滤波⽅法，⼀种是以图优化为代表的⾮线性优化⽅法。不过，⽬前SLAM研究的主流热点⼏乎都是基于图优化的。

⼩⽩：据我所知，滤波⽅法很早就有了，前⼈的研究也很深。为什么现在图优化变成了主流了？

师兄：你说的没错。滤波⽅法尤其是EKF⽅法，在SLAM发展很长的⼀段历史中⼀直占据主导地位，早期的⼤神们研究了各种各样的滤波器来改善滤波效果，那会⼊门SLAM，EKF是必须要掌握的。顺便总结下滤波⽅法的优缺点：

优点：在当时计算资源受限、待估计量⽐较简单的情况下，EKF为代表的滤波⽅法⽐较有效，经常⽤在激光SLAM中。

缺点：它的⼀个⼤缺点就是存储量和状态量是平⽅增长关系，因为存储的是协⽅差矩阵，因此不适合⼤型场景。⽽现在基于视觉的SLAM⽅案，路标点（特征点）数据很⼤，滤波⽅法根本吃不消，所以此时滤波的⽅法效率⾮常低。

⼩⽩：原来如此。那图优化在视觉SLAM中效率很⾼吗？

师兄：这个其实说来话长了。很久很久以前，其实就是不到⼗年前吧(感觉好像很久)，⼤家还都是⽤

滤波⽅法，因为在图优化⾥，Bundle Adjustment（后⾯简称BA）起到了核⼼作⽤。但是那会SLAM的研究者们发现包含⼤量特征点和相机位姿的BA计算量其实很⼤，根本没办法实时。

⼩⽩：啊？后来发⽣了什么？（认真听故事ing）

师兄：后来SLAM研究者们发现了其实在视觉SLAM中，虽然包含⼤量特征点和相机位姿，但其实BA是稀疏的，稀疏的就好办了，就可以加速了啊！⽐较代表性的就是2009年，⼏个⼤神发表了⾃⼰的研究成果《SBA：A software package for generic sparse bundle adjustment》，⽽且计算机硬件发展也很快，因此基于图优化的视觉SLAM也可以实时了！⼩⽩：厉害厉害！向⼤⽜们致敬！

图优化是什么？

⼩⽩：图优化既然是主流，那我可以跳过滤波⽅法直接学习图优化吧，反正滤波⽅法也看不懂。。

师兄：额，图优化确实是主流，以后有需要你可以再去看滤波⽅法，那我们今天就只讲图优化好啦

⼩⽩：好滴，那问题来了，究竟什么是图优化啊？

师兄：图优化⾥的图就是数据结构⾥的图，⼀个图由若⼲个顶点（vertex），以及连接这些顶点的边（edge）组成，给你举个例⼦

⽐如⼀个机器⼈在房屋⾥移动，它在某个时刻 t 的位姿（pose）就是⼀个顶点，这个也是待优化的变量。⽽位姿之间的关系就构成了⼀个边，⽐如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是边，边通常表⽰误差项。

在SLAM⾥，图优化⼀般分解为两个任务：

1、构建图。机器⼈位姿作为顶点，位姿间关系作为边。

2、优化图。调整机器⼈的位姿（顶点）来尽量满⾜边的约束，使得误差最⼩。

下⾯就是⼀个直观的例⼦。我们根据机器⼈位姿来作为图的顶点，这个位姿可以来⾃机器⼈的编码器，也可以是ICP匹配得到的，图的边就是位姿之间的关系。由于误差的存在，实际上机器⼈建⽴的地图是不准的，如下图左。我们通过设置边的约束，使得图优化向着满⾜边约束的⽅向优化，最后得到了⼀个优化后的地图（如下图中所⽰），它和真正的地图（下图右）⾮常接近。

⼩⽩：哇塞，这个图优化效果这么明显啊！刚开始误差那么⼤，最后都校正过来了

师兄：是啊，所以图优化在SLAM中举⾜轻重啊，⼀定得掌握！

⼩⽩：好，有学习的动⼒了！我们开启编程模式吧！

先了解g2o 框架

师兄：前⾯我们简单介绍了图优化，你也看到了它的神通⼴⼤，那如何编程实现呢？

⼩⽩：对啊，有没有现成的库啊，我还只是个“调包侠”。。

师兄：这个必须有啊！在SLAM领域，基于图优化的⼀个⽤的⾮常⼴泛的库就是g2o，它是General Graphic Optimization 的简称，是⼀个⽤来优化⾮线性误差函数的c++框架。这个库可以满⾜你调包侠的梦想~

⼩⽩：哈哈，太好了，否则打死我也写不出来啊！那这个g2o怎么⽤呢？

shaq vs师兄：我先说安装吧，其实g2o安装很简单，参考GitHub上官⽹：

按照步骤来安装就⾏了。需要注意的是安装之前确保电脑上已经安装好了第三⽅依赖。

⼩⽩：好的，这个看起来很好装。不过问题是，我看相关的代码，感觉很复杂啊，不知如何下⼿啊

师兄：别急，第⼀次接触g2o，确实有这种感觉，⽽且官⽹⽂档写的也⽐较“不通俗不易懂”，不过如果你能捋顺了它的框架，再去看代码，应该很快能够⼊⼿了

⼩⽩：是的，先对框架了然于胸才⾏，不然即使能凑合看懂别⼈代码，⾃⼰也不会写啊！

师兄：嗯嗯，其实g2o帮助我们实现了很多内部的算法，只是在进⾏构造的时候，需要遵循⼀些规则，在我看来这是可以接受的，毕竟⼀个程序不可能满⾜所有的要求，因此在以后g2o的使⽤中还是应该多看多记，这样才能更好的使⽤这个库。

⼩⽩：记住了。养成记笔记的好习惯，还要多练习。

师兄：好，那我们⾸先看⼀下下⾯这个图，是g2o的基本框架结构。如果你查资料的话，你会在很多地⽅都能看到。看图的时候要注意箭头类型

1、图的核⼼

⼩⽩：师兄，这个图该从哪⾥开始看？感觉好多东西。。

师兄：如果你想要知道这个图中哪个最重要，就去看看箭头源头在哪⾥

⼩⽩：我看看。。。好像是最左侧的SparseOptimizer？

师兄：对的，SparseOptimizer是整个图的核⼼，我们注意右上⾓的 is-a 实⼼箭头，这个SparseOptimizer它是⼀个Optimizable Graph，从⽽也是⼀个超图（HyperGraph）。不再失落

⼩⽩：我去，师兄，怎么突然冒出来这么多奇怪的术语，都啥意思啊？

师兄：这个你不需要⼀个个弄懂，不然可能黄花菜都凉了。你先暂时只需要了解⼀下它们的名字，有些以后⽤不到，有些以后⽤到了再回看。⽬前如果遇到重要的我会具体解释。

⼩⽩：好。那下⼀步看哪⾥？

2、顶点和边

师兄：我们先来看上⾯的结构吧。注意看 has-many 箭头，你看这个超图包含了许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）。⽽这些顶点顶点继承⾃ Base Vertex，也就是OptimizableGraph::Vertex，⽽边可以继承⾃BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge ⼩⽩：头有点晕了，师兄

师兄：哈哈，不⽤⼀个个记，现阶段了解这些就⾏。顶点和边在编程中很重要的，关于顶点和边的定义我们以后会详细说的。下⾯我们来看底部的结构。

⼩⽩：嗯嗯，知道啦！

3、配置SparseOptimizer的优化算法和求解器

师兄：你看下⾯，整个图的核⼼SparseOptimizer 包含⼀个优化算法（OptimizationAlgorithm）的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（⾼斯⽜顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell's dogleg 三者中间选择⼀个（我们常⽤的是GN和LM）

⼩⽩：GN和LM就是我们以前讲过的⾮线性优化⽅法中常⽤的两种吧

4、如何求解

师兄：那么如何求解呢？OptimizationWithHessian 内部包含⼀个求解器（Solver），这个Solver实际是由⼀个BlockSolver组成的。这个BlockSolver有两个部分，⼀个是SparseBlockMatrix ，⽤于计算稀疏的雅可⽐和Hessian矩阵；⼀个是线性⽅程的求解器（LinearSolver），它⽤于计算迭代过程中最关键的⼀步HΔx=−b，LinearSolver有⼏种⽅法可以选择：PCG, CSparse, Choldmod，具体定义后⾯会介绍

到此，就是上⾯图的⼀个简单理解。

⼀步步带你看懂g2o编程流程

⼩⽩：师兄，看完了我也不知道编程时具体怎么编呢！

师兄：我正好要说这个。⾸先这⾥需要说⼀下，我们梳理是从顶层到底层，但是编程实现时需要反过来，像建房⼦⼀样，从底层开始搭建框架⼀直到顶层。g2o的整个框架就是按照下图中我标的这个顺序来写的。

⾼博在⼗四讲中g2o求解曲线参数的例⼦来说明，源代码地址

为了⽅便理解，我重新加了注释。如下所⽰，

typedefg2o::BlockSolver< g2o::BlockSolverTraits< 3, 1> > Block; // 每个误差项优化变量维度为3，误差值维度为1 // 第1步：创建⼀个线性求解器LinearSolver

Block::LinearSolverType* linearSolver = newg2o::LinearSolverDense<Block::PoseMatrixType>();

// 第2步：创建BlockSolver。并⽤上⾯定义的线性求解器初始化

Block* solver_ptr = newBlock( linearSolver );

// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选⼀个，再⽤上述块求解器BlockSolver初始化

g2o::OptimizationAlgorithmLevenberg* solver = newg2o::OptimizationAlgorithmLevenberg( solver_ptr );

// 第4步：创建终极⼤boss 稀疏优化器（SparseOptimizer）

g2o::SparseOptimizer optimizer; // 图模型唐溶

optimizer.setAlgorithm( solver ); // 设置求解器

optimizer.setVerbose( true); // 打开调试输出

// 第5步：定义图的顶点和边。并添加到SparseOptimizer中

CurveFittingVertex* v = newCurveFittingVertex(); //往图中增加顶点

数字光纤直放站

v->setEstimate( Eigen::Vector3d( 0, 0, 0) );

v->setId( 0);

optimizer.addVertex( v );

for( inti= 0; i<N; i++ ) // 往图中增加边

{

CurveFittingEdge* edge = newCurveFittingEdge( x_data[i] );

edge->setId(i);

edge->setVertex( 0, v ); // 设置连接的顶点

edge->setMeasurement( y_data[i] ); // 观测数值

edge->setInformation( Eigen::Matrix< double, 1, 1>::Identity()* 1/(w_sigma*w_sigma) ); // 信息矩阵：协⽅差矩阵之逆optimizer.addEdge( edge );

}

gg-238// 第6步：设置优化参数，开始执⾏优化

optimizer.initializeOptimization();

optimizer.optimize( 100);

(左右滑动试试)

结合上⾯的流程图和代码。下⾯⼀步步解释具体步骤。

1、创建⼀个线性求解器LinearSolver

我们要求的增量⽅程的形式是：H△X=-b，通常情况下想到的⽅法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较⼩，此时只需要矩阵的求逆就能解决问题。但是当H的维度较⼤时，矩阵求逆变得很困难，求解问题也变得很复杂。

⼩⽩：那有什么办法吗？

师兄：办法肯定是有的。此时我们就需要⼀些特殊的⽅法对矩阵进⾏求逆，你看下图是GitHub上g2o相关部分的代码

如果你点进去看，可以分别查看每个⽅法的解释，如果不想挨个点进去看，看看下⾯我的总结就⾏了

LinearSolverCholmod ：使⽤sparse cholesky分解法。继承⾃LinearSolverCCS

LinearSolverCSparse：使⽤CSparse法。继承⾃LinearSolverCCS

LinearSolverPCG ：使⽤preconditioned conjugate gradient 法，继承⾃LinearSolver

LinearSolverDense ：使⽤dense cholesky分解法。继承⾃LinearSolver

LinearSolverEigen：依赖项只有eigen，使⽤eigen中sparse Cholesky 求解，因此编译好后可以⽅便的在其他地⽅使⽤，性能和CSparse差不多。继承⾃LinearSolver

2、创建BlockSolver。并⽤上⾯定义的线性求解器初始化。

BlockSolver 内部包含 LinearSolver，⽤上⾯我们定义的线性求解器LinearSolver来初始化。它的定义在如下⽂件夹内：g2o/g2o/core/block_solver.h

你点进去会发现 BlockSolver有两种定义⽅式

⼀种是指定的固定变量的solver，我们来看⼀下定义

usingBlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

其中p代表pose的维度（注意⼀定是流形manifold下的最⼩表⽰），l表⽰landmark的维度十三经索引

本文发布于:2024-09-22 03:40:03，感谢您对本站的认可！

本文链接：https://www.17tex.com/xueshu/53834.html

上一篇：面向像素式拓扑优化结果的可移动变形组件转换优化方法

下一篇：SLAM练习题（十）——图优化G2O

标签：优化顶点求解位姿编程需要滤波

留言与评论（共有 0 条评论）