g2o常用模块的简单介绍

g2o常用模块的简单介绍  参考https://blog.csdn.net/qq_29828623/article/details/70158585

https://blog.csdn.net/fb_941219/article/details/107757706#t6

g2o是一个c++编写的项目，用cmake构建。它的github地址在：https://github.com/RainerKuemmerle/g2o
是一个重度模板类的c++项目，其中矩阵数据结构多来自Eigen。

g2o项目中含有若干文件夹。刨开那些gitignore之类的零碎文件，主要有以下几个：

EXTERNAL　　 三方库，有ceres, csparse, freeglut，可以选择性地编译；
cmake_modules　　 给cmake用来寻找库的文件。我们用g2o时也会用它里头的东西，例如FindG2O.cmake
doc　　　　　 文档。包括g2o自带的说明书（难度挺大的一个说明文档）。
g2o　　　　　　 最重要的源代码都在这里！
script　　　　 在android等其他系统编译用的脚本，由于我们在ubuntu下就没必要多讲了。
综上所述，最重要的就是g2o的源代码文件啦！所以我们要进一步展开看一看！

我们同样地介绍一下各文件夹的内容：

apps　　　　一些应用程序。好用的g2o_viewer就在这里。其他还有一些不常用的命令行工具等。
core　　　　核心组件，很重要！基本的顶点、边、图结构的定义，算法的定义，求解器接口的定义在这里。
examples　　一些例程，可以参照着这里的东西来写。不过注释不太多。
solvers　　　　求解器的实现。主要来自choldmod, csparse。在使用g2o时要先选择其中一种。
stuff　　　　对用户来讲可有可无的一些工具函数。
types　　　　各种顶点和边。

其中types文件夹的文件很重要！我们用户在构建图优化问题时，先要想好自己的顶点和边是否已经提供了定义。如果没有，要自己实现。如果有，就用g2o提供的即可。types 的选取，则是 g2o 用户主要关心的内容。然后 core 下面的内容，我们要争取弄的比较熟悉，才能确保使用中出现错误可以正确地应对。

　g2o最基本的类结构是怎么样的呢？我们如何来表达一个Graph，选择求解器呢？我们祭出一张图：

跟着高翔博士的博客走一遍
SparseOptimizer 是我们最终要维护的东西。它是一个Optimizable Graph，从而也是一个Hyper Graph。
SparseOptimizer 含有很多个顶点 （都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

在优化之前，需要指定求解器和迭代算法。
从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell’s dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞；另一个是一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一，用于计算迭代过程中最关键的一步：求解 H Δx = -b。

综上所述，在g2o中选择优化方法一共需要三个步骤：

1) 选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹中定义的东西。
2）选择一个 BlockSolver 。
3）选择一个迭代策略，从GN, LM, Doglog中选。

地址：get clone https://github.com/gaoxiang12/g2o_ba_example

总结一下 g2o 的代码流程

• 步骤一： 创建线性方程求解器，确定分解方法

// 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  
// 创建一个线性求解器 LinearSolver，采用 dense cholesky 分解法
Block::LinearSolverType* linearSolver 
    = new g2o::LinearSolverDense<Block::PoseMatrixType>(); 

•  

要求解的增量方程为 H Δ x = − b H \Delta x=-bHΔx=−b，通常想到的方法就是直接求逆Δ x = − H − 1 ∗ b \Delta x=-H^{-1} * bΔx=−H−1∗b，对于较小维度的 H 矩阵可以直接求逆，但当 H 维度较大时采用下面的方法进行求逆：

• LinearSolverCholmod：使用 sparse cholesky 分解法，继承自LinearSolverCCS；
• LinearSolverCSparse：使用 CSparse 法，继承自LinearSolverCCS；
• LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自 LinearSolver
• LinearSolverDense ：使用 dense cholesky 分解法，继承自 LinearSolver
• LinearSolverEigen： 依赖项只有 eigen，使用 eigen 中 sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和 CSparse 差不多，继承自 LinearSolver；

• 步骤二： 构造线性方程的矩阵块，并用上面定义的线性求解器初始化

Block* solver_ptr = new Block( linearSolver );

•  

BlockSolver 内部包含 LinearSolver，用上面定义的线性求解器 LinearSolver 来初始化，前面已经给定了优化变量的维度；

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

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

•  

优化算法

g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg 
g2o::OptimizationAlgorithmDogleg 

•  
• 步骤四： 创建稀疏优化器（SparseOptimizer）

g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm( solver );   // 用前面定义好的求解器作为求解方法：
optimizer.setVerbose( true );       // 打开调试输出

•  
• 步骤五： 定义图的顶点和边，并添加到 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] );
    // 设置边的 id
    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 );
}

•  
• 步骤六： 设置优化参数，开始执行优化

optimizer.initializeOptimization();
optimizer.optimize(100); // 迭代次数