一步步带你走入基于图优化的g2o库：General Graphic Optimization

目录

什么是图优化？

图优化的例子 

下面是g2o的基本框架结构，让我们一起来梳理一下。

用g2o求解的一般步骤:

1、创建一个线性求解器LinearSolver。

2、创建一个BlockSolver。

3、创建总求解器solver。

4.创建一个稀疏优化器

5.定义图的顶点，并加入到定义好的稀疏优化器中

1.HyperGraph超图

2.Optimizable­Graph优化图

3.BaseVertex

6.定义图的边，并加入到定义好的稀疏优化器中

7.设置参数，进行优化。

用g2o拟合曲线 

结束语

---

什么是图优化？

图优化是通过使用图结构来表示和解决优化问题的一种方法。图是由顶点（Vertex）和边（Edge）组成。在SLAM中，搭载传感器的主体的位姿构成顶点，位姿之间的变换关系构成边。一旦图构建完成了，就要主体的位姿去尽量满足这些边构成的约束。对于一个非线性的最小二乘问题，我们可以用顶点来表示优化变量，用边来表示误差项，这样就能构建一个对应的图，我们称这个图为贝叶斯图或者是因子图。可以把SLAM中的图优化问题分解为：构建图、优化图。

图优化的例子 

位姿节点和路标节点构成图优化的顶点 ，运动模型和观测模型构成图优化的边。这样我们把一个最小二乘问题转换成了上述的图优化问题。g2o是一个通用的图优化库，因此你可以在g2o里求解任何能够表示为图优化的最小二乘问题。

下面是g2o的基本框架结构，让我们一起来梳理一下。

SparseOptimizer是图优化框架中的一个类，通常用于处理大规模稀疏图结构。它是一个OptimizableGraph（图优化框架中的核心类，负责管理和优化图结构），也是一个HyperGraph（超图：是一种图的扩展，允许一个边（称为超边）连接多个顶点，而不仅仅是连接两个顶点）。它包含一个优化算法OptimizationAlgorithm。OptimizationAlgorithm是通过OptimizationWithHessian来实现的。

HyperGraph包含了许多顶点HyperGraph::Vertex（顶点继承自OptimizableGraph::Vertex）和边HyperGraph::Edge（边继承自OptimizableGraph::Edge）。

OptimizationAlgorithm迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell's dogleg 三者中间选择一个。（常用的是GN和LM）

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

用g2o求解的一般步骤:

1、创建一个线性求解器LinearSolver。

求解的增量方程形式为：H△X=-b，我们首先想到的是直接求逆，也就是△X=-H.inv*b。当H的维度较小，确实可以求逆解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得复杂。这时候就需要换另一种思路进行求解，即求线性方程。可以采用下述的一些方法进行求解：

// 使用sparse cholesky分解法。继承自LinearSolverCCS
// 这里的“CCS”代表 Compressed Column Storage（压缩列存储），这是一种常见的稀疏矩阵存储格式。
LinearSolverCholmod 
// 使用CSparse法。继承自LinearSolverCCS
LinearSolverCSparse
// 使用preconditioned conjugate gradient（预条件共轭梯度法 ），继承自LinearSolver
LinearSolverPCG 

2、创建一个BlockSolver。

BlockSolver用线性求解器LinearSolver来初始化。它的定义在g2o/core/block_solver.h中。

BlockSolver定义方式（动态和固定）如下：

  //variable size solver 可变大小求解器
  typedef BlockSolver< BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > BlockSolverX;
  // 前者代表位姿的维度，后者代表路标的维度。

  // solver for BA/3D SLAM BA/3D SLAM 求解器
  typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3; 
  // 每个优化变量有 6 个自由度（例如，一个三维位姿），并且有 3 个观测自由度（例如，一个地图点的三维 
  // 位置）。 
  // 例如在Optimizer.cc中进行局部BA创建一个g2o的优化器时创建。
  g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

  // solver fo BA with scale 带尺度的 BA 求解器
  typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3;
  // 每个优化变量有 7 个自由度（6 个来自位姿和 1 个尺度参数），同样有 3 个观测自由度。
  
  // 2Dof landmarks 3Dof poses  2D 地标与 3D 位姿求解器
  typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2;
  // 每个优化变量有 3 个自由度（例如，3D 地标的位置），并且与 2 个自由度（例如，2D 位姿）相关联。

