g2o入门(一)曲线拟合

最新推荐文章于 2023-03-21 15:25:38 发布

原创

最新推荐文章于 2023-03-21 15:25:38 发布 · 4.2k 阅读

33 ·

CC 4.0 BY-SA版权

文章标签：

#图论 #算法 #slam

简介

g2o是一个通用的图优化库，可以应用到任何能够表示成图优化的最小二乘问题。例如BA，icp，曲线拟合等。

但它不像Ceres一样有丰富详细的学习资料。

图优化，是把优化问题表现成图（Graph）（指图论中的图）的一种方式，一组优化变量和变量之间的误差项，使用图来图来描述，使它们之间的关系更加直观化；

三角形和圆圈，都是顶点，待优化的变量；
红色与蓝色的线，都是边，描述相连顶点优化变量关系，也就是误差项；

g2o使用步骤如下：

定义顶点和边的类型；
构建图；
选择优化算法；
调用 g2o 进行优化，返回结果。

先看上半部分。SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点（都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

　　在优化之前，需要指定我们用的求解器和迭代算法。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞；一个是用于计算迭代过程中最关键的一步

曲线拟合问题

1、问题描述

其中a,b,c为待估计的参数，w为噪声。在程序里利用模型生成x，y的数据，在给数据添加服从高斯分布的噪声。之后用ceres优化求解参数a,b,c。

2、g2o基本类

2.1、基本模板点与边

Base Vertex 点

BaseUnaryEdge 边

BaseBinaryEdge 边

BaseMultiEdge 边

2.2、 g2o已经定义好的边和点

在types中已经提供了一些边和节点的定义，例如

相机位姿结点：g2o::VertexSE3Expmap，来自<g2o/types/sba/types_six_dof_expmap.h>；

特征点空间坐标结点：g2o::VertexSBAPointXYZ，来自<g2o/types/sba/types_sba.h>；

重投影误差：g2o::EdgeProjectXYZ2UV，来自<g2o/types/sba/types_six_dof_expmap.h>；

3、代码实现讲解

3.1、自定义顶点

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

BaseVertex为g2o的基本节点类，需要继承从重写他内部的函数从而形成我们想要的新的顶点。

1、class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d>创建自己的节点

后边两个参数分别代表优化参数的维度为3，优化参数的类型为Ei