G2O (General Graph Optimization)

前言

        以高翔的《视觉SLAM14讲》中的 g2o 拟合曲线为例，讲解 g2o 的使用。源文件为 g2oCurveFitting.cpp。

#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>> my_BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
  typedef g2o::LinearSolverDense<my_BlockSolverType::PoseMatrixType> my_LinearSolverType; // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<my_BlockSolverType>(g2o::make_unique<my_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 << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}

一、g2o 编程框架

       SparseOptimizer 包含一个OptimizationAlgorithm（优化算法）的对象。OptimizationAlgorithm 是通过 OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell's dogleg 中选择。

        OptimizationWithHessian 内部包含一个Solver（求解器），这个 Solver 实际是由一个BlockSolver 组成的。这个 BlockSolver 有两个部分，一部分是 SparseBlockMatrix<T> ，用于计算稀疏的雅可比和 Hessian 矩阵；另一部分是 LinearSolver（线性求解器），它用于计算迭代过程中最关键的一步 HΔx=−b，LinearSolver 有几种方法可以选择：PCG（预条件共轭梯度）, CSparse, Choldmod（Cholesky 分解） 。

• ①创建 线性求解器 LinearSolver

        增量方程的形式是：H△X=-b，通常情况下通过直接求逆，也就是△X=-H.inv * b 的方式求解 △X。但当 H 的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。此时需要一些特殊的方法对矩阵进行求逆，在 g2o 中主要有以下几种线性求解方法：

//***g2o源码 g2o/g2o/solvers/dense/linear_solver_dense.h ***//
g2o::LinearSolverCholmod<>  // 使用sparse cholesky分解法。继承自LinearSolverCCS
g2o::LinearSolverCSparse<>  // 使用CSparse法。继承自LinearSolverCCS
g2o::LinearSolverPCG<>	    // 使用preconditioned conjugate gradient 法，继承自LinearSolver
g2o::LinearSolverDense<>    // 使用dense cholesky分解法。继承自LinearSolver
g2o::LinearSolverEigen<>    // 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver

• ②创建 块求解器 BlockSolver。并用上面定义的 线性求解器 初始化 块求解器

         BlockSolver 有两种定义方式。一种是固定尺寸的 BlockSolver 

  //***g2o源码 g2o/g2o/core/block_solver.h ***//
  // p:PoseDim 位姿的维度
  // l:LandmarkDim 路标点的维度
  template<int p, int l>
  using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

        另一种是可变尺寸的 BlockSolver 

  //***g2o源码 g2o/g2o/core/block_solver.h ***//
  //variable size solver
  using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

      在某些应用场景中，位姿和路标点在程序开始时并不能被确定，此时块求解器就没办法固定尺寸，应该使用可变尺寸的 BlockSolver，以便让所有的参数都在中间过程中被确定。

        在块求解器头文件 block_solver.h 的最后，预定义了比较常用的几种类型，如下所示：

  //***g2o源码 g2o/g2o/core/block_solver.h ***//
  // 表示 pose 是 6 维，观测点是 3 维。用于 3D SLAM 中的 BA
  // solver for BA/3D SLAM
  using BlockSolver_6_3 = BlockSolverPL<6, 3>;
    
  // 在 BlockSolver_6_3 的基础上多了一个 scale
  // solver fo BA with scale
  using BlockSolver_7_3 = BlockSolverPL<7, 3>;

  // 表示 pose 是 3 维，观测点是 2 维
  // 2Dof landmarks 3Dof poses
  using BlockSolver_3_2 = BlockSolverPL<3, 2>;

         注意：虽然是先创建 线性求解器 LinearSolver，再创建 块求解器 BlockSolver，但在实际使用中，创建 线性求解器 时要传入 Hessian 矩阵的类型，Hessian 矩阵的类型可由 my_BlockSolverType::PoseMatrixType 确定，所以一般在创建 线性求解器 前 先创建 块求解器类型。如下所示：

  //***高翔代码***//
  // g2o::BlockSolver 是一个模板类，用于定义优化问题的 块求解器。它接受一个 g2o::BlockSolverTraits 模板参数。
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<p, l>> my_BlockSolverType;  // 块求解器类型
  // PoseMatrixType 是从 my_BlockSolverType 中提取的位姿矩阵类型，专门用于处理和表示位姿相关的矩阵。PoseMatrixType 是一个方阵，大小为 PoseDim x PoseDim。
  // g2o::LinearSolverDense 是一个模板类，用于定义优化问题的 线性求解器，它使用 使用 dense cholesky 分解法 求解矩阵的逆。它接受一个模版参数，表示 H 矩阵的类型，即 PoseMatrixType。
  typedef g2o::LinearSolverDense<my_BlockSolverType::PoseMatrixType> my_LinearSolverType; // 线性求解器类型

• ③创建 总求解器 solver。并从GN, LM, DogLeg 中选择迭代策略，再用上述定义的 块求解器初始化 总求解器

        g2o/g2o/core/ 目录下，Solver 的优化方法有三种：分别是高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法    

