G2O initial guess 方法的选择

G2O initial guess 方法的选择

---

目录

• G2O initial guess 方法的选择
• • 〇、前言
  • 一、查看对应源码
  • 二、编程验证
  • • 2.1 g2o_test.cpp
    • 2.2 CMakeLists.txt

---

〇、前言

由于不同优化初始化方法会对g2o的结果产生影响，如下图所示，在勾选Robust Kernel的条件下:using initial guess from spanning tree 和 using initial guess from odometry 在其他设置相同的情况下得到的优化结果差别很大。因此我们希望可以通过程序来选择合适的优化初值设置方法。

PS：这里产生差异的原因和是否勾选Non Sequential也相关，用spanning tree初始化时若勾选Non Sequential会得到与左图相同的结果，而No Sequential的意思是只把回环边设置为鲁棒核

一、查看对应源码

由于没有搜索到相关资料，所以最好的方式就是查看g2o_viewer的源码。(同样的事已经做过一次，只是记性不好，这也是写这篇博客的原因)。

首先通过QT Designer查到对应下拉框的名字叫cbxIniitialGuessMethod，按钮btnInitialGuess

再找到对应的响应函数，然后就是我们需要的代码。

void MainWindow::on_btnInitialGuess_clicked()
{
  if (viewer->graph->activeEdges().size() == 0)
    viewer->graph->initializeOptimization();

  switch (cbxIniitialGuessMethod->currentIndex()) {
    case 0:
      // spanning tree
      viewer->graph->computeInitialGuess();
      break;
    case 1:
      // odometry
      {
        EstimatePropagatorCostOdometry costFunction(viewer->graph);
        viewer->graph->computeInitialGuess(costFunction);
      }
      break;
    default:
      cerr << __PRETTY_FUNCTION__ << " Unknown initialization method" << endl;
      break;
  }
  viewer->setUpdateDisplay(true);
  viewer->update();
}

二、编程验证

2.1 g2o_test.cpp

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <vector>
#include <numeric>
#include <sstream>
//用于构建和计算图优化 g2o
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/core/eigen_types.h>
#include <g2o/types/slam3d/isometry3d_mappings.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/estimate_propagator.h>

using namespace std;
int main(int argc,char** argv)
{
    int N = 0;
    if ( argc != 3 )
    {
        cout<<"Usage: pose_graph_g2o_SE3_lie sphere.g2o"<<endl;
        return 1;
    }

    ifstream fin ( argv[1] );
    if ( !fin )
    {
        cout<<"file "<<argv[1]<<" does not exist."<<endl;
        return 1;
    }


    stringstream ss;
    ss<< argv[2];
    ss>> N;
    cout<<"result file will be saved in result"<<N<<".g2o"<<endl;



    typedef g2o::BlockSolver<g2o::BlockSolverTraits<-1,-1>> Block; //6x6 BlockSolver


    // 线性方程求解器
    //Block::LinearSolverType* linearSolver=new g2o::LinearSolverCholmod<Block::PoseMatrixType>();
    //unique_ptr<Block::LinearSolverType> linearSolver(new g2o::LinearSolverCholmod<Block::PoseMatrixType>());
    auto linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<Block::PoseMatrixType>>();
    linearSolver->setBlockOrdering(true);


    // 矩阵块求解器
    //unique_ptr<Block> solver_ptr(new Block(std::move(linearSolver)));
    cout<<"blockOrdering = "<<linearSolver->blockOrdering()<<endl;

    // 梯度下降方法 GN LM DogLeg
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<Block>(std::move(linearSolver)));


    solver->printProperties(cout);
    // 图模型
    g2o::SparseOptimizer optimizer;
    optimizer.setVerbose(true);
    optimizer.setAlgorithm(solver);

    vector<g2o::VertexSE3*> vectices;
    vector<g2o::EdgeSE3*> edges;
    //vector<g2o::EdgeSE3*> odometryEdges;
    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量


    //加载文件
    while ( !fin.eof() )
    {
        string name;
        fin>>name;
        if ( name == "VERTEX_SE3:QUAT" )
        {
            // 顶点
            g2o::VertexSE3* v = new g2o::VertexSE3();
            int index = 0;
            fin>>index;
            v->setId( index );
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            vectices.push_back(v);
            if ( index==0 )
                v->setFixed(true);
        }
        else if ( name=="EDGE_SE3:QUAT" )
        {
            // SE3-SE3 边
            g2o::EdgeSE3* e = new g2o::EdgeSE3();
            int idx1, idx2;     // 关联的两个顶点
            fin>>idx1>>idx2;
            e->setId( edgeCnt++ );
            e->setVertex( 0, optimizer.vertices()[idx1] );
            e->setVertex( 1, optimizer.vertices()[idx2] );
            e->read(fin);
            optimizer.addEdge(e);
            edges.push_back(e);
        }
        if ( !fin.good() ) break;
    }

    cout<<"read total "<<vertexCnt<<" vertices, "<<edgeCnt<<" edges."<<endl;



    //设置鲁棒核函数
    bool onlyLoop = false; //对应 No Sequential
    for (g2o::SparseOptimizer::EdgeSet::const_iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it)
    {
        g2o::OptimizableGraph::Edge* e = static_cast<g2o::OptimizableGraph::Edge*>(*it);
        if (onlyLoop)
        {
            if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1)
            {
                e->setRobustKernel(new g2o::RobustKernelCauchy());
                e->robustKernel()->setDelta(1.0);
            }
        }
        else
        {
            e->setRobustKernel(new g2o::RobustKernelCauchy());
            e->robustKernel()->setDelta(1.0);
        }
    }

    // using initial guess from spanning tree
    // optimizer.computeInitialGuess();

    // using initial guess from odometry
    g2o::EstimatePropagatorCostOdometry costFunction(&optimizer);
    optimizer.computeInitialGuess(costFunction);

    optimizer.initializeOptimization();
    optimizer.optimize(30);

    char filename1[80];
    bool status;
    cout<<"save"<<endl;
    sprintf(filename1,"result%d.g2o",N);
    status = optimizer.save(filename1);
    cout<<"status"<< status<<endl;
}

2.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(G2O_test)

set(CMAKE_BUILD_TYPE Debug)

include_directories("/usr/include/eigen3")
include_directories("/usr/include/suitesparse")

find_package( g2o REQUIRED )
include_directories( ${G2O_INCLUDE_DIRS} )

add_executable(g2o_test g2o_test.cpp)
target_link_libraries(g2o_test cholmod g2o_core g2o_stuff g2o_types_slam3d  ${G2O_LIBRARIES})

您的点赞是对我最大的肯定和帮助！