G2O的使用步骤_linearsolver 和 blocksolver-CSDN博客

1.G2O使用步骤

使用步骤如上图中的红色字体

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

基于图优化的SLAM一般都是用来求解增量方程H△X=b，而求解增量方程最简单的方法是直接求解雅克比矩阵H的逆即：

△X=-H.inv*b

但是由于当雅克比矩阵维度较大时，矩阵求逆就会很困难，因此这一步主要利用什么方法进行求解增量方程，从图中可得，一般有三类方法PCG，CSparse，Sparse Cholesky，具体：

LinearSolverCholmod ：使用Sparse Cholesky分解法。继承自LinearSolverCCS

LinearSolverDense ：使用Dense Cholesky分解法。继承自LinearSolver

LinearSolverEigen：依赖项只有eigen，使用eigen中Sparse Cholesky 求解

LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS

LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver

第2步创建BlockSolver

BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化；这步可以认为是设置顶点的类型。BlockSolver里面有两种定义方式，一种是指定的固定变量的solver，如下：

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

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

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

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

比较常用的有这几种：

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

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

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

第3步创建总求解器solver

这一步可以理解为采用什么方法进行最小二成优化求解增量；一般的方法有高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，并用上免得求解器BlockSolver初始化

第4步 创建稀疏优化器SparseOptimizer

创建稀疏优化器，并采用已定义求解器作为求解方法

具体代码如下

    // Step 1 初始化g2o优化器
    g2o::SparseOptimizer optimizer;
    // H△X=b增量方程，linearSolver就是采用什么方法求解增量方程
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    // LinearSolverEigen 依赖项只有eigen，使用eigen中sparse Cholesky 求解，性能和CSparse差不多
    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
    // BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化。
    // 定义增量方程中的自变量X含义 BlockSolver_6_3 表示pose 是6维，观测点是3维
    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    // 选用LM算法进行最小二乘优化，通过多步迭代计算最好的增量
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

下次记录顶点和边的使用.....