使用g2o进行图优化

G2O(General Graphic Optimization)

是一个用来优化非线性误差函数的c++开源库.

安装过程参考官网即可.

1 g2o框架

 由以下5个部分构成:

(1)SparseOptimizer是整个图的核心,找到它向上走,它是一个Optimizable Graph 也就是一个超图HyperGraph,所以在使用时需要添加顶点和边.
(2)顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）,顶点继承自 Base Vertex,边可以继承自BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边）.
(3)整个框架的上部分就说的顶点和边(图的结构),下部分就是优化算法.SparseOptimizer包含一个优化算法OptimizationAlgorithm的对象,通过OptimizationWithHessian 来实现的.迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell’s dogleg 三者中间选择一个.
(4)OptimizationWithHessian 内部包含一个求解器Solver,Solver实际是由一个BlockSolver组成的.
(5)BlockSolver有两个部分，一个是SparseBlockMatrix ，用于计算稀疏的雅可比和Hessian矩阵；一个是线性方程的求解器LinearSolver，它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod.

2 g2o优化流程

由于定义时需要初始化,所以应该从底向上初始化.

(1)创建一个线性求解器LinearSolver
(2)创建BlockSolver。并用上面定义的线性求解器初始化
(3)创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
(4)创建SparseOptimizer 稀疏优化器
(5)定义图的顶点和边。并添加到SparseOptimizer中
(6)设置优化参数，开始执行优化

3 二维数据优化例子

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

    /*创建线性求解器,solvers文件夹中*/
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

    /*创建BlockSolver,在/core/block_solver.h中,可定义数据维度*/
    Block* solver_ptr = new Block(linearSolver);

    /*创建总求解器solver。并从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 );

    /*创建SparseOptimizer 稀疏优化器, 设置求解方法*/
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);


    /*添加顶点SE2*/
    for (size_t i = 0; i < Vertexs.size(); i++) {
        VertexSE2* v = new VertexSE2();
        v->setEstimate(Vertexs[i]);
        v->setId(i);
        if (i == 0) {
            v->setFixed(true);
        }
        optimizer.addVertex(v);
    }

    /*添加边*/
    for (size_t i = 0; i < Edges.size(); i++) {
        EdgeSE2* edge = new EdgeSE2();

        Edge tmpEdge = Edges[i];

        edge->setId(i);
        edge->setVertex(0, optimizer.vertices()[tmpEdge.xi]);
        edge->setVertex(1, optimizer.vertices()[tmpEdge.xj]);

        edge->setMeasurement(tmpEdge.measurement);
        edge->setInformation(tmpEdge.infoMatrix);
        optimizer.addEdge(edge);
    }

    /*设置优化参数 并求解*/
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    SparseOptimizerTerminateAction* terminateAction = new SparseOptimizerTerminateAction;
    terminateAction->setGainThreshold(1e-4);
    optimizer.addPostIterationAction(terminateAction);
    optimizer.optimize(100);

    /*取出求解结果*/
    for (size_t i = 0; i < Vertexs.size(); i++) {
        VertexSE2* v = static_cast<VertexSE2*>(optimizer.vertices()[i]);
        Vertexs[i] = v->estimate().toVector();
    }