轻松入门g2o

概述

g2o是近年来图优化中应用最广泛的工具，在orb-slam，SLAM十四讲中都是经常被用到的优化工具。
看了高博的书，或许还是存在很多不解之处（里面简单例子太少了，只有一个abc的拟合，又涉及了继承，所以一个例子看不懂）

别人的博客也只是讲讲原理，但是没有一个带小白入门的过程。

如果看过高博的例子，就知道开头要怎么定义 optimizer，但是怎么定义边和顶点以及怎么添加边和顶点以及怎么写参数一头雾水，今天就把这些弄清楚！

---

基础

首先g2o编程是需要两部分组件的，一个叫 Vertex（顶点），另一个叫 Edge(边)。
顶点是要被优化的量，边描述了误差和顶点之间的关系。
g2o中有3中边，一元边，二元边，多元边，一元边只能有一个顶点，二元边两个顶点，多元边多个顶点。

g2o代码

https://github.com/gaoxiang12/slambook
这个里面有g2o代码，但是缺少data文件夹，g2o在3rdparty中

g2o/g2o/examples有例子。

可以下面这个连接找到
https://github.com/OpenSLAM-org/openslam_g2o
这个链接可能编译不通过，所以不推荐用这个g2o，只是用一下 他的data文件夹 就好了。

---

g2o已有类型

g20已有的类型分为两种，一种是模板纯虚类，这种类型不能直接用，必须要继承，如 BaseVertex<D, E> , BaseUnaryEdge< ..> BaseBinaryEdge <...> BaseEdge<>，第一个是顶点，后面三个是边。
另一种已经有的类是关于SLAM的，所以简单的问题就不能用这些类，需要继承顶点模板类和边模板类。

---

有话说

先举个简单的例子，要不然直接说码代码应该也是很难理解的

举例，代码见 https://github.com/OpenSLAM-org/openslam_g2o/blob/master/g2o/examples/data_fitting/circle_fit.cpp

举一个简单的例子（g2o的example中一个简单例子）：
问题： 有10个平面点，它们大概可以看做是组成了一个圆，我们现在来找这个看起来大概是圆的形状的圆心，使圆心到各个点的距离最小，同时求出半径r，也就是求（a,b ,r）.

我们现在的优化变量就是 (a,b,r)，因为前面说了 顶点是要被优化的量，所以我们将(a,b,r)当成一个整体，作为一个被优化的对象！ 注意！只有这一个被优化的对象，所以只有一个顶点。
于是我们写出了下面的代码：

  VertexParams* params = new VertexParams();   //一定是 指针形式！！（但我总觉得会内存泄露）
  params->setId(0);
  params->setFixed(false);
  params->setEstimate(Eigen::Vector3d(1,1,1)); // some initial value for the params
  optimizer.addVertex(params);    //添加这个顶点

• 第四行填写的是 (a,b,r)的初始值，如果这个问题的函数抽象为凸函数，那么其实初值设置成多少都无所谓，否则应该谨慎设置，如SLAM问题的初值不应该太随便
• 第二行的 setId是给顶点编个号，从0开始，以后添加的顶点逐渐增加编号即可
• 第三行的setFixed函数是说要不要优化这个变量，当然要了，所以不能固定（有些顶点的值是要固定的）
• 第五行中的optimizer即是优化工具，我们把 顶点交给它，optimizer负责启动优化过程，之后我们可以通过params->estimate()来获取优化后的结果.

现在来添加边，边描述了误差和顶点之间的关系，

    EdgePointOnCurve* e = new EdgePointOnCurve;
    e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
    e->setVertex(0, params);
    e->setMeasurement(points[i]);
    optimizer.addEdge(e);

• 第一行申请一个边的指针
• 第二行设置信息矩阵，这个信息矩阵是必须要设置的，应该仿照这样的形式来设置信息矩阵，只是第二行代码中Matrix<double , 1, 1>中的1,1应该根据不同问题去改变，这里的两个数字首先是相同的，表示一种类似于协方差的矩阵（不太懂），维度1表示我们的误差的维度是1. 如果误差维度是2，就应该写成Matrix<double, 2,2> ，比如像素坐标误差就是2维的。
• 第三行设置的是边连线的顶线，这里写的是 e->setVertex(0, params);这里只有这一个setvertex,而其实 edge可以对应很多个边，因此还有可能有e->setVertex(1, params);`` e->setVertex(2, params);`` e->setVertex(3, params);等。这里的setVertex也并非仅仅随便添加就可以，e是有类型的。 g2o中可以分为三种类型的边，一元边，二元边，多元边，这要看你是要的哪个类型的边，代码中的EdgePointOnCurve是从一元边继承来的，所以只有一个顶点。
• 第四行e->setMeasurement(points[i]); 表示设置观测值，这个是很重要的，不能随便写。 我们通过这个观测点与圆心可以求出一个假设半径r2,将r-r2可以作为误差，我们的目的就是要优化误差，使其减小。

