g2o入门

g2o是什么，能干什么

g2o是一个通用的求解器，并不限定于某些SLAM问题。可以用图来表达的优化问题都能用g2o解决。

基本步骤

1. 在主程序运行之前：定义节点、边，包括内部的初始化函数、更新函数、误差计算函数、输入输出函数等等；

2. 在主程序内部：实例化g2o求解器、选择迭代求解方式、实例化所使用的节点与边来逐步建立图模型、设置迭代次数并开始求解。

节点，边

节点可以认为是待优化的变量，边用来连接节点，且为误差项。

库结构

1. SparseOptimizer 是一个Optimizable Graph，从而也是一个Hyper Graph。

2. 一个 SparseOptimizer 含有很多个顶点 （都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。

3. 我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

4. 在优化之前，需要指定我们用的求解器和迭代算法。一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一（我们常用的是GN或LM）。

5. 同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞； 一个是用于计算迭代步长 $H\Delta x=-b$，这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一。

代码示例

给定一组数据，拟合曲线，求解参数，公式为$y=a\ast e^{-lambda\ast x}+b$

1. 首先定义顶点

曲线模型的顶点：共有继承自g2o::BaseVertex，所携带的待优化变量个数为3，存储形式为vector3d。

定义了这个节点的重置函数与更新函数，以及最后的读写函数。这里重置函数、更新函数、读写函数都不是咱们用户自己调用的，而是在迭代优化过程中由g2o求解器去调用的，这里只要写出来摆在这就好（其中更新函数只是将当前值与更新增量进行加法，是迭代求解的思想）。

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

  virtual bool read(std::istream& /*is*/) { return false; }

  virtual bool write(std::ostream& /*os*/) const { return false; }

  virtual void setToOriginImpl() {}

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

2. 定义边

这里定义的边是一个一元边，继承于BaseUnaryEdge。后面尖括号里的<1, Eigen::Vector2d, VertexParams>则分别是：误差项维数、输入数据的变量形式、与这条边相连的节点类型（上面定义的）。

进而定义了这个边的一个构造函数和读写函数。

最后开始通过重载（）操作，来定义最关键的误差计算函数：这里的params则是待优化变量，也就是刚定义好的节点中存储的三个变量。

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;
      }
    
      template <typename T>
      bool operator()(const T* params, T* error) const {
        const T& a = params[0];
        const T& b = params[1];
        const T& lambda = params[2];
        T fval = a * exp(-lambda * T(measurement()(0))) + b;
        error[0] = fval - measurement()(1);
        return true;
      }
    
      G2O_MAKE_AUTO_AD_FUNCTIONS  // use autodiff
};

来看看误差计算函数中的具体语句：用a,b,lambda掏出这个节点内部的待优化变量，然后则是完成上面公式的误差运算，其中真实值是通过_measurement体现的（这个measurement值怎么进来的在后文会有解释）。

节点与边的定义中都出现了EIGEN_MAKE_ALIGNED_OPERATOR_NEW，这里是为了解决在new一个这样类型的对象时解决对齐问题。

G2O_MAKE_AUTO_AD_FUNCTIONS表示使用自动求导

3. 准备工作

在主函数里：

参数设置：

int numPoints;
  int maxIterations;
  bool verbose;
  string dumpFilename;
  g2o::CommandArgs arg;
  arg.param("dump", dumpFilename, "", "dump the points into a file");
  arg.param("numPoints", numPoints, 50, "number of points sampled from the curve");
  arg.param("i", maxIterations, 10, "perform n iterations");
  arg.param("v", verbose, false, "verbose output of the optimization process");

  arg.parseArgs(argc, argv);

生成数据

// generate random data
  g2o::Sampler::seedRand();
  double a = 2.;
  double b = 0.4;
  double lambda = 0.2;
  Eigen::Vector2d* points = new Eigen::Vector2d[numPoints];
  
  for (int i = 0; i < numPoints; ++i) {
    double x = g2o::Sampler::uniformRand(0, 10);
    double y = a * exp(-lambda * x) + b;
    // add Gaussian noise
    y += g2o::Sampler::gaussRand(0, 0.02);
    points[i].x() = x;
    points[i].y() = y;
  }

4. 构建图优化

首先实例化一个g2o::SparseOptimizer类的 optimizer优化器，关闭调试输出。

而一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell’s dogleg 三者之一，因此这里为优化器分配迭代算法LM。

 // setup the solver
  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(false);// 关闭调试输出

  // allocate the solver
  g2o::OptimizationAlgorithmProperty solverProperty;
  optimizer.setAlgorithm(
            g2o::OptimizationAlgorithmFactory::instance()->construct("lm_dense", solverProperty));

这里添加了图模型中所使用的顶点，并将其命名为顶点params。通过调用params中的setEstimate（）函数将待优化变量的初始值设定为1，1，1，并将这个顶点在整个求解器中的id设置为0，最后添加到求解器中。

  // build the optimization problem given the points
  // 1. add the parameter vertex
  VertexParams* params = new VertexParams();
  params->setId(0);
  params->setEstimate(Eigen::Vector3d(1, 1, 1));  // some initial value for the params
  optimizer.addVertex(params);

这里开始往图模型中添加边。
第一行是在实例化这条边；
第二行是信息矩阵，在此默认为一个1*1的矩阵；
第三行是将这条边与节点params相连，并让params为这条边连接的第一个节点（从0开始计数）；
第四行则是通过边内的函数setMeasurement（），将数据对points[i]输入进去来计算误差；
最后一行是将设置好的边添加进入求解器。

  // 2. add the points we measured to be on the curve
  for (int i = 0; i < numPoints; ++i) {
    EdgePointOnCurve* e = new EdgePointOnCurve;
    e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
    e->setVertex(0, params);
    e->setMeasurement(points[i]);
    optimizer.addEdge(e);
  }

设置优化器参数，进行优化

  // perform the optimization
  optimizer.initializeOptimization(); // 初始化
  optimizer.setVerbose(verbose); //设置调试输出
  optimizer.optimize(maxIterations); //根据最大迭代次数进行优化

  if (verbose) cout << endl;

  // print out the result
  cout << "Target curve" << endl;
  cout << "a * exp(-lambda * x) + b" << endl;
  cout << "Iterative least squares solution" << endl;
  cout << "a      = " << params->estimate()(0) << endl;
  cout << "b      = " << params->estimate()(1) << endl;
  cout << "lambda = " << params->estimate()(2) << endl;
  cout << endl;

  // clean up
  delete[] points;

小结

步骤：

1. 自定义顶点和边
2. 实例化一个g2o::SparseOptimizer类的 optimizer优化器，为优化器分配迭代算法
3. 实例化顶点，并加入优化器中
4. 实例化边，设置信息矩阵，与顶点连接，传入数据，加入优化器
5. 设置优化器参数，进行优化。

http://www.360doc.cn/article/73571518_959376677.html

https://mp.weixin.qq.com/s/j9h9lT14jCu-VvEPHQhtBw