g2o函数

g2o简介

g2o(General Graphical Optimization)，是一个在SLAM领域广为使用的优化库。基于图优化理论。

图优化理论

把优化问题表现成图的一种方式。一个图由若干个顶点和连接着这些顶点的边组成。
用顶点表示优化变量，用边表示误差项。对于一个非线性最小二乘问题，我们可以为其构建一个图。

主要步骤

• 定义顶点和边的类型
• 构建图
• 选择优化算法
• 调用g2o进行优化，返回结果

g2o函数介绍

Vertex是优化的变量

settoOriginImpl()用来初始化
oplusImpl()用来更新
在Edge的定义中，传入_vertices中。

Edge是误差项

computeError()：定义误差项Error
linearizeOplus：求雅可比矩阵。_jacobianOplusXi是指第一个绑定的节点，_jacobianOplusXj为第二个。

使用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/xxx/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})