SLAM：：g2o学习例程（一）曲线拟合

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})