g2o全称是General Graph Optimization，也就是图优化，我们在做SLAM后端或者更加常见的任何优化问题（曲线拟合）都可以使用G2O进行处理。

先放出本文的几个参考链接：

半闲居士（高翔博士）

非线性优化库g2o使用教程，探索一些常见的用法，以及信息矩阵、鲁棒核函数对于优化的结果的影响

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、DL）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分：

一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞矩阵；
一个是LinearSolver用于计算迭代过程中最关键的一步，添加状态改正量；

H△x=−b.H△x = -b. H△x=−b.

准备数据

准备要进行拟合的数据，加上噪声：

    int numPoints = 200;double a = 1.;double b = 2;double c = 3;Eigen::Vector2d *points = new Eigen::Vector2d[numPoints];ofstream points_file("../points.txt", ios::out);//准备用于拟合的数据  加上噪声for (int i = 0; i < numPoints; ++i) {double x = g2o::Sampler::uniformRand(0, 10);double y = sin(a*x) + cos(b*x) + c;y += g2o::Sampler::gaussRand(0, 0.1);points[i].x() = x;points[i].y() = y;points_file << x << " " << y << endl;}points_file.close();

定义顶点与边

G2O已经给我们内置定义好了很多类型的顶点与边，但是我们在使用过程中可能要根据自己的需要重新定义。例如，我们想要求解一个曲线拟合优化的问题，曲线的真实方程为：
y=sin(ax)+cos(bx)+c.其中a=1,b=2,c=3y= sin(ax)+cos(bx)+c. 其中a = 1,b = 2,c=3 y=sin(ax)+cos(bx)+c.其中a=1,b=2,c=3
则针对这一问题，顶点为我们所需要求解的a,b,c，边就为预测值与观测值之间的差值。

构造顶点：

class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;VertexParams() = default;bool read(std::istream & /*is*/) override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}bool write(std::ostream & /*os*/) const override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}//该函数作用是更新顶点的估计值void setToOriginImpl() override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;}//更新优化之后的顶点void oplusImpl(const double *update) override {Eigen::Vector3d::ConstMapType v(update);_estimate += v;}
};

构造边：

/*!* 从BaseUnaryEdge继承得到一元边*/
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWEdgePointOnCurve() = default;bool read(std::istream & /*is*/) override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}bool write(std::ostream & /*os*/) const override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}//边的误差计算void computeError() override {const VertexParams *params = dynamic_cast<const VertexParams *>(vertex(0));//顶点const double &a = params->estimate()(0);const double &b = params->estimate()(1);const double &c = params->estimate()(2);// double fval = a * exp(-lambda * measurement()(0)) + b;double fval = sin(a * measurement()(0)) + cos(b * measurement()(0)) + c;_error(0) = std::abs(fval - measurement()(1));}
};

构建G2O优化器

g2o在使用过程中主要包括三种数据：

顶点：待优化的变量（状态）
边：顶点之间的约束关系，常用误差表示
求解器：线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹

因此，在g2o优化器定义过程中可以通过下述步骤实现：

    g2o::SparseOptimizer optimizer;// 优化器类型为LMstring solver_type = "lm_var";// 优化器生成器g2o::OptimizationAlgorithmFactory *solver_factory = g2o::OptimizationAlgorithmFactory::instance();// 存储优化器性质g2o::OptimizationAlgorithmProperty solver_property;// 生成优化器g2o::OptimizationAlgorithm *solver = solver_factory->construct(solver_type, solver_property);optimizer.setAlgorithm(solver);// 判断是否构建成功if (!optimizer.solver()) {std::cout << "G2O 优化器创建失败！" << std::endl;}

设置初值，添加顶点与边