//***g2o源码 g2o/g2o/core/optimization_algorithm_gauss_newton.h ***//
// Gauss Newton 法
g2o::OptimizationAlgorithmGaussNewton(std::unique_ptr<Solver> solver)

// Levenberg–Marquardt 法
g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Solver> solver)

// Dogleg 法
g2o::OptimizationAlgorithmDogleg(std::unique_ptr<Solver> solver)

// 其中 solver 为 块求解器

        实际创建  总求解器 solver 的创建如下所示：

  //***高翔代码***//
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
                    g2o::make_unique<my_BlockSolverType>(g2o::make_unique<my_LinearSolverType>()));

        先创建一个 线性求解器 LineraSolver，再创建 块求解器 BlockSolver（使用上面创建的线性求解器初始化），最后创建 总求解器 （使用上面创建的块求解器初始化）。

• ④创建 稀疏优化器 SparseOptimizer，并用已定义总求解器 solver 作为求解方法

  //***高翔代码***//
  g2o::SparseOptimizer optimizer;   // 稀疏优化器
  optimizer.setAlgorithm(solver);   // 将已定义的总求解器 solver 设置为 稀疏优化器 SparseOptimizer 的求解方法
  optimizer.setVerbose(true);       // 打开调试输出，输出优化过程中的详细信息

//***g2o源码 g2o/core/sparse_optimizer.h ***//
void setAlgorithm(OptimizationAlgorithm* algorithm);
void setVerbose(bool verbose);

• ⑤定义图的顶点并添加到 稀疏优化器 SparseOptimizer 中

  //***高翔代码***//
  // 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

    //***g2o源码 g2o/g2o/core/base_vertex.h ***//    
    //! set the estimate for the vertex also calls updateCache()
    void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}

    //***g2o源码 g2o/g2o/core/optimizable_graph.h ***//
    //设置图中节点（顶点）的 ID，并在改变 ID 后确保图的结构一致性
    virtual void setId(int id) {_id = id;}

    //***g2o源码 g2o/g2o/core/optimizable_graph.h ***//
    /**
     * 添加一个新顶点。然后“获取”新顶点。
     * @param userData：用户自定义数据，通常用于存储与顶点相关的附加信息，类型为 Data*。这个参数是可选的，默认值为 0（或 nullptr）。
     * @如果图中已经存在与v具有相同id的顶点，则返回false，否则返回true。
     */
    virtual bool addVertex(HyperGraph::Vertex* v, Data* userData);
    virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0);}
    bool addVertex(OptimizableGraph::Vertex* v, Data* userData);
    bool addVertex(OptimizableGraph::Vertex* v) { return addVertex(v, 0); }

• ⑥ 定义图的边并添加到 稀疏优化器 SparseOptimizer 中

  //***高翔代码***//
  // 往图中增加边
  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);
  }

//***g2o源码 g2o/g2o/core/hyper_graph.h ***//
void HyperGraph::Edge::setId(int id) { _id = id; }

  //***g2o源码 g2o/g2o/core/hyper_graph.h ***//
  /**
  * 将超边上的第 i 个顶点设置为提供的指针
  * @param i: 要设置的顶点在超边中的索引，类型为 size_t
  * @param v: 指向要设置的顶点的指针，类型为 Vertex*
  */
  void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}

//***g2o源码 g2o/g2o/core/base_edge.h ***//
//设置与边相关的测量值
virtual void setMeasurement(const Measurement& m) { _measurement = m;}

//***g2o源码 g2o/g2o/core/base_edge.h ***//
//约束的信息矩阵
//设置边的信息矩阵(协方差矩阵之逆)
EIGEN_STRONG_INLINE void setInformation(const InformationType& information) { _information = information;}

  //***g2o源码 g2o/g2o/core/optimizable_graph.h ***//
  /**
   * 添加一个新边
   * 新边应该已经设置好它所连接的顶点（使用 setFrom 和 setTo 函数）。
   * @如果插入不起作用（顶点类型不兼容/缺少顶点），则返回false。反之亦然。
   */
  virtual bool addEdge(HyperGraph::Edge* e);
  bool addEdge(OptimizableGraph::Edge* e);

