AI 助理
备案控制台
开发者社区 人工智能 文章 正文

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

2023-01-16 484
版权
版权声明：
本文内容由阿里云实名注册用户自发贡献，版权归原作者所有，阿里云开发者社区不拥有其著作权，亦不承担相应法律责任。具体规则请查看《 阿里云开发者社区用户服务协议》和 《阿里云开发者社区知识产权保护指引》。如果您发现本社区中有涉嫌抄袭的内容，填写 侵权投诉表单进行举报，一经查实，本社区将立刻删除涉嫌侵权内容。
简介： G2O (General Graph Optimization)入门及简单使用

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

定义顶点与边

G2O已经给我们内置定义好了很多类型的顶点与边，但是我们在使用过程中可能要根据自己的需要重新定义。例如，我们想要求解一个曲线拟合优化的问题，曲线的真实方程为：

                      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()
文章标签：
算法
xchwang1998
目录
相关文章
bruce_xiaowei
|
2月前
|
Python
Python Tricks : How to Write Debuggable Decorators
Python Tricks : How to Write Debuggable Decorators
bruce_xiaowei
13 1
bruce_xiaowei
|
2月前
|
Go C# Python
Python Tricks ：Lambdas Are Single-Expression Functions 原创
Python Tricks ：Lambdas Are Single-Expression Functions 原创
bruce_xiaowei
14 0
泡沫o0
|
7月前
|
缓存 算法 C语言
【C++ 标准查找算法 】C++标准库查找算法深入解析（In-depth Analysis of C++ Standard Library Search Algorithms）
【C++ 标准查找算法 】C++标准库查找算法深入解析（In-depth Analysis of C++ Standard Library Search Algorithms）
泡沫o0
107 0
诸神缄默不语
|
机器学习/深度学习 PyTorch API
Dynamic Quantization PyTorch官方教程学习笔记
本文是PyTorch的教程Dynamic Quantization — PyTorch Tutorials 1.11.0+cu102 documentation的学习笔记。事实上由于我对该领域的不了解，本篇笔记基本上就是翻译+一点点我对其的理解。 本文介绍如何用Dynamic Quantization加速一个LSTM1模型的推理过程。Dynamic Quantization减少了模型权重的尺寸，加速了模型处理过程。 本文代码及全部输出已以jupyter notebook格式展示在GitHub上，可以直接下载运行：all-notes-in-one/dynamicquantization.ipyn
诸神缄默不语
430 0
遥凌
|
算法 前端开发 调度
The Cascades Framework for Query Optimization
这篇paper是前一篇Volcano optimizer的后续，其涉及的概念和优化思路是一脉相承的，在阅读本篇之前，最好对Volcano optimizer有足够的了解，详见文章Volcano优化器框架。
遥凌
575 0
莫源
|
存储 API 数据库
Learn Influxdb the hard way (1) - Overview of the architecture
莫源
9252 0
莫源
|
Go 时序数据库
Learn Influxdb the hard way (2) - Dive into the code backbone
莫源
2257 0
余二五
|
安全 图形学
R-Language（R语言或称r-project）的安装
余二五
1139 0
我是wangxiao-23158
|
决策智能
论文笔记：Multi-Agent Actor-Critic for Mixed Cooperative-Competitive Environments
Multi-Agent Actor-Critic for Mixed Cooperative-Competitive Environments 2017-10-25  16:38:23     　【Project Page】https://blog.
我是wangxiao-23158
2450 0
真正的迪迦
|
Python C语言 .NET
Python chapter 8 learning notes
版权声明：本文为博主原创文章，原文均发表自http://www.yushuai.me。未经允许，禁止转载。 https://blog.csdn.net/davidcheungchina/article/details/78267298 ...
真正的迪迦
987 0

热门文章

最新文章

  • 1
    Gitlab----使用Docker方式安装部署Gitlab
  • 2
    Mac搭建安卓模拟器（支持M1/M2/M3/M4）
  • 3
    java报错javax.net.ssl.SSLHandshakeException:  sun.security.validator.ValidatorException
  • 4
    开发人员各级岗位胜任力模型
  • 5
    Hive之数据倾斜的原因和解决方法
  • 6
    【Java】留下没有基础眼泪的面试题
  • 7
    iptables系列教程（二）| iptables语法规则
  • 8
    13.4. 临时表是否需要建索引
  • 9
    共享的两种方式
  • 10
    Andriod动态布局
  • 1
    Linux 各发行版安装 ping 命令指南
    37
  • 2
    Linux缓存管理：如何安全地清理系统缓存
    97
  • 3
    《CMake：掌控 C++人工智能项目编译的魔法棒》
    31
  • 4
    《C++人工智能开发 IDE 全解析：助力智能创新之路》
    35
  • 5
    《解锁 Eigen 库在 C++人工智能项目中的潜能与优化之道》
    33
  • 6
    《解析 MXNet 的 C++版本在分布式训练中的机遇与挑战》
    22
  • 7
    《探索 Caffe2 的 C++接口在移动设备上的性能优化之路》
    26
  • 8
    docker高级篇（大厂进阶）：安装redis集群
    42
  • 9
    Lyra：SmartMore 联合香港多所高校推出的多模态大型语言模型，专注于提升语音、视觉和语言模态的交互能力
    55
  • 10
    LatentLM：微软联合清华大学推出的多模态生成模型，能够统一处理和生成图像、文本、音频和语音合成
    44

    • 相关电子书

    更多
  • NLP with MLlib--Global Empire Building for Fun and Profit
  • Improving Python and Spark Performance and Interoperability
  • Practical tips for defending
    • 下一篇
    fs.oss.accessKeyId和fs.oss.accessKeySecret。