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

大家好，欢迎来到IT知识分享网。

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.$$

准备数据

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

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

定义顶点与边

• 构造顶点：

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_NEW EdgePointOnCurve() = 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; // 优化器类型为LM 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; } 

设置初值，添加顶点与边

针对我们目前要求解的这一问题，顶点为我们所需要拟合的曲线系数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_NEW EdgePointOnCurve() = 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 plt filename = './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) + c plt.plot(x, y, 'r') plt.scatter(X, Y) plt.show()