首页 > 其他分享> 文章详细

g2o入门(一)曲线拟合

2022-01-28 11:04:04 阅读：363 来源： 互联网

标签：曲线拟合 abc const 入门 double edge g2o CurveFittingVertex

简介

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，优化参数的类型为Eigen::Vector3d

2、virtual void setToOriginImpl()设置初始值

给待估计的节点设置初始值，本例中将节点设置为(0,0,0)

3、virtual void oplusImpl(const double *update)设置更新方式

采用相加的方式更新

3.2、定义边

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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
};

首先明确边代表误差项

1、定义自己的边

class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>

因为只有一个节点所以继承一元边。边代表的误差如下：

所以维度为1，类型设为double，节点是 CurveFittingVertex

2、设置误差

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));
  }

_vertices[0] 对应 “class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>”绑定的第一个顶点 CurveFittingVertex

变量abc即为优化参数

3、计算雅可比

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

3.3、构建图

 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);  // 添加
 }

1、求解器

typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;
// 每个误差项优化变量维度为3，误差值维度为1；

typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
// 线性求解器类型

g2o::OptimizationAlgorithmGaussNewton
// GN
2、增加节点

CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

3、增加边

 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);  // 添加
 }

3.4 、进行优化

		// 执行优化
		  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;
}

3.5、完整代码

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

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

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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;
}

参考

slam 基本算法 --- 分别使用【高斯牛顿，g2o】进行曲线拟合（理论+实践）_CGJustDoIT的博客-CSDN博客

深入理解图优化与g2o：g2o篇 - 半闲居士 - 博客园

slam十四讲

标签：曲线拟合,abc,const,入门,double,edge,g2o,CurveFittingVertex
来源： https://blog.csdn.net/dushib/article/details/122719612

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9