g2o小記

1耗時問題

在g2o中有許多Solver可以選擇。

實測發現Dense非常非常的慢，慢到不能用。

後面改成Cholmod就非常快。

在使用Cholmod時還需要在cmakelists裡加一個lib的依賴 cholmod

這個cmakelists比較亂，可以做很多精簡，這裡僅僅做個記錄

cmake_minimum_required( VERSION  )
cmake_policy(VERSION )
project ( g2o_test )

set( CMAKE_CXX_COMPILER "g++" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3 " )

list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
############### dependencies ######################
# Eigen
include_directories( "/usr/include/eigen3" )

# G2O
find_package( G2O REQUIRED )
find_package( Cholmod REQUIRED )
include_directories( ${G2O_INCLUDE_DIRS} )
find_package( CSparse REQUIRED )

include_directories(
    ${G2O_INCLUDE_DIRS}
    ${CSPARSE_INCLUDE_DIR}
    ${Cholmod_INCLUDE_DIR}
    )
message("include_dir = " ${G2O_CORE_LIBRARY_DEBUG})
message("include_dir = " ${Cholmod_INCLUDE_DIR})
set( THIRD_PARTY_LIBS 
    ${Cholmod_LIBRARIES}
  #  ${G2O_CORE_LIBRARY_DEBUG}
 #   ${G2O_STUFF_LIBRARY_DEBUG}
  #  ${G2O_TYPES_SLAM3D_DEBUG}
  #  ${G2O_TYPES_SBA_DEBUG}
    cholmod

    g2o_types_slam3d
    g2o_types_sba
    g2o_core
    g2o_stuff
    g2o_solver_dense.so
    g2o_solver_cholmod.so
)
add_executable( mytest mytest.cpp )
target_link_libraries( mytest
    ${THIRD_PARTY_LIBS}
)
）
           

test.cpp 自己建構了一個閉環，然後進行優化

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <Eigen/Geometry>

#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/core/factory.h"

#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/core/optimization_algorithm_levenberg.h"

using namespace std;
using namespace g2o;
using namespace Eigen;

int main()
{

    g2o::SparseOptimizer optimizer;//全局優化器
    optimizer.setVerbose(true);//調試資訊輸出
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;//6代表pose的自由度 3代表landmark的自由度
    //linearSolver= new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
    linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr
        = new g2o::BlockSolver_6_3(linearSolver);
    //優化方法LM
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);


    vector<VertexSE3*> vertices;//pose
    vector<EdgeSE3*> odometryEdges;
    vector<EdgeSE3*> edges;

    //設定pose
    int id = ;
    for(int i=; i<; i++)
    {
        VertexSE3* v = new VertexSE3;
        v->setId(id++);

        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();;//旋轉和平移的集合,4*4的矩陣
        Eigen::Matrix3d rot = Eigen::Matrix3d::Identity();
        Eigen::Vector3d trans =  Eigen::Vector3d(i, , );
        T.rotate(rot);
        T.translate(trans);
        //cout << "Transform matrix = \n" << T.matrix() <<endl;

        v->setEstimate(T);
        vertices.push_back(v);
        optimizer.addVertex(v);
    }

    //生成裡程計的邊
    for(int i=; i<vertices.size(); i++)
    {
        VertexSE3* pre = vertices[i-];
        VertexSE3* cur = vertices[i];
        Eigen::Isometry3d T = pre->estimate().inverse() * cur->estimate();

        EdgeSE3* e = new EdgeSE3;
        e->setVertex(,pre);
        e->setVertex(,cur);
        e->setMeasurement(T);
        Eigen::Matrix<double, , > information = Eigen::Matrix<double, , >::Zero();
        information.block<,>(,) =  * Eigen::Matrix3d::Identity();
        information.block<,>(,) =  * Eigen::Matrix3d::Identity();;
        e->setInformation(information);
        odometryEdges.push_back(e);
        edges.push_back(e);
        optimizer.addEdge(e);
    }

    //添加一條首尾相連的邊,從尾巴指向頭
    {
        EdgeSE3* e = new EdgeSE3;
        Eigen::Isometry3d T = vertices[vertices.size()-]->estimate().inverse() * vertices[]->estimate();
        Eigen::Isometry3d T_delta = Eigen::Isometry3d::Identity();
        T_delta.pretranslate(Eigen::Vector3d(,,));
        T = T * T_delta;//故意加上偏差
        e->setVertex(, vertices[vertices.size()-]);
        e->setVertex(, vertices[]);
        e->setMeasurement(T);
        Eigen::Matrix<double, , > information = Eigen::Matrix<double, , >::Zero();
        information.block<,>(,) =  * Eigen::Matrix3d::Identity();
        information.block<,>(,) =  * Eigen::Matrix3d::Identity();;
        e->setInformation(information);
        odometryEdges.push_back(e);
        edges.push_back(e);
        optimizer.addEdge(e);
    }


    optimizer.save("test.g2o");
    optimizer.initializeOptimization();
    optimizer.optimize();

    return ;
}
           

2優化方法選擇

實驗發現，我們的小車跑了700m，在閉環時z軸有幾米誤差，xy誤差在0.5m，使用LM方法，隻會将終點附近的節點位置進行調整，是以會很突兀，而GN方法可以很好的将誤差平攤到全局。

全局視角優化前

全局視角優化前

局部放大優化前

GN優化後

LM優化後

//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);//這個效果不好
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);//這個能将誤差全局平攤