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