g2o：如何使⽤g2o的例⼦
g2o 重要的⽂献:
论⽂ Grisetti, Giorgio, et al. “A tutorial on graph-based SLAM.
A tutorial on graph-based SLAM.” IEEE Intelligent Transportation Systems Magazine 2.4
1.论⽂
(2010): 31-43.
g2o: A general framework for graph optimization.” Robotics and Automation
2.论⽂
论⽂ Kümmerle, Rainer, et al. “g2o: A general framework for graph optimization
(ICRA), 2011 IEEE International Conference on. IEEE, 2011.
Least Squares and SLAM Pose-SLAM
课件 Least Squares and SLAM Pose-SLAM
3.课件
g2o代码⽰例
1. simple_optimize
⾸先, 看效果
数据集可以在 下载, 详见.
包括 city10000, intel, manhattan, ring, ringCity, sphere2500.
先测试sphere2500.
运⾏后
测试manhattanOlson3500.
运⾏后
sphere2500 和manhattanOlson3500的维度不⼀样，故⽽给出两次测试.其次, 看代码
此处的代码是sphere2500测试.
simple_optimize.cpp
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "g2o/types/slam2d/vertex_se2.h"
#include "g2o/types/slam3d/vertex_se3.h"
//#include "g2o/types/slam3d/edge_se3.h"
G2O_USE_TYPE_GROUP(slam3d);
G2O_USE_TYPE_GROUP(slam2d);
#include <iostream>
using namespace std;
using namespace g2o;
#define MAXITERATION 10
int main()
{
// create the linear solver
BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();
// create the block solver on the top of the linear solver
BlockSolverX* blockSolver = new BlockSolverX(linearSolver);
//create the algorithm to carry out the optimization
OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);
// create the optimizer
SparseOptimizer optimizer;
if(!optimizer.load("../data/sphere_bignoise_vertex3.g2o"))
// if(!optimizer.load("../data/manhattanOlson3500.g2o"))
{
cout<<"Error loading graph"<<endl;
return -1;
}else
{
cout<<"Loaded "<<optimizer.vertices().size()<<" vertices"<<endl;
cout<<"Loaded "<<optimizer.edges().size()<<" edges"<<endl;
}
/
/ VertexSE2* firstRobotPose = dynamic_cast<VertexSE2*>(optimizer.vertex(0));
VertexSE3* firstRobotPose = dynamic_cast<VertexSE3*>(optimizer.vertex(0));
firstRobotPose->setFixed(true);
optimizer.setAlgorithm(optimizationAlgorithm);
optimizer.setVerbose(true);
optimizer.initializeOptimization();
cerr<<"Optimizing ..."<<endl;
optimizer.optimize(MAXITERATION);
cerr<<"done."<<endl;
optimizer.save("../data/sphere_after.g2o");
// optimizer.save("../data/manhattanOlson3500_after.g2o");
optimizer.clear();
return 0;
}
cmake如何使用代码中需要注意的地⽅,
1. G2O_USE_TYPE_GROUP 保证能够进graph的load 和save, 进⾏类型注册, 也就是optimizer.load 和optimizer.save 在执⾏时有相对
的类型对应. slam3d 和slam2d是对应不同的顶点和边的类型. 这⾥都registration.
G2O_USE_TYPE_GROUP(slam3d);
G2O_USE_TYPE_GROUP(slam2d);
1
2
1. MAXITERATION 执⾏优化次数
2. VertexSE3* firstRobotPose
此处本来是设定不优化的固定点的，但是考虑不同的维度的顶点需要采⽤不同的类型设置固定点，所以在此提出.
<
cmake_minimum_required(VERSION 2.8)
project(simple_optimize)
#Enable support for C++11
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
#set binary path
SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set lib path
SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#set .cmake path
#SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "{CMAKE_CURRENT_SOURCE_DIR}/cmake_modules")
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
#ADD_SUBDIRECTORY(${PROJECT_SOURCE_DIR}/src)
# find g2o lib
find_package(G2O REQUIRED)
IF(G2O_FOUND)
include_directories(${G2O_INCLUDE_DIR})
message("G2O lib is found:"${G2O_INCLUDE_DIR})
ENDIF(G2O_FOUND)
find_package(Eigen3 REQUIRED)
find_package(CSparse REQUIRED)
find_package(Cholmod REQUIRED)
include_directories(${CSPARSE_INCLUDE_DIR})
include_directories(${EIGEN3_INCLUDE_DIR})
include_directories(${CHOLMOD_INCLUDE_DIR})
SET(G2O_LIBS g2o_cli g2o_ext_freeglut_minimal g2o_simulator g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d g2o_core g2o_interface g
ADD_EXECUTABLE(simple_optimize simple_optimize.cpp)
target_link_libraries(simple_optimize ${G2O_LIBS})
⽽对于manhattanOlson3500的测试代码, 三维到⼆维仅仅需要修改3处
1. optimizer.load()
2. optimizer.save()
3. VertexSE3* firstRobotPose 成VertexSE2* firstRobotPose
最后, 介绍使⽤过程
为⽅便使⽤，也为⾃⼰存档, CSDN下载地址:

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。