SLAM::g2o学习例程(一)曲线拟合
使用g2o库对曲线 进行拟合
g2o库使用的是截至2019年7月10日github上的最新版本
直接上代码,带注释
#include <iostream>
#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "g2o/core/factory.h"
//#include "g2o/types/slam3d/types_slam3d.h"
//#include "g2o/types/slam2d/types_slam2d.h"
#include "g2o/stuff/command_args.h"
#include "Eigen/Core"
#include "opencv2/core/core.hpp"
#include <cmath>
#include <chrono>
using namespace std;
using namespace g2o;
//先定于曲线模型的顶点模板
//<3表示一个顶点中待优化的参数个数,Eigen::Vector3d表示这些待估计变量放在一个顶点中的存储类型>
class CurveFittingVertex:public g2o::BaseVertex<3,Eigen::Vector3d>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() //纯虚函数重载
{
_estimate<<0,0,0;
}
virtual void oplusImpl(const double* update) //更新
{
_estimate+=Eigen::Vector3d(update);
}
// 存和读 纯虚函数
virtual bool read(istream &in){}
virtual bool write(ostream &out) const {}
};
//假设曲线模型为 y=ax^2+bx+c
//定义边的模板 :观测值维度,类型,连接顶点类型
class CurveFittingEdge:public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x):BaseUnaryEdge(),_x(x){} //对父类BaseUnaryEdge初始化,同时初始化成员变量_x
//误差函数模型 重载
void computeError(){
//_vertices是VertexContainer顶点容器类型
const CurveFittingVertex* v=static_cast<const CurveFittingVertex*>(_vertices[0]);
const Eigen::Vector3d abc=v->estimate();
//abc(0,0)表示[v1,v2,v2]^T中的v1
//abc(1,0)表示[v1,v2,v2]^T中的v2
//abc(2,0)表示[v1,v2,v2]^T中的v3
//误差则为测量值y_hat-y 这里为什么有个exp?
_error(0,0)=_measurement-std::exp(abc(0,0)*_x*_x+abc(1,0)*_x+abc(2,0));
}
virtual bool read(istream &in){}
virtual bool write(ostream &out) const{}
public:
double _x; //x值
};
int main()
{
cout << "Hello G2O" << endl;
//定义曲线真实参数
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点个数
double w_sigma=0.1; //噪声
cv::RNG rng; //opencv随机数
//double abc[3]={0,0,0}; //abc参数(待优化值)
vector<double> x_data,y_data;
cout<<"生成数据:"<<endl;
for(int i=0;i<N;i++){
double x=i/100.0;
x_data.push_back(x);
y_data.push_back(
exp(a*x*x+b*x+c)+rng.gaussian(w_sigma));
cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
//创建线性求解器
auto linearSolver=make_unique<LinearSolverCSparse<BlockSolverX::PoseMatrixType>>();
//创建块求解器
auto blockSolver=make_unique<BlockSolverX>(std::move(linearSolver));
//选择优化方法
OptimizationAlgorithmLevenberg * optimizationAlgorithm=
new OptimizationAlgorithmLevenberg(std::move(blockSolver));
//创建图模型 设置优化方法
SparseOptimizer optimizer;
optimizer.setVerbose(true);
optimizer.setAlgorithm(optimizationAlgorithm);
//加入顶点
CurveFittingVertex*v =new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(0,0,0));
v->setId(0);
optimizer.addVertex(v);
//加入边
for(int i=0;i<N;i++){
//使用模拟的x值构造边对象
CurveFittingEdge *edge=new CurveFittingEdge(x_data[i]);
//边的id
edge->setId(i);
//边的链接对象,这里只添加了一个顶点v,因此另一端链接起始顶点0
edge->setVertex(0,v);
//设定边的观测值,即赋值_measurement=y_data[i]
edge->setMeasurement(y_data[i]);
// 信息矩阵:协方差矩阵之逆
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) );
//添加边
optimizer.addEdge(edge);
}
//开始优化
cout<<"开始执行优化"<<endl;
chrono::steady_clock::time_point t1=chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(100);//迭代,参数为迭代次数
chrono::steady_clock::time_point t2=chrono::steady_clock::now();
chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
//输出优化值
Eigen::Vector3d abc_estimate=v->estimate();
cout<<"a b c is"<<abc_estimate.transpose()<<endl;
return 0;
}
CMakelists.txt:
cmake_minimum_required(VERSION 2.8)
project(g2o_curveFitting)
add_executable(${PROJECT_NAME} "main.cpp")
#启用c++11
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
include_directories(${CSPARSE_INCLUDE_DIR})
#寻找g2o库
LIST( APPEND CMAKE_MODULE_PATH /usr/local/include/g2o/cmake_modules )
SET( G2O_ROOT /usr/local/include/g2o )
find_package(G2O REQUIRED)
IF(G2O_FOUND)
include_directories(${G2O_INCLUDE_DIR})
message("G2O lib is found:" ${G2O_INCLUDE_DIR})
ENDIF(G2O_FOUND)
#寻找EIGEN3
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
#寻找CSparse
find_package(CSparse REQUIRED)
include_directories(${CSPARSE_INCLUDE_DIR})
#寻找opencv库
find_package(OpenCV REQUIRED)
message(STATUS ${OpenCV_INCLUDE_DIRS})
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 g2o_solver_csparse g2o_solver_structure_only g2o_types_sba g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data g2o_types_sim3 cxsparse )
target_link_libraries(${PROJECT_NAME} ${G2O_LIBS})
#链接Opencv库
target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS})