标签：求解 BlockSolver SparseOptimizer SLAM g2o 优化

SLAM图优化g2o

图优化g2o框架

图优化的英文是 graph optimization 或者 graph-based optimization， “图”其实是数据结构中的graph。凸优化的英文是 convex optimization，这里的“凸”其实是凸函数的意思，所以单从英文就能区分开。

图优化有什么优势？

SLAM的后端一般分为两种处理方法，一种是以扩展卡尔曼滤波（EKF）为代表的滤波方法，一种是以图优化为代表的非线性优化方法。SLAM研究的主流热点几乎都是基于图优化。

滤波方法尤其是EKF方法，在SLAM发展很长的一段历史中一直占据主导地位，早期的大神们研究了各种各样的滤波器来改善滤波效果，SLAM，EKF是必须要掌握的。滤波方法的优缺点：

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

缺点：存储量和状态量是平方增长关系，存储的是协方差矩阵，不适合大型场景。基于视觉的SLAM方案，路标点（特征点）数据很大，滤波方法根本吃不消，所以此时滤波的方法效率非常低。

图优化在视觉SLAM中效率很高吗？

以前都是用滤波方法，因为在图优化里，Bundle Adjustment（后面简称BA）起到了核心作用。大量特征点和相机位姿的BA计算量其实很大，根本没办法实时。

在视觉SLAM中，虽然包含大量特征点和相机位姿，其实BA是稀疏的，稀疏的就好办了，就可以加速了啊！比较代表性的就是2009年，几个大神发表了自己的研究成果《SBA：A software package for generic sparse bundle adjustment》，而且计算机硬件发展也很快，因此基于图优化的视觉SLAM也可以实时了！

图优化是什么？

图优化里的图就是数据结构里的图，一个图由若干个顶点（vertex），以及连接这些顶点的边（edge）组成。

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

在SLAM里，图优化一般分解为两个任务：

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

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

根据机器人位姿来作为图的顶点，这个位姿可以来自机器人的编码器，也可以是ICP匹配得到的，图的边就是位姿之间的关系。由于误差的存在，实际上机器人建立的地图是不准的，如下图左。通过设置边的约束，使得图优化向着满足边约束的方向优化，最后得到了一个优化后的地图（如下图中所示），与真正的地图（下图右）非常接近。

 

g2o 框架

图优化如何编程实现呢？

在SLAM领域，基于图优化的一个用的非常广泛的库就是g2o，是General Graphic Optimization 的简称，是一个用来优化非线性误差函数的c++框架。

这个g2o怎么用呢？

先安装，其实g2o安装很简单，参考GitHub上官网：

https://github.com/RainerKuemmerle/g2o

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

第一次接触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的整个框架就是按照下图中标的这个顺序来写的。

 

 

 SLAM十四讲中g2o求解曲线参数的例子来说明，源代码地址

https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp

为了方便理解，重新加了注释。如下所示，

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1

// 第1步：创建一个线性求解器LinearSolver

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

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

Block* solver_ptr = new Block( linearSolver );      

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

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

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

g2o::SparseOptimizer optimizer;     // 图模型

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

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

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

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

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

v->setId(0);

optimizer.addVertex( v );

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

{

  CurveFittingEdge* edge = new CurveFittingEdge( 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 );

}

// 第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，定义

 using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

其中p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度

另一种是可变尺寸的solver，定义如下

using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

为何会有可变尺寸的solver呢？

这是因为在某些应用场景， Pose和Landmark在程序开始时并不能确定，此时这个块状求解器就没办法固定变量，此时使用这个可变尺寸的solver，所有的参数都在中间过程中被确定

看block_solver.h的最后，预定义了比较常用的几种类型，如下所示：

BlockSolver_6_3 ：表示pose 是6维，观测点是3维。用于3D SLAM中的BA

BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale

BlockSolver_3_2：表示pose 是3维，观测点是2维

以后遇到了知道这些数字是什么意思就行了

3、创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化

g2o/g2o/core/ 目录下， Solver的优化方法有三种：分别是高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，如下图所示，也和前面的图相匹配

 

 

 上图最后那个OptimizationAlgorithmWithHessian 是干嘛的？

GN、 LM、 Doglet算法内部，都继承自同一个类：OptimizationWithHessian，如下图所示，这也和最前面那个图是相符。

 

 

 看 OptimizationAlgorithmWithHessian，继承自OptimizationAlgorithm，这也和前面的相符。

 

 

 总之，在该阶段，可以选三种方法：

g2o::OptimizationAlgorithmGaussNewton

g2o::OptimizationAlgorithmLevenberg 

g2o::OptimizationAlgorithmDogleg 

4、创建终极大boss 稀疏优化器（SparseOptimizer），用已定义求解器作为求解方法。

创建稀疏优化器

g2o::SparseOptimizer    optimizer;

用前面定义好的求解器作为求解方法：

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

其中setVerbose是设置优化过程输出信息用的

SparseOptimizer::setVerbose(bool verbose)

看一下定义

 

5、定义图的顶点和边。并添加到SparseOptimizer中。

这部分比较复杂，不展开介绍。

6、设置优化参数，开始执行优化。

设置SparseOptimizer的初始化、迭代次数、保存结果等。

初始化

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

设置迭代次数，然后就开始执行图优化了。

SparseOptimizer::optimize(int iterations, bool online)

 

标签：求解,BlockSolver,SparseOptimizer,SLAM,g2o,优化
来源： https://www.cnblogs.com/wujianming-110117/p/14716463.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。