针对我们目前要求解的这一问题，顶点为我们所需要拟合的曲线系数a,b,c，边就为预测值（拟合出a,b,c之后代入公式计算的预测值）与观测值（生成的带噪声数据）之间的差值。

 VertexParams *params = new VertexParams();params->setId(0);params->setEstimate(Eigen::Vector3d(0.7, 2.4, 2));//初始化顶点的估计值// 添加顶点（待求解的a b c）optimizer.addVertex(params);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);}

添加的顶点只有一个，边有很多条，其中，我们所使用的边为一元边，其链接的顶点只是一个。所构成的图如下所示：

优化

使用设置好的优化器进行优化：

    optimizer.initializeOptimization();optimizer.computeInitialGuess();optimizer.computeActiveErrors();optimizer.setVerbose(false);optimizer.optimize(maxIterations);

优化后的结果如下图所示，散点代表观测量，红色曲线为拟合的结果：

G2O优化源代码

#include <Eigen/Core>
#include <iostream>#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include <g2o/core/optimization_algorithm_factory.h>
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/core/robust_kernel_impl.h"using namespace std;// linerSolver三种求解器，用于计算迭代过程中最关键的一步HΔx=−b
G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
G2O_USE_OPTIMIZATION_LIBRARY(csparse)/*!* 继承BaseVertex类，构造顶点*/
class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;VertexParams() = default;bool read(std::istream & /*is*/) override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}bool write(std::ostream & /*os*/) const override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}//该函数作用是更新顶点的估计值void setToOriginImpl() override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;}//更新优化之后的顶点void oplusImpl(const double *update) override {Eigen::Vector3d::ConstMapType v(update);_estimate += v;}
};/*!* 从BaseUnaryEdge继承得到一元边*/
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWEdgePointOnCurve() = default;bool read(std::istream & /*is*/) override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}bool write(std::ostream & /*os*/) const override {cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}//边的误差计算void computeError() override {const VertexParams *params = dynamic_cast<const VertexParams *>(vertex(0));//顶点const double &a = params->estimate()(0);const double &b = params->estimate()(1);const double &c = params->estimate()(2);// double fval = a * exp(-lambda * measurement()(0)) + b;double fval = sin(a * measurement()(0)) + cos(b * measurement()(0)) + c;_error(0) = std::abs(fval - measurement()(1));}
};int main(int argc, char **argv) {int numPoints = 200;int maxIterations = 50;bool verbose = true;double a = 1.;double b = 2;double c = 3;Eigen::Vector2d *points = new Eigen::Vector2d[numPoints];ofstream points_file("../points.txt", ios::out);//准备用于拟合的数据  加上噪声for (int i = 0; i < numPoints; ++i) {double x = g2o::Sampler::uniformRand(0, 10);double y = sin(a*x) + cos(b*x) + c;y += g2o::Sampler::gaussRand(0, 0.1);// if (i == 20) {//     x = 8;//     y = 2.5;// }points[i].x() = x;points[i].y() = y;points_file << x << " " << y << endl;}points_file.close();g2o::SparseOptimizer optimizer;// 优化器类型string solver_type = "lm_var";// 优化器生成器g2o::OptimizationAlgorithmFactory *solver_factory = g2o::OptimizationAlgorithmFactory::instance();// 存储优化器性质g2o::OptimizationAlgorithmProperty solver_property;// 生成优化器g2o::OptimizationAlgorithm *solver = solver_factory->construct(solver_type, solver_property);optimizer.setAlgorithm(solver);if (!optimizer.solver()) {std::cout << "G2O 优化器创建失败！" << std::endl;}VertexParams *params = new VertexParams();params->setId(0);params->setEstimate(Eigen::Vector3d(0.7, 2.4, 2));//初始化顶点的估计值optimizer.addVertex(params);for (int i = 0; i < numPoints; ++i) {EdgePointOnCurve *e = new EdgePointOnCurve;e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());// if (i == 20) {//     e->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 10);// }e->setVertex(0, params);e->setMeasurement(points[i]);// g2o::RobustKernelHuber *robust_kernel_huber = new g2o::RobustKernelHuber;// robust_kernel_huber->setDelta(0.1);// e->setRobustKernel(robust_kernel_huber);optimizer.addEdge(e);}optimizer.initializeOptimization();optimizer.computeInitialGuess();optimizer.computeActiveErrors();optimizer.setVerbose(false);optimizer.optimize(maxIterations);ofstream result_file("../result.txt");result_file << params->estimate()[0] << " "<< params->estimate()[1] << " "<< params->estimate()[2];result_file.close();cout << endl << "a, b, c: "<< params->estimate()[0] << ", "<< params->estimate()[1] << ", "<< params->estimate()[2] << endl;delete[] points;return 0;
}