3、创建总求解器solver。

1.位于g2o/g2o/core/ 目录下，其中包括了三大优化方法：高斯牛顿（GaussNewton）法，列文伯格-马夸尔特（Levenberg–Marquardt）法、Dogleg法。

 2.这三大优化方法共同公有继承于同一个类：OptimizationWithHessian。

class  OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian

class  OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian

class  OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian

 3.OptimizationAlgorithmWithHessian则又是公有继承于类：OptimizationAlgorithm。

 class  OptimizationAlgorithmWithHessian : public OptimizationAlgorithm

4.创建一个稀疏优化器

5.定义图的顶点，并加入到定义好的稀疏优化器中

1.HyperGraph超图

位于hyper_graph.h中。

超图主要内容：

声明一个超图类HyperGraph，它位于命名空间g2o中。

超图类公共部分1：

声明了一个枚举变量HyperGraphElementType，它储存了超图的一些元素（顶点、边等它们在枚举中被默认赋值了）

声明一个抽象基类HyperGraphElement（它实际上是一个struct）

定义一些类型的别名EdgeSet（边集合）VertexSet（顶点集合）等。

在超图类中声明两个抽象类（定点类、边类），它们共有继承与抽象基类HyperGraphElement（struct）

顶点类可以实现获取id、设置id、 获取顶点连接的边以及在继承的抽象基类中必须实现的功能：动态识别这是一个顶点类型。

边类可以实现获取边所连接的顶点的集合、获取超边所连接的指定id的顶点、替换指定id位置的顶点以及在继承的抽象基类中必须实现的功能：动态识别这是一个边类型。

超图类公共部分2： 

构造函数、析构函数、可以实现获取超图中指定id的顶点指针、 移除超图中的顶点对象、移除超图中的边对象、清除超图的所有内容、获取超图边、获取超图点、给超图添加顶点、给超图添加边、更改超图中已经存在的顶点的id。

超图类私有部分：

禁止超图类的复制与赋值。

涉及顶点的有三个类：HyperGraph::Vertex、Optimizable­Graph::Vertex和BaseVertex。 

#ifndef G2O_AIS_HYPER_GRAPH_HH
#define G2O_AIS_HYPER_GRAPH_HH

#include <map>
#include <set>
#include <bitset>
#include <cassert>
#include <vector>
#include <limits>
#include <cstddef>

#ifdef _MSC_VER
#include <unordered_map>
#else
#include <tr1/unordered_map>
#endif


/** @addtogroup graph */
//@{
namespace g2o {

  /**
     Class that models a directed  Hyper-Graph. An hyper graph is a graph where an edge
     can connect one or more nodes. Both Vertices and Edges of an hyoper graph
     derive from the same class HyperGraphElement, thus one can implement generic algorithms
     that operate transparently on edges or vertices (see HyperGraphAction).

     The vertices are uniquely identified by an int id, while the edges are
     identfied by their pointers. 
   */
  class  HyperGraph
  {
    public:

      /**
       * \brief enum of all the types we have in our graphs
          这个枚举包含了图中的所有类型。
       */
      enum  HyperGraphElementType {
        HGET_VERTEX,//顶点类型
        HGET_EDGE,//边类型
        //其他类型，用于扩展。
        HGET_PARAMETER,
        HGET_CACHE,
        HGET_DATA,
        //
        HGET_NUM_ELEMS // keep as last elem保持处于枚举的最后一个元素的位置
      };
      //std::bitset是C++标准库中的模板类，提供了一个定长的位（bit）数组，
      //可以用来高效地管理和操作布尔值。每一位可以是0或1。
      //typedef给现有类型定义别名
      typedef std::bitset<HyperGraph::HGET_NUM_ELEMS> GraphElemBitset;
      // 前向声明，告诉编译器有这样的一个类。
      class  Vertex;
      class  Edge;
      
