Introduction to g2o Graph Optimization and Basic Usage

I. Introduction to g2o

g2o (General Graph Optimization) is a graph optimization library that combines nonlinear optimization with graph theory. We can use g2o to solve any least-squares problem that can be represented as graph optimization.

Graph optimization represents an optimization problem as a graph. A graph consists of vertices and edges, where vertices represent optimization variables and edges represent error terms. For any nonlinear least-squares problem, we can construct a corresponding graph. (Note: Here, “graph” means a graph in the graph-theoretic sense. It can be defined in probabilistic terms as a Bayesian graph or a factor graph.)

II. Installing g2o

First, install the g2o dependencies.

sudo apt install qt5-qmake qt5-default libqglviewer-dev-qt5 libsuitesparse-dev libcxsparse3 libcholmod3 

Then clone the project from GitHub and compile and install it using the following commands:

git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o/
mkdir build
cd build
cmake ../
make

The g2o header files are under /usr/local/g2o, and the library files are under /usr.local/lib.

III. Using g2o for Curve Fitting

1. Fitting Procedure

① Define the vertex and edge types (optimization variables and error terms) ② Build the graph ③ Select an optimization algorithm ④ Call g2o to perform the optimization and return the result

2. Experiment—Curve Fitting

This example program also depends on OpenCV, Eigen, and Ceres, which must be installed beforehand.

main.cpp file:

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

// 曲线模型的顶点（优化变量）（参数：维度、数据类型）
// 优化变量维数：3维    数据类型：Eigen::Vector3d
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);           // 更新优化变量（估计值）。增量方程计算出增量△x后，通过此函数对估计值进行调整
    }

    //读盘
    virtual bool read(istream &in) {}

    //存盘
    virtual bool write(ostream &out) const {}
};


// 曲线模型的边（误差项）（参数：观测值维度、类型、连接定点类型）
// 边的模型：BaseUnaryEdge   连接顶点个数：1    测量值数据类型：double  顶点类型：CurveFittingVertex
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]);        // _vertices[]存储顶点信息
        const Eigen::Vector3d abc = v->estimate();
        _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1,0) * _x + abc(2, 0));        // _error存储computeError()函数计算的误差
    }

    // 计算雅克比矩阵
    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() {
    //定义数据参数
    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;                            //随机数产生器

    //生成100个带高斯噪声的数据
    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));
    }

    // 构建图优化
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;    // 配置BlockSolver，每个误差项优化变量维度为3，误差值维度为1
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;    // 创建BlockSolver，并用定义的线性求解器初始化

    // 设置梯度下降的方法，创建总求解器solver
    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);                     //定义边的编号（决定在H矩阵中的位置）
        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);    //执行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() << " s." << endl;

    Eigen::Vector3d abc_estimate = v->estimate();   //获取当前值
    cout << "estimated model: " << abc_estimate.transpose() << endl;

    return 0;
}

CMakeLists.txt file:

cmake_minimum_required(VERSION 3.20)
project(g2oCurveFitting)
set(CMAKE_CXX_STANDARD 14)

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

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

# Ceres库
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

# g2o库
list( APPEND CMAKE_MODULE_PATH /home/huffie/slam/3rdparty/g2o/cmake_modules ) #刚才clone的项目文件夹
set(G2O_ROOT /usr/local/include/g2o)
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(g2oCurveFitting main.cpp)

target_link_libraries(g2oCurveFitting ${OpenCV_LIBS})
target_link_libraries(g2oCurveFitting  g2o_stuff   g2o_core )
target_link_libraries(g2oCurveFitting ${CERES_LIBRARIES})