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

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. Hx=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 = s i n ( a x ) + c o s ( b x ) + c . 其中 a = 1 , b = 2 , c = 3 y= 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_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()
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

晓晨的博客

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值