      /**
       * base hyper graph element, specialized in vertex and edge
       */
      //在C++中，结构体（struct）可以被类（class）继承。
      struct  HyperGraphElement //这可以叫做是抽象基类
      {
        virtual ~HyperGraphElement() {}
        // 定义为虚函数是为了确保派生类（如顶点或边）对象通过基类指针销毁时，
        // 可以正确调用派生类的析构函数，避免资源泄漏。
        // 虚函数允许基类提供默认行为，子类可以选择性重写虚函数来改变行为。
        /**
         * returns the type of the graph element, see HyperGraphElementType
         */
        virtual HyperGraphElementType elementType() const = 0;
        // 纯虚函数强制所有派生类实现某些功能
      };

      typedef std::set<Edge*>                           EdgeSet;//定义边集合类型的别名。
      typedef std::set<Vertex*>                         VertexSet;

      typedef std::tr1::unordered_map<int, Vertex*>     VertexIDMap;// 用于根据顶点ID快速查找对应的顶点对象。
      typedef std::vector<Vertex*>                      VertexContainer;//用于存储某条边所连接的顶点

      //! abstract Vertex, your types must derive from that one
      //抽象类
      class  Vertex : public HyperGraphElement {
        public:
          //! creates a vertex having an ID specified by the argument
          //当一个构造函数被标记为explicit 时，它只能通过显式调用来使用，而不能通过隐式类型转换进行调用。
          explicit Vertex(int id=-1);
          virtual ~Vertex();
          //! returns the id
          int id() const {return _id;}//常量成员函数不会修改任何成员变量，只有常量对象才能调用。
	  virtual void setId( int newId) { _id=newId; }
          //设计了常量和非常量两个版本，常量版本保证调用者不会修改集合内容，非常量版本保证非常量对象调用并修改集合内容。
          //! returns the set of hyper-edges that are leaving/entering in this vertex
          //公共成员函数，获取与当前顶点相连的所有超边的集合。常量超边集合引用。
          const EdgeSet& edges() const {return _edges;}
          //! returns the set of hyper-edges that are leaving/entering in this vertex
          EdgeSet& edges() {return _edges;}
          virtual HyperGraphElementType elementType() const { return HGET_VERTEX;}//动态类型识别，识别这是一个顶点类型。
        protected:
          int _id;
          EdgeSet _edges;//用类型的别名声明一个变量。
      };

      /** 
       * Abstract Edge class. Your nice edge classes should inherit from that one.
       * An hyper-edge has pointers to the vertices it connects and stores them in a vector.
       */
      class  Edge : public HyperGraphElement //公有继承基类（struct）
      {
        public:
          //! creates and empty edge with no vertices
          explicit Edge(int id = -1);//构造函数，只能被显示调用。
          virtual ~Edge();//析构函数

