从零开始学习g2o
1. g2o安装：
g2o安装很简单，参考GitHub上官网：
https://github.com/RainerKuemmerle/g2o.
2. g2o的基本框架结构：
2.1 图的核心
SparseOptimizer是整个图的核心，我们注意右上角的 is-a 实心箭头，这个SparseOptimizer它是一个Optimizable Graph，从而也是一个超图（HyperGraph）。
2.2 顶点和边
注意看 has-many 箭头，这个超图包含了许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）。而这些顶点顶点继承自 Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edg 。
在图优化中，顶点代表了要被优化的变量，而边则是连接被优化变量的桥梁(一般指误差函数)
2.3 配置SparseOptimizer的优化算法和求解器
整个图的核心SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。
OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛
顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell’s dogleg 三者中间选择一个（我们常用的是GN和
LM，算法的详细过程可以参考视觉slam14讲中的相关内容）
2.4 如何求解
OptimizationWithHessian 内部包含一个求解器（Solver），这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分，
- 一个是SparseBlockMatrix ，用于计算稀疏的雅可比和Hessian 矩阵；
- 一个是线性方程的求解器（LinearSolver），它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod，具体定义后面会介绍。
3. 实战应用,g2o编程流程
高博在十四讲中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);
结合上面的流程图和代码。下面一步步解释具体步骤。
3.1 创建一个线性求解器LinearSolver
我们要求的增量方程的形式是：H△X=-b，通常情况下想到的方法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。
此时我们就需要一些特殊的方法对矩阵进行求逆，我们看下图是GitHub上g2o相关部分的代码
如果你点进去看，可以分别查看每个方法的解释，如果不想挨个点进去看，看看下面我的总结就行了
| LinearSolverCholmod | 使用sparse cholesky分解法。继承自LinearSolverCCS |
|---|---|
| 使用sparse cholesky分解法。继承自LinearSolverCCS | 使用sparse cholesky分解法。继承自LinearSolverCCS |
| LinearSolverPCG | LinearSolverPCG |
| LinearSolverDense | 使用dense cholesky分解法。继承自LinearSolver |
| 使用dense cholesky分解法。继承自LinearSolver | 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver |
3.2 创建BlockSolver。
并用上面定义的线性求解器初始化BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化。它的定义在如下文件夹内：
g2o/g2o/core/block_solver.h
你点进去会发现 BlockSolver有两种定义方式
一种是指定的固定变量的solver，我们来看一下定义
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
其中p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度。
(pose即是一般情况下需要调整的自变量，landmark即是表示目标函数的应变量。在slam中pose即是需要调整的位姿，landmark代表误差函数)
另一种是可变尺寸的solver，定义如下:
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
另外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.3 创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg
g2o::OptimizationAlgorithmDogleg
3.4 稀疏优化器（SparseOptimizer），并用已定义求解器作为求解方法
创建稀疏优化器：
g2o::SparseOptimizer optimizer;
用前面定义好的求解器作为求解方法：
SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)
其中setVerbose是设置优化过程输出信息用的
SparseOptimizer::setVerbose(bool verbose)
3.5 定义图的顶点和边。并添加到SparseOptimizer中
待续
3.6 设置优化参数，开始执行优化
初始化
SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)
设置迭代次数，然后就开始执行图优化了。
SparseOptimizer::optimize(int iterations, bool online)
参考：
[1] 计算机视觉life