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);//这个能将误差全局平摊