          /**
           * resizes the number of vertices connected by this edge
           */
          virtual void resize(size_t size);//动态调整边所连接的顶点数量。
          //同样设计了常量和非常量两个版本，函数用于返回超边连接的顶点集合。
          /**
            returns the vector of pointers to the vertices connected by the hyper-edge.
            */
          const VertexContainer& vertices() const { return _vertices;}//用了前边定义的别名VertexContainer
          /**
            returns the vector of pointers to the vertices connected by the hyper-edge.
            */
          VertexContainer& vertices() { return _vertices;}
          //设计常量和非常量两个版本，函数用于返回超边连接的第i个顶点。
          /**
            returns the pointer to the ith vertex connected to the hyper-edge.
            */
          const Vertex* vertex(size_t i) const { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
          /**
            returns the pointer to the ith vertex connected to the hyper-edge.
            */
          Vertex* vertex(size_t i) { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
          /**
            set the ith vertex on the hyper-edge to the pointer supplied
            */
          //函数用于设置当前超边（Edge）所连接的第i个顶点为指定的顶点v。它通过索引i直接替换_vertices容器中对应位置的顶点指针。
          void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}

          int id() const {return _id;}
          void setId(int id);
          virtual HyperGraphElementType elementType() const { return HGET_EDGE;}//虚函数返回类型为枚举类型，动态类型识别，识别这是一个边类型。
        protected:
          VertexContainer _vertices;
          int _id; ///< unique id
      };
    //多个public访问说明符的使用并不是必须的，而是为了增强代码的可读性和逻辑分离性。
    public:
      //! constructs an empty hyper graph
      HyperGraph();//最外层超图类的默认构造函数，构造一个空的超图。
      //! destroys the hyper-graph and all the vertices of the graph
      // 销毁HyperGraph对象，并释放与超图相关的所有资源。
      virtual ~HyperGraph();

      // 设计常量和非常量两个函数版本查找并返回超图中ID为id的顶点指针：
      //! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
      Vertex* vertex(int id);
      //! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
      const Vertex* vertex(int id) const;

      //! removes a vertex from the graph. Returns true on success (vertex was present)
      virtual bool removeVertex(Vertex* v);//从超图中移除一个顶点对象。如果顶点存在并成功移除，返回 true；否则返回false。
      //! removes a vertex from the graph. Returns true on success (edge was present)
      virtual bool removeEdge(Edge* e);//从超图中移除一个边对象。如果顶点存在并成功移除，返回 true；否则返回false。
      //! clears the graph and empties all structures.
      virtual void clear();// 清空整个超图，包括所有的顶点和边，释放动态分配的所有资源。
      //两个版本获取顶点。
      //! @returns the map <i>id -> vertex</i> where the vertices are stored
      const VertexIDMap& vertices() const {return _vertices;}
      //! @returns the map <i>id -> vertex</i> where the vertices are stored
      VertexIDMap& vertices() {return _vertices;}
      //两个版本获取边。
      //! @returns the set of edges of the hyper graph
      const EdgeSet& edges() const {return _edges;}
      //! @returns the set of edges of the hyper graph
      EdgeSet& edges() {return _edges;}

      /**
       * adds a vertex to the graph. The id of the vertex should be set before
       * invoking this function. the function fails if another vertex
       * with the same id is already in the graph.
       * returns true, on success, or false on failure.
       */
      /*
      该函数用于将一个顶点添加到超图中。
      在调用该函数之前，顶点的唯一标识符（id）必须已设置。
      如果超图中已经存在具有相同 id 的顶点，函数将失败，并返回 false。
      如果添加成功，函数返回 true。
      */
      virtual bool addVertex(Vertex* v);

      /**
       * Adds an edge  to the graph. If the edge is already in the graph, it
       * does nothing and returns false. Otherwise it returns true.
       */
      /*该函数用于将一条边（Edge）添加到超图中。
      如果边已经存在，函数不会重复添加，并返回 false。
      如果添加成功，函数返回 true。
      */
      virtual bool addEdge(Edge* e);

      /**
       * changes the id of a vertex already in the graph, and updates the bookkeeping
       @ returns false if the vertex is not in the graph;
       */
    /*该函数用于更改一个已存在于超图中的顶点的 ID。
    如果顶点不存在于超图中，函数返回 false，表示操作失败。
    如果更改成功，函数需要更新内部的 ID 映射关系（VertexIDMap）。
    */
      virtual bool changeId(Vertex* v, int newId);

    protected:
      //都是通过别名声明的变量。
      VertexIDMap _vertices;
      EdgeSet _edges;

