g2o图优化

g2o的基本框架结构

1. 图的核心
   SparseOptimizer是整个图的核心，SparseOptimizer它是一个Optimizable Graph，从而也是一个超图（HyperGraph）

2. 顶点和边
   这个超图包含了许多顶点（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编程流程

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的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。
   需要一些特殊的方法对矩阵进行求逆：https://github.com/RainerKuemmerle/g2o/tree/master/g2o/solvers

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来初始化。它的定义在如下文件夹内：
   BlockSolver有两种定义方式:

//固定变量的solver
//p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度
 using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
 //可变尺寸的solver
 //在某些应用场景，我们的Pose和Landmark在程序开始时并不能确定
 using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

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

    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );

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

//创建稀疏优化器
g2o::SparseOptimizer optimizer;
//用前面定义好的求解器作为求解方法：
optimizer.setAlgorithm( solver );   // 设置求解器
//setVerbose是设置优化过程输出信息用的
optimizer.setVerbose( true );       // 打开调试输出

5. 定义图的顶点和边。并添加到SparseOptimizer中。
g2o定义顶点：https://blog.csdn.net/weixin_42905141/article/details/100827638
g2o定义边：https://blog.csdn.net/weixin_42905141/article/details/100830126

7. 设置优化参数，开始执行优化。

    // 执行优化前的初始化
    //SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)
    optimizer.initializeOptimization();
    开始执行优化,并设置迭代次数
    //SparseOptimizer::optimize(int iterations, bool online)
    optimizer.optimize(100);

参考：https://mp.weixin.qq.com/s/j9h9lT14jCu-VvEPHQhtBw