用案例来展示g2o优化库的使用

之前看高博的slam十四讲时候用过g2o，然而后来一直没有用项目来练手已经忘得差不多了，还是得多动手实践呀，加强记忆。写个博客记录一下，下次要用回忆更快一些。

g2o介绍

它是基于图优化的一个开源库，使用非线性优化算法和图论结合的理论。在g2o中，将待优化的变量看成图的顶点，将误差项看成图的边。这么说可能比较难以理解，这里有一篇非常简单的案例用来说明：案例

在我看来，很多问题是很天然的可以用图优化描述的，而且相对ceres，我觉得g2o在解决变量数目非常多的情况，更加来得自然和简单。

不做过多赘述，本文主要是想工程描述一下g2o的基本用法。

案例

案例来自高博的slambook2的曲线拟合例子。

1.   曲线为： y = exp(ax*x + b*x + c) 生成N组数据（x, y）
2.   向y中增加噪声生成新的（x, y）

我按着自己的理解，手写了一个，并添加多一些注释：

#include <iostream>
#include <vector>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <Eigen/Core>

#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>
using namespace std;
// 重要的东西，我们需要自己构建我们问题的顶点和边，模板参数：优化变量维度和数据类型
// 高博源码是Eigen::Vector3d，我使用的是doule数组
class Vertex_fitting : public g2o::BaseVertex<3, double *>
{
   
public:
    virtual bool read(istream &is) override {
   }
    virtual bool write(ostream &os) const override {
   }
    // update function
    // 事关变量是如何更新，这个案例很简单就是 x+dx
    // 如果是位姿图中SE3，那么显然是变换矩阵相乘
    virtual void oplusImpl(const double *update) override
    {
   
        _estimate[0] += update[0];
        _estimate[1] += update[1];
        _estimate[2] += update[2];
    }
    // set origin estimate
    // 顶点重置函数，设定被优化变量的原始值,emmm这个和顶点的setEstimate函数有啥区别，不太懂
    virtual void setToOriginImpl() override
    {
   
        _estimate