    // 私有化复制构造函数和赋值运算符，禁用了超图类（HyperGraph）的复制和赋值功能。
    private:
      // Disable the copy constructor and assignment operator
      HyperGraph(const HyperGraph&) { }//复制
      HyperGraph& operator= (const HyperGraph&) { return *this; }//赋值
  };

} // end namespace


#endif

2.Optimizable­Graph优化图

它继承于超图类。

OptimizableGraph:: Vertex也是非常底层的类， 在具体使用时一般都会进行扩展。

3.BaseVertex

g2o提供了 个比较通用的适合大部分情况的模板， 也就是第3个类——BaseVertex。 我们找到源码中关于BaseVertex的定义， 可以发现BaseVertex继承自OptimizableGraph::Vertex。  

#ifndef G2O_BASE_VERTEX_H
#define G2O_BASE_VERTEX_H

#include "optimizable_graph.h"
#include "creators.h"
#include "../stuff/macros.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/StdVector>
#include <stack>

namespace g2o {

  using namespace Eigen;


/**
 * \brief Templatized BaseVertex
 *
 * Templatized BaseVertex
 * D  : minimal dimension of the vertex, e.g., 3 for rotation in 3D
 * T  : internal type to represent the estimate, e.g., Quaternion for rotation in 3D
 */
/**
 * \brief 模板化的 BaseVertex 类
 *
 * 模板化的 BaseVertex 类
 * D  : 顶点的最小维度，例如在3D中的旋转为3。
 * T  : 用于表示估计值的内部类型，例如在3D中的旋转可用四元数（Quaternion）表示。
 */
  template <int D, typename T>
  class BaseVertex : public OptimizableGraph::Vertex //派生类继承于优化图的顶点类。
  {
    public:
    //声明一系列别名
    typedef T EstimateType;//别名
    typedef std::stack<EstimateType, 
                       std::vector<EstimateType,  Eigen::aligned_allocator<EstimateType> > >
    BackupStackType;//别名

    static const int Dimension = D;           ///< dimension of the estimate (minimal) in the manifold space

    typedef Eigen::Map<Matrix<double, D, D>, Matrix<double,D,D>::Flags & AlignedBit ? Aligned : Unaligned >  HessianBlockType;

  public:
    BaseVertex();//默认构造函数

    //实现在继承的OptimizableGraph::Vertex类中定义的纯虚函数。
    //获取海森矩阵元素
    virtual const double& hessian(int i, int j) const { assert(i<D && j<D); return _hessian(i,j);}
    virtual double& hessian(int i, int j)  { assert(i<D && j<D); return _hessian(i,j);}
    //获取行列式
    virtual double hessianDeterminant() const {return _hessian.determinant();}
    //获取指向海森矩阵数据的指针
    virtual double* hessianData() { return const_cast<double*>(_hessian.data());}

    virtual void mapHessianMemory(double* d);

    virtual int copyB(double* b_) const {
      memcpy(b_, _b.data(), Dimension * sizeof(double));
      return Dimension; 
    }

    virtual const double& b(int i) const { assert(i < D); return _b(i);}
    virtual double& b(int i) { assert(i < D); return _b(i);}
    virtual double* bData() { return _b.data();}

    virtual void clearQuadraticForm();

    //! updates the current vertex with the direct solution x += H_ii\b_ii
    //! @returns the determinant of the inverted hessian
    virtual double solveDirect(double lambda=0);
    //上面这些函数定义都是为了实现它所继承的那个类中定义的纯虚函数。

    //! return right hand side b of the constructed linear system
    //返回线性系统的右端项向量b（常量和非常量版本）
    Matrix<double, D, 1>& b() { return _b;}
    const Matrix<double, D, 1>& b() const { return _b;}
    //! return the hessian block associated with the vertex
    //返回与顶点关联的Hessian矩阵块A
    HessianBlockType& A() { return _hessian;}
    const HessianBlockType& A() const { return _hessian;}

    //同样是继承的类的纯虚函数的实现。
    virtual void push() { _backup.push(_estimate);}
    virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();}
    virtual void discardTop() { assert(!_backup.empty()); _backup.pop();}
    virtual int stackSize() const {return _backup.size();}

