首页 > 其他分享 >g2o优化库实现曲线拟合

g2o优化库实现曲线拟合

时间:2023-06-06 17:11:18浏览次数:43  
标签:曲线拟合 partial double sigma data g2o include 优化

g2o优化库实现曲线拟合

最近学习了一下g20优化库的基本使用,尝试着自己写了一个曲线拟合的函数,也就是下面这个多项式函数:

\[y = ax^3 + bx^2 + cx + d \]

我们以 \(a = 3, b = -2, c=5, b=7\)为例,拟合出的图像大概长这样。image-20230606161854820

下面简单记录一下思路:

目标函数:

\[\min _{a, b, c,d} \frac{1}{2} \sum_{i=1}^N\left\|y_i-(ax_i^3 + bx_i^2 + cx_i + d))\right\|^2 . \]

目标就是求解出一组 \(a,b,c,d\)使得观测值和估计值的误差最小化。

  • 高斯牛顿法

    推导出每个误差项关于优化变量的导数:

    \[\begin{aligned} & \frac{\partial e_i}{\partial a}=-x_i^3\\ & \frac{\partial e_i}{\partial b}=-x_i^2 \\ & \frac{\partial e_i}{\partial c}=-x_i \\ & \frac{\partial e_i}{\partial d}=-1 \end{aligned} \]

    于是 \(\boldsymbol{J}_i=\left[\frac{\partial e_i}{\partial a}, \frac{\partial e_i}{\partial b}, \frac{\partial e_i}{\partial c} \frac{\partial e_i}{\partial d}, \right]^{\mathrm{T}}\), 高斯牛顿法的增量方程为:

    \[\left(\sum_{i=1}^{N} \boldsymbol{J}_i\left(\sigma^2\right)^{-1} \boldsymbol{J}_i^{\mathrm{T}}\right) \Delta \boldsymbol{x}_k=\sum_{i=1}^{N}-\boldsymbol{J}_i\left(\sigma^2\right)^{-1} e_i, \]

    代码在附录里,假设要估计的 \(a=3, b=-2,c=5,d=7\),大概只需要三次迭代就能得到估计的结果。
    image-20230606164222918

  • g2o

    优化变量构成一个顶点,每个误差项构成一条边。

    顶点类需要重写变量的重置,更新函数;

    边类需要设置关联的顶点,重写误差计算函数,必要时也可以给出解析导数。

​ 下面我们主要测试一下是否给出误差项关于优化变量的解析导数对g2o求解次数的影响(对应的就是g2o_curve.cpp中雅各比矩阵是否注释)

不提供解析导数 提供解析导数
image-20230606165949202 image-20230606170057915
0.01s 0.003s

我们可以看到速度差别还挺大的。

附录

gaussNewton.cpp

//
// Created by xin on 23-6-6.
// use g2o curve fitting y = a*x_3 + b*x_2 + c*x + d
//
#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;


int main() {

    double a = 3.0, b = -2.0, c = 5.0, d = 7; // 真实参数值
    double ae = 1, be = 1, ce = 1, de = 100;  // 初始参数值

    int N = 500; //数据点个数
    double w_sigma = 1.0;
    double inv_sigma = 1.0 / w_sigma;
    cv::RNG rng;

    vector<double> x_data, y_data;
    for (int i = 0; i < N; i++) {
        double x = double(i);
        x_data.push_back(x);
        y_data.push_back(a * pow(x, 3) + b * pow(x, 2) + c*x + d + rng.gaussian(w_sigma * w_sigma));
       //std::cout << x << "," << a * pow(x, 3) + b * pow(x, 2) + c * x + d + rng.gaussian(w_sigma * w_sigma) << endl;
    }

    // 开始Gauss-Newton迭代
    int iterations = 1000;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for (int iter = 0; iter < iterations; iter++) {

        Eigen::Matrix4d H = Eigen::Matrix4d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton
        Eigen::Vector4d b = Eigen::Vector4d::Zero();             // bias
        cost = 0;

        for (int i = 0; i < N; i++) { //求每一个数据点的 J 矩阵
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            double error = yi - (ae*pow(xi, 3) + be*pow(xi, 2) + ce*xi + de);
            Eigen::Vector4d J; // 雅可比矩阵
            J[0] = -pow(xi, 3);
            J[1] = -pow(xi, 2);
            J[2] = -xi;
            J[3] = -1;

            H += inv_sigma * inv_sigma * J * J.transpose();
            b += -inv_sigma * inv_sigma * error * J;

            cost += error * error;
        }

        // 求解线性方程 Hx=b
        Eigen::Vector4d dx = H.ldlt().solve(b);
        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost) {
            cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
            break;
        }

        ae += dx[0];
        be += dx[1];
        ce += dx[2];
        de += dx[3];

        lastCost = cost;

        cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
             "\t\testimated params: " << ae << "," << be << "," << ce << "," << de << endl;
    }

    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;

    cout << "estimated abcd = " << ae << ", " << be << ", " << ce  << "," << de << endl;
    return 0;


    return 0;

}

g2o_curve.cpp

//
// Created by xin on 23-6-6.
// use g2o curve fitting y = a*x_3 + b*x_2 + c*x + d
//
#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<4, Eigen::Vector4d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    // 重置
    virtual void setToOriginImpl() override {
        _estimate << 0, 0, 0, 0;
    }

    // 更新
    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Vector4d(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]); //此处拿到关联的顶点
        Eigen::Vector4d abcd = v->estimate();
        _error(0, 0) =
                _measurement - (abcd[0] * pow(_x, 3) + abcd[1] * pow(_x, 2) + abcd[2] * _x + abcd[3]);
    }

    // 雅各比矩阵,可以注释掉对比一下求解速度
    virtual void linearizeOplus() override {
        _jacobianOplusXi[0] = -_x * _x * _x;
        _jacobianOplusXi[1] = -_x * _x;
        _jacobianOplusXi[2] = -_x;
        _jacobianOplusXi[3] = -1;
    }

    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}


