G2O

一、概念

G2O(General Graph Optimization)是通用图优化计算库，能将优化问题表达成图，就可以使用G2O进行求解，如常见的Bundle Adjustment，ICP，数据拟合等。

图优化模型中，将待优化的变量作为顶点，条件信息作为边。如上图中，可以解释为三个顶点为不同时刻待优化估计的传感器的位姿，各个边表示传感器位姿量测信息，最终需要估计各个顶点的最优结果。

二、安装使用

#g2o安装
#去github下载工程，并使用cmake编译安装
git clone https://github.com/RainerKuemmerle/g2o.git
mkdir build
cd build
cmake ..
make
sudo make install

# g2o的使用，以下CMakeLists.txt文件中要添加的内容
# g2o库为非标准库，需要向CMAKE_MODULE_PATH中添加FindG2O.cmake才能后序使用find_package找到相关文件
LIST( APPEND CMAKE_MODULE_PATH /home/zhangph/data/Tools/g2o/cmake_modules/  ) 
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})
target_link_libraries( demo_g2o
    ${OpenCV_LIBS} 
    ${G2O_CORE_LIBRARY} 
    ${G2O_STUFF_LIBRARY}
)

三、使用例程

以上为g2o库的核心类的关系图，对照上图在使用过程中，主要分为以下几步：

构建求解器
构建优化器
添加顶点和边
启动优化

cmake_minimum_required(VERSION 3.0.0)
project(demo_g2o VERSION 0.1.0)

#设置输出core_dump
add_definitions(" -g")
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# g2o
LIST( APPEND CMAKE_MODULE_PATH /home/zhangph/data/Tools/g2o/cmake_modules/  ) 
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

# Eigen
include_directories("/usr/include/eigen3")

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

#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>

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

//生成随机观测
void getRndData(double para[], int N, double w_sigma, vector<double>&x_data, vector<double>&y_data){ 
   
    double ar = para[0];
    double br = para[1];
    double cr = para[2];
    cv::RNG rng;//opencv中的随机数生成器

    for (size_t 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));
    }
    
}

int main(int argc, char** agrv) { 
   
    cout << "G2O Test Demo!"<<endl;

    //0.生成随机数据 y = exp(ax^2 + bx +c )采样点会受到噪声影响
    double para[3] = { 
   1.0, 2.0, 1.0};
    int N = 100;
    double w_sigma = 1.0;
    vector<double>x_data, y_data;
    getRndData(para, N, w_sigma, x_data, y_data);
    // cout<<x_data.at(1)<<y_data.at(1)<<endl;
    // cout<<x_data.at(2)<<y_data.at(2)<<endl;
    //-----------------------------使用g2o进行图优化计算------------------------//
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
    //1.创建线性求解器 LinearSolver，
    //2.创建BlockSolver，使用上面的线性求解器初始化
    //3.创建总求解器solver，并从GN,LN,Dogleg中选择一个，再用上述BlockSolver初始化
    auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    //4.创建稀疏优化器
    g2o::SparseOptimizer opt;//图模型
    opt.setAlgorithm(solver);//设置求解器
    opt.setVerbose(true);//打开调试输出
    //5.定义待优化的图的顶点和边，添加到图模型中
    //顶点指待优化对象，这里是三个系数abc，由于它们不时变，因而只需要一个顶点
    CurveFittingVertex *v = new CurveFittingVertex();
    v->setEstimate(Eigen::Vector3d(2, -1, 5));//(0, 0, 0)为迭代初始值
    v->setId(0);
    opt.addVertex(v);
    //一条边表示对相关顶点的一次观测
    for (size_t 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)); // 信息矩阵：协方差矩阵之逆
        opt.addEdge(edge);
    }

    //6.执行优化
    opt.initializeOptimization();//优化初始化
    opt.optimize(10);//设置迭代次数
    
    //7. 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

}