    //! return the current estimate of the vertex
    const EstimateType& estimate() const { return _estimate;}
    //! set the estimate for the vertex also calls updateCache()
    void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}

  protected:
    //用别名声明一些变量用于public
    HessianBlockType _hessian;
    Matrix<double, D, 1> _b;
    EstimateType _estimate;
    BackupStackType _backup;
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#include "base_vertex.hpp"

} // end namespace g2o


#endif

1.g2o内部定义的常用的顶点类型：

// g2o定义好的常用顶点类型
// 2D位姿顶点(x,y,theta)
VertexSE2 : public BaseVertex<3, SE2> 
// 六维向量(x,y, z, qx, qy, qz)省略了四元数中的qw 
VertexSE3 : public BaseVertex<6, Isometry3> 
// 二维点和三维点
VertexPointXY : public BaseVertex<2, Vector2> 
VertexPointXYZ : public BaseVertex<3, Vector3> 
//在局部建图线程中进行局部BA时添加局部地图点顶点时，该类型即为这个。
//g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
VertexSBAPointXYZ : public BaseVertex<3, Vector3> 
// SE(3)顶点，内部用变换矩阵参数化，外部用指数映射参数化。
VertexSE3Expmap : public BaseVertex<6, SE3Quat> 
// SBACam顶点
VertexCam : public BaseVertex<6, SBACam> 
// Sim(3) 顶点
VertexSim3Expmap : public BaseVertex<7, Sim3>

2.复写一些函数： 

当需要的顶点类型不在其中时，这时候需要自己去定义一下。 

//复写将节点设置为原点的函数 
virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }
//读写函数只需要声明一下
virtual bool read(istream &in) {}

virtual bool write(ostream &out) const {}
//复写增量更新函数
virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(update);
  }

3.向优化器中加入顶点 

6.定义图的边，并加入到定义好的稀疏优化器中

1.边的3种类型

BaseUnary Edge、 BaseBinaryEdge和 BaseMultiEdge, 它们分别表示一元边、二元边、多元边。

2.主要参数

D是int 类型的， 表示测量值的维度(Dimension)。

E表示测量值的数据类型。 

VertexXi、 VertexXj分别表不不同顶点的类型。

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

以上面这个代码为例：BaseBinaryEdge类型的边是个二元边。 第1个参数 2" 是说测量值是二维的，测量值就是图像的二维像素坐标， 对应测量值的类型是Vec­tor2D,边连接的两个顶点分别是三维点VertexSBAPointXYZ和李群位姿Ver­texSE3Expmap。

3.复写一些函数 

// 复写计算误差函数
virtual void computeError() override {...}
// 复写雅克比矩阵函数
virtual void linearizeOplus() override {...}
// 复写读写函数
virtual bool read(istream &in) {}

virtual bool write(ostream &out) const {}

4.边的一些重要成员变量和函数 

measurement 存储观测值
error 存储计符的误差
vertices ［ ］ 存储顶点信息
setVertex(int, vertex) 定义顶点及其编号 
setid(int) 定义边的编号
setMeasurement(type) 定义观测值
setinformation () 定义信息矩阵

5.向优化器中添加边 

7.设置参数，进行优化。

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

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
// 公有继承模板类BaseVertex。
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 是 Eigen 库提供的一个宏，用于解决在使用 Eigen 库时可能出 
  // 现的内存对齐问题。它主要用于需要特殊对齐的动态分配情况。
  // 这个宏会在类中定义一个对齐版本的 new 和 delete 操作符，保证使用 new 分配的内存满足 Eigen 
  // 的对齐要求。

  // 重置
  // 定义了一个纯虚函数setToOriginImpl，其作用是将节点node设置为原点
  // 关键字override说明该函数是对基类中同名虚函数的重写。
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }

  // 更新（增量更新）
  // 这是一个带指针参数的纯虚函数，参数v通常表示一个更新向量，函数的功能是利用v中的参数来更新节点 
  // 的位置（或状态）。
  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 << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}

结束语

以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。