---

如何定义自己的顶点和边

1.定义自己的顶点

class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexParams()
    {
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual void setToOriginImpl()
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Vector3d::ConstMapType v(update);
      _estimate += v;
    }
};

a. 一般情况下必须要继承 BaseVertex<>
b. 如代码所示，需要 写出这些函数的重载，最主要的是 oplusImpl，同时根据自己的问题设置好 public g2o::BaseVertex<3, Eigen::Vector3d>的模板参数，也就是3可以改成别的数字，取决于一个顶点的参数量有几个，同时这个顶点的基本数据类型是什么？因为是3个量，所以可以设置为 Eigen::Vector3d

2.如何定义自己的边

class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgePointOnCurve()
    {
    }
    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }
    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    void computeError()
    {
      const VertexParams* params = static_cast<const VertexParams*>(vertex(0));
      const double& a = params->estimate()(0);
      const double& b = params->estimate()(1);
      const double& lambda = params->estimate()(2);
      double fval = a * exp(-lambda * measurement()(0)) + b;
      _error(0) = fval - measurement()(1);
    }
};

a. 类似于顶点，这里也必须继承，但继承的对象不一样，因为这里的问题一个点只会对应一个圆心，所以继承一元边
b. 继承时填写的参数有（误差的维度，误差的类型， 第一个顶点的类型,[ 第二个顶点类型…第三个。。第四个]）
c. 最主要的函数是 computeError()，该函数的含义就是 拿顶点的 measurement 减去观测值，前面顶点有 setEstimate, 边有 setMeasurement， 这些量就会在这里用到，至于求导的事情不用我们关心。

---

结束

有了上面的叙述，就应该能理解怎么书写代码了。我还写了一个 多个顶点的代码，附在下面。

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_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/Eigen>
#include <opencv2/opencv.hpp>
#include <cmath>
#include <chrono>

using namespace std;

class MyVertex: public g2o::BaseVertex<1, double>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl()
    {
        _estimate = 0.0;
    }
    virtual void oplusImpl(const double* update)
    {
        _estimate += (*update);
    }

    virtual bool read(istream & in) {}
    virtual bool write(ostream & out) const {}
};

class MyEdge: public g2o::BaseBinaryEdge < 1, double, MyVertex, MyVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    void computeError()
    {
        const MyVertex * v1= static_cast<const MyVertex*>(_vertices[0]);
        const MyVertex * v2 = static_cast<const MyVertex*>(_vertices[1]);
        _error(0,0) = _measurement - (v2->estimate() - v1->estimate()); 
    }
    virtual bool read(istream & in) {}
    virtual bool write(ostream & out) const {}
};

int main()
{
    double edge1= 1, edge2 = -0.8, edge3 = 0.2;

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<1,1>> Block;
    // std::unique_ptr<Block::LinearSolverType> linearSolver( new g2o::LinearSolverDense<Block::PoseMatrixType>());
    // std::unique_ptr<Block> solver_ptr ( new Block(std::move(linearSolver)));

    g2o::SparseOptimizer optimizer;
    Block::LinearSolverType * linearSolver;
    linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block * solver_ptr = new Block(linearSolver);
    
    g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    MyVertex *v1 = new MyVertex;
    v1->setId(0);
    v1->setEstimate(0);
    v1->setFixed(true);
    optimizer.addVertex(v1);

    MyVertex *v2 = new MyVertex;
    v2->setId(1);
    v2->setEstimate(1);
    v2->setFixed(false);
    optimizer.addVertex(v2);

    MyVertex *v3 = new MyVertex;
    v3->setId(2);
    v3->setEstimate(23);
    v3->setFixed(false);
    optimizer.addVertex(v3);

    MyEdge * e1 = new MyEdge;
    MyEdge * e2 = new MyEdge;
    MyEdge * e3 = new MyEdge;

    e1->setId(1);
    e1->setVertex(0, v1);
    e1->setVertex(1,v2);
    e1->setMeasurement(1);
    e1->setInformation(Eigen::Matrix<double,1,1>::Identity());

    e2->setId(2);
    e2->setVertex(0, v2);
    e2->setVertex(1, v3);
    e2->setMeasurement(1.0);
    e2->setInformation(Eigen::Matrix<double,1,1>::Identity());

    e3->setId(3);
    e3->setVertex(0, v3);
    e3->setVertex(1, v1);
    e3->setMeasurement(-1.8);
    e3->setInformation(Eigen::Matrix<double,1,1>::Identity());
    optimizer.addEdge(e1);
    optimizer.addEdge(e2);
    optimizer.addEdge(e3);

    cout << "start optimization" << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout << v1->estimate() << ' ' << v2->estimate() << ' ' << v3->estimate() << endl;
    return 0;

}