public:
    double _x; // 这个误差项对应的自变量x
};


int main() {

    double a = 3.0, b = -2.0, c = 5.0, d = 7; // 真实参数值
    double ae = 1, be = 1, ce = 1, de = 100;  // 初始参数值

    int N = 500; //数据点个数
    double w_sigma = 1.0;
    double inv_sigma = 1.0 / w_sigma;
    cv::RNG rng;

    vector<double> x_data, y_data;
    for (int i = 0; i < N; i++) {
        double x = double(i);
        x_data.push_back(x);
        y_data.push_back(a * pow(x, 3) + b * pow(x, 2) + c*x + d + rng.gaussian(w_sigma * w_sigma));

       std::cout << x << "," << a * pow(x, 3) + b * pow(x, 2) + c * x + d + rng.gaussian(w_sigma * w_sigma) << endl;
    }

    // 构建图优化
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<4, 1>> BlockSolverType;
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型 dense CSparse

    //梯度下降方法
    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->setId(0);
    v->setEstimate(Eigen::Vector4d(ae, be, ce, de));
    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 << "starting optimization" << endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    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;


    // output 结果
    Eigen::Vector4d abcd_estimate = v->estimate();
    cout << "estimated model" << abcd_estimate.transpose() << endl;
    return 0;

}

标签:曲线拟合,partial,double,sigma,data,g2o,include,优化
From: https://www.cnblogs.com/programmerwang/p/17461105.html

相关文章

  • [ABAQUS有限元分析]挤扩支盘桩支护基坑优化设计方法
    ​工程背景为探讨基坑支护工程中挤扩支盘桩的优化设计方案,结合室内模型试验结果,利用ABAQUS软件模拟支盘桩的成桩过程及成桩后基坑开挖过程,分析桩体受力特征及桩后土体变形特征,进而探讨桩土作用机制。模型设计平面图如图1所示。   混凝土桩与土......
  • 浅谈 ByteHouse Projection 优化实践
    预聚合是OLAP系统中常用的一种优化手段,在通过在加载数据时就进行部分聚合计算,生成聚合后的中间表或视图,从而在查询时直接使用这些预先计算好的聚合结果,提高查询性能,实现这种预聚合方法大多都使用物化视图来实现。Clickhouse社区实现的Projection功能类似于物化视图,原始的概念......
  • m基于遗传优化的凸松弛算法完成从二维人体图像中提取三维姿态的matlab仿真
    1.算法仿真效果matlab2022a仿真结果如下:   2.算法涉及理论知识概要      三维姿态估计是计算机视觉领域中一个非常重要的问题,它在许多应用中都具有重要的作用,如人机交互、姿态识别、动作捕捉等。在过去的几年中,随着深度学习技术的发展,基于深度学习的方法取得了......
  • m基于遗传优化的凸松弛算法完成从二维人体图像中提取三维姿态的matlab仿真
    1.算法仿真效果matlab2022a仿真结果如下:2.算法涉及理论知识概要三维姿态估计是计算机视觉领域中一个非常重要的问题,它在许多应用中都具有重要的作用,如人机交互、姿态识别、动作捕捉等。在过去的几年中,随着深度学习技术的发展,基于深度学习的方法取得了很大的进展,但是这些方法仍......
  • SQL 优化
    ————————————————版权声明:本文为CSDN博主「biyusr」的原创文章,遵循CC4.0BY-SA版权协议,转载请附上原文出处链接及本声明。原文链接:https://blog.csdn.net/biyusr/article/details/125599865  1、对查询进行优化,应尽量避免全表扫描,首先应考虑在where及ord......
  • MySQL数据库表结构优化方式详解
    前言从今天开始本系列文章就带各位小伙伴学习数据库技术。数据库技术是Java开发中必不可少的一部分知识内容。也是非常重要的技术。本系列教程由浅入深,全面讲解数据库体系。非常适合零基础的小伙伴来学习。全文大约【2083】字,不说废话,只讲可以让你学到技术、明白原理的纯干......
  • 2021-08-12--Web前端性能指标和性能优化(综述)
    title:网站的几个性能指标和优化(简易)categories:-网络安全与性能优化tags:-性能优化-性能指标-白屏时间-首屏时间-TTFBabbrlink:5c56date:2021-08-1223:42:49updated:2021-08-1223:42:49来源:https://m.sohu.com/a/201865334_509523/关于......
  • 记一次618军演压测TPS上不去排查及优化
    本文内容主要介绍,618医药供应链质量组一次军演压测发现的问题及排查优化过程。旨在给大家借鉴参考。背景本次军演压测背景是,2B业务线及多个业务侧共同和B中台联合军演。现象当压测商品卡片接口的时候,cpu达到10%,TPS只有240不满足预期指标,但是TP99已经达到了1422ms。排查对于......
  • Andrid listview异步图片加载之优化篇
    关于listview的异步加载,网上其实很多示例了,总体思想差不多,不过很多版本或是有bug,或是有性能问题有待优化。有鉴于此,本人在网上找了个相对理想的版本并在此基础上进行改造,下面就让在下阐述其原理以探索个中奥秘,与诸君共赏…        贴张效果图先:         异步加载......
  • 4.1 优化程序的方法
    消除循环的低效率代码移动是程序优化的一种方法,包括识别要执行多次(在循环中)但是不会改变计算结果的计算,因而可以将计算移动到代码前面不会被多次求值的部分。例如将循环中strlen()函数的返回值赋给一个变量,就不用每次循环都执行一次strlen()操作。减少过程调用过程调用会带来开销,而......