g2o小记

1耗时问题

在g2o中有许多Solver可以选择。 实测发现Dense非常非常的慢，慢到不能用。 后面改成Cholmod就非常快。 在使用Cholmod时还需要在cmakelists里加一个lib的依赖 cholmod 这个cmakelists比较乱，可以做很多精简，这里仅仅做个记录

cmake_minimum_required( VERSION 2.8 ) cmake_policy(VERSION 2.8) 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 = 0; for(int i=0; i<1000; 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, 0, 0); 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=1; i<vertices.size(); i++) { VertexSE3* pre = vertices[i-1]; VertexSE3* cur = vertices[i]; Eigen::Isometry3d T = pre->estimate().inverse() * cur->estimate(); EdgeSE3* e = new EdgeSE3; e->setVertex(0,pre); e->setVertex(1,cur); e->setMeasurement(T); Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Zero(); information.block<3,3>(0,0) = 0.01 * Eigen::Matrix3d::Identity(); information.block<3,3>(3,3) = 0.01 * 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()-1]->estimate().inverse() * vertices[0]->estimate(); Eigen::Isometry3d T_delta = Eigen::Isometry3d::Identity(); T_delta.pretranslate(Eigen::Vector3d(1,2,3)); T = T * T_delta;//故意加上偏差 e->setVertex(0, vertices[vertices.size()-1]); e->setVertex(1, vertices[0]); e->setMeasurement(T); Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Zero(); information.block<3,3>(0,0) = 0.01 * Eigen::Matrix3d::Identity(); information.block<3,3>(3,3) = 0.01 * 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(10); return 0; }

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