画图源代码

import numpy as np
import matplotlib.pyplot as pltfilename = './points.txt'
X, Y = [], []
with open(filename, 'r') as f:lines = f.readlines()for line in lines:value = [float(s) for s in line.split()]X.append(float(value[0]))Y.append(float(value[1]))result_name = './result.txt'
with open(result_name, 'r') as r:lines = r.readlines()for line in lines:value = [float(s) for s in line.split()]a = float(value[0])b = float(value[1])c = float(value[2])x = np.linspace(0, 10, 100)
y = np.sin(a*x) + np.cos(b*x) + cplt.plot(x, y, 'r')
plt.scatter(X, Y)
plt.show()

G2O (General Graph Optimization)入门及简单使用相关推荐

g2o图优化库入门介绍
g2o图优化库入门介绍 1.背景知识介绍 2.代码详解一.点和边的类型定义二.构建图优化实例,配置求解器三.添加点和边四.执行优化 3.ax2+bx+c实现一.程序: 二.运行结果 1.背景 ...
TVM 图优化Graph Optimization
TVM 图优化Graph Optimization Codegen
webpack入门之简单例子跑起来
webpack入门之简单例子跑起来 webpack介绍 Webpack是当下最热门的前端资源模块化管理和打包工具,它可以将很多松散的模块按照依赖和规则打包成符合生产环境部署的前端资源,还可以将按需加载 ...
Dubbo入门(2) - 简单实践
作者:不洗碗工作室 - Marklux 出处:Dubbo入门(2) - 简单实践版权归作者所有,转载请注明出处在了解什么是分布式框架之后,我们需要上手实践一下,来了解整个系统是如何运作起来的. 本 ...
简单php修改mysql数据类型_MySQL入门很简单—MySQL数据类型
MySQL入门很简单---MySQL数据类型一.整数类型: //类型名称字节数(长度)无符号数的取值范围有符号的取值范围 TINYNIT 1 (4)0~255-128~127 SMALLINT2 ( ...
mysql入门很简单(一)
之前一直都只会一些mysql简单的操作,对mysql的理解也不是很熟悉,找了很多mysql的基础书籍,也不是很理想,后来发现一本<mysql入门很简单>发现还不错,看了两遍,mysql基础 ...
《Java Web开发入门很简单》学习笔记
<Java Web开发入门很简单>学习笔记 1123 第1章了解Java Web开发领域 Java Web主要涉及技术包括:HTML.JavaScript.CSS.JSP.Servlet ...
html+css+小图标,HTML+CSS入门一个简单实用的CSS loading图标
本篇教程介绍了HTML+CSS入门一个简单实用的CSS loading图标,希望阅读本篇文章以后大家有所收获,帮助大家HTML+CSS入门.< 在web开发中,为了提高用户体验,在加载数据的时 ...
踢爆IT劣书出版黑幕——由清华大学出版社之《C语言入门很简单》想到的（1）...
1.前言与作者首先声明,我是由于非常偶然的机会获得<C语言入门很简单>这本书的,绝对不是买的.买这种书实在丢不起那人. 去年这书刚出版时,在CU论坛举行试读推广,我当时随口说了几句(没说 ...

G2O (General Graph Optimization)入门及简单使用