• ⑦设置优化参数，开始执行优化

        初始化 稀疏优化器 SparseOptimizer ，并设置迭代次数，最终输出优化结果。

  //***高翔代码***//
  // 执行优化
  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 << "estimated model: " << abc_estimate.transpose() << endl;

        initializeOptimization() 函数有 3 种重载版本

    //***g2o源码 g2o/g2o/core/sparse_optimizer.h ***//
    // 优化器的新接口
    // 旧函数将被删除
    /**
     * 优化某个特定子图，而不是整个图。
     * 调用它之前，必须确保正确设置顶点的边缘化（marginalized()）和固定状态（fixed()）。这是为了在优化过程中能够明确哪些顶点应该被固定，哪些顶点参与舒尔补（Schur Complement）计算。
     * @param eset: 要优化的边的子集。这个边集合定义了子图，通过这些边涉及的顶点决定哪些顶点将参与优化。
     * @出现问题时return false
     */
    virtual bool initializeOptimization(HyperGraph::EdgeSet& eset);

    /**
     * 优化某个特定子图，而不是整个图。
     * 调用它之前，必须确保正确设置顶点的边缘化（marginalized()）和固定状态（fixed()）。这是为了在优化过程中能够明确哪些顶点应该被固定，哪些顶点参与舒尔补（Schur Complement）计算。
     * @param vset: 要优化的顶点子集。只有包含在这个集合中的顶点才会参与优化。
     * @param level: 在多层次优化中表示优化层级，默认为0
     * @出现问题时return false
     */
    virtual bool initializeOptimization(HyperGraph::VertexSet& vset, int level=0);

    /**
     * 优化整个图。
     * 调用它之前，必须确保正确设置顶点的边缘化（marginalized()）和固定状态（fixed()）。这是为了在优化过程中能够明确哪些顶点应该被固定，哪些顶点参与舒尔补（Schur Complement）计算。
     * @param level: 在多层次优化中表示优化层级，默认为0
     * @出现问题时return false
     */
    virtual bool initializeOptimization(int level=0);

   //***g2o源码 g2o/g2o/core/sparse_optimizer.h ***//
   /**
     * 根据当前图的配置和存储在类实例中的设置，启动一次图优化运行
     * 在调用 initializeOptimization() 之后才能被调用
     * @param iterations: 迭代次数
     * @param online: 默认值为 false，表示是否以“在线模式”进行优化。在线模式（增量式优化）允许在处理新数据时只优化增量部分。
     * @returns 实际执行的迭代次数。这个值可以小于传入的 iterations 参数，如果优化在收敛条件下提前停止。
     */
    int optimize(int iterations, bool online = false);

    //***g2o源码 g2o/g2o/core/base_vertex.h ***//
    //return 顶点的当前估计值
    const EstimateType& estimate() const { return _estimate;}

二、自定义 g2o 顶点

//***高翔代码***//
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
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 {}
};

//***g2o源码 g2o/g2o/core/base_vertex.h ***//
/**
 * \brief 模板化 BaseVertex
 *
 * D  : int 类型，表示vertex的最小维度，例如3D空间中旋转是3维的，则 D = 3
 * T  : 待估计 vertex 的数据类型，例如用四元数表达三维旋转时，T 为 Quaternion 类型
 */
template <int D, typename T>
class BaseVertex : public OptimizableGraph::Vertex {
    // 类的具体实现...
};

        在我们定义自己的 Vertex 之前，可以先看下 g2o 本身已经定义好的一些常用的顶点类型：  

//***g2o源码***//

// g2o/g2o/types/slam2d/vertex_se2.h
// 2D 位姿顶点, (x,y,theta)
class G2O_TYPES_SLAM2D_API VertexSE2 : public BaseVertex<3, SE2> 

// g2o/g2o/types/slam3d/vertex_se3.h
// 欧式变换矩阵(4x4)，状态由 6d 向量表示（x，y，z，qx，qy，qz），省略了四元数的w部分。
class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3>

// g2o/g2o/types/slam2d/vertex_point_xy.h
// 二维点
class G2O_TYPES_SLAM2D_API VertexPointXY : public BaseVertex<2, Vector2>

// g2o/g2o/types/slam3d/vertex_pointxyz.h
// 三维空间中被跟踪的点
class G2O_TYPES_SLAM3D_API VertexPointXYZ : public BaseVertex<3, Vector3>

// g2o/g2o/types/sba/types_sba.h
// 三维空间中的点，主要用于 Bundle Adjustment，BA
class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>

// g2o/g2o/types/sba/types_six_dof_expmap.h
// SE(3)顶点,内部用变换矩阵参数化，外部用指数映射参数化
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>

// g2o/g2o/g2o/types/sba/vertex_cam.h
// SBACam 顶点，（x，y，z，qw，qx，qy，qz）
// 状态由 6d 向量表示（x，y，z，qx，qy，qz），省略了四元数的w部分。qw 假设为正值，以避免在表示旋转时的歧义。
class G2O_TYPES_SBA_API VertexCam : public BaseVertex<6, SBACam> 

// g2o/g2o/types/sim3/types_seven_dof_expmap.h
// Sim(3)顶点，（x，y，z，qw，qx，qy，qz）
// 状态由 7d 向量表示（x，y，z，qx，qy，qz），省略了四元数的w部分。qw 假设为正值，以避免在表示旋转时的歧义
// 表示两个相机之间的相对变换
class G2O_TYPES_SIM3_API VertexSim3Expmap : public BaseVertex<7, Sim3>

三、自定义 g2o 边

//***高翔代码***//
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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
};

//***g2o源码 g2o/g2o/core/base_vertex.h ***//

参考

        对流形（Manifold）的最简单快速的理解