g2o函數

g2o簡介

g2o(General Graphical Optimization)，是一個在SLAM領域廣爲使用的優化庫。基於圖優化理論。

圖優化理論

把優化問題表現成圖的一種方式。一個圖由若干個頂點和連接着這些頂點的邊組成。
用頂點表示優化變量，用邊表示誤差項。對於一個非線性最小二乘問題，我們可以爲其構建一個圖。

主要步驟

• 定義頂點和邊的類型
• 構建圖
• 選擇優化算法
• 調用g2o進行優化，返回結果

使用g2o擬合曲線

#include<iostream>
#include<g2o/core/g2o_core_api.h>
#include<g2o/core/base_vertex.h>
#include<g2o/core/base_unary_edge.h>
#include<g2o/core/block_solver.h>
#include<g2o/core/optimization_algorithm_levenberg.h>
#include<g2o/core/optimization_algorithm_gauss_newton.h>
#include<g2o/core/optimization_algorithm_dogleg.h>
#include<g2o/solvers/dense/linear_solver_dense.h>
#include<Eigen/Core>
#include<opencv2/core/core.hpp>
#include<cmath>
#include<chrono>

using namespace std;

//曲線模型的頂點，模板參數：優化變量維度和數據類型
class CurveFittingVertex:public g2o::BaseVertex<3,Eigen::Vector3d>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    //重置
    virtual void setToOriginImpl()override{
        _estimate<<0,0,0;
    }
    
    //更新
    virtual void oplusImpl(const double *update)override{
        _estimate+=Eigen::Vector3d(update);
    }
    
    //存盤和讀盤：留空
    virtual bool read(istream &in){}
    virtual bool write(ostream &out)const{}
};

//誤差模型 模板參數：觀測值維度，類型，連接頂點類型
class CurveFittingEdge:public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>{
public :
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    CurveFittingEdge(double x):BaseUnaryEdge(),_x(x){}
    
    //計算曲線模型誤差
    virtual void computeError()override{
        const CurveFittingVertex *v=static_cast<const CurveFittingVertex *>(_vertices[0]);
        const Eigen::Vector3d abc=v->estimate();
        _error(0,0)=_measurement-std::exp(abc(0,0)*_x*_x+abc(1,0)*_x+abc(2,0));
    }
    //計算雅可比矩陣
    virtual void linearizeOplus()override{
        const CurveFittingVertex *v=static_cast<const CurveFittingVertex *>(_vertices[0]);
        const Eigen::Vector3d abc=v->estimate();
        double y=exp(abc[0]*_x*_x+abc[1]*_x+abc[2]);
        _jacobianOplusXi[0]=-_x*_x*y;
        _jacobianOplusXi[1]=-_x*y;
        _jacobianOplusXi[2]=-y;
    }
    
    virtual bool read(istream &in){}
    virtual bool write(ostream &out)const{}
public:
    double _x;//x值，y值爲_measurement
};

int main(int argc,char **argv){
    double ar=1.0,br=2.0,cr=1.0;        //真實參數值
    double ae=2.0,be=-1.0,ce=5.0;       //估計參數值
    int N=100;
    double w_sigma=1.0;                 //噪聲sigma
    double inv_sigma=1.0/w_sigma;
    cv::RNG rng;                        //OpenCV隨機數生成
    
    vector<double> x_data,y_data;   //數據
    for(int i=0;i<N;i++){
        double x=i/100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma*w_sigma));
    }
    //構建圖優化，先設定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,1>>BlockSolverType;//每個誤差項優化變量維度爲3，誤差值維度爲1
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType>LinearSolverType;//線性求解器類型


    //梯度下降方法，可以從GN、LM、DogLeg中選
    auto solver=new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;  //圖模型
    optimizer.setAlgorithm(solver); //設置求解器
    optimizer.setVerbose(true);     //打開調試輸出
    
    //往圖中增加頂點
    CurveFittingVertex *v=new CurveFittingVertex();
    v->setEstimate(Eigen::Vector3d(ae,be,ce));
    v->setId(0);
    optimizer.addVertex(v);
    
    //往圖中增加邊
    for(int i=0;i<N;i++){
        CurveFittingEdge *edge=new CurveFittingEdge(x_data[i]);
        edge->setId(i);
        edge->setVertex(0,v);            //設置連接的頂點
        edge->setMeasurement(y_data[i]); //觀測數值
        edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));//信息矩陣：協方差矩陣之逆
        optimizer.addEdge(edge);
    }
    //執行優化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1=chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(10);//這個參數推測是迭代次數
    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<<"estimate model: "<<abc_estimate.transpose()<<endl;
    
    return 0;
}

CMakeLists.txt
注意G2O_CORE_LIBRARY的寫法，否則可能會出現XXX未定義的引用

cmake_minimum_required(VERSION 2.8)
project(ch6_3)

set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
list(APPEND CMAKE_MODULE_PATH /home/yufan/MyLibs/g2o-master/cmake_modules)
set(G2O_ROOT/usr/local/include/g2o)
set(G2O_LIBS/usr/local/include/g2o)

include_directories("/usr/include/eigen3")

#OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(g2oCurveFitting g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${G2O_STUFF_LIBRARY} ${G2O_CORE_LIBRARY} ${OpenCV_LIBS})