首页 > 其他分享 >g2o（2）求解曲线y=ax2+bx+c

g2o（2）求解曲线y=ax2+bx+c

时间：2023-11-15 21:46:11浏览次数：32  
标签：ax2 abc const double data g2o include bx

 

https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486858&idx=1&sn=ce458d5eb6b1ad11b065d71899e31a04&chksm=97d7e81da0a0610b1e3e12415b6de1501329920c3074ab5b48e759edbb33d264a73f1a9f9faf&scene=21#wechat_redirect

简要流程 0-0获取数据 x 和 y 0-1确定要优化的量a ,b, c 1-1 获取理论模型 y=ax^2+bx+c 1-2 确定误差方程e =min||f(x+Δx)-f(x)|| 2 根据误差方程e确定一阶导J=2ax+b和二阶导H=2a 矩阵，信息矩阵   3 根据误差方程e , 求解模型（例如高斯牛顿）确定 H和g表达形式    构造 H *Δx= g最小二乘      J(x)*J(x)*Δx=-J(x) f(x)         H* Δx = g 4 求解 Δx=H.inv*g  确定求解H逆的方法 5 跟新 x=x+Δx 6 求解 e =f(x+Δx)-f(x)误差 7 迭代直到小于阈值或者总次数

 

 

 

 

 

源码地址

G2O代码

CMakeLists.txt 
cmake_minimum_required( VERSION 2.8 )
project( g2o_curve_fitting )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找G2O
find_package( G2O REQUIRED )
include_directories( 
    ${G2O_INCLUDE_DIRS}
    "/usr/include/eigen3"
)

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable( curve_fitting main.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting 
    ${OpenCV_LIBS}
    g2o_core g2o_stuff
)

　　

main.cpp 
#include <iostream>
#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<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }
    
    virtual void oplusImpl( const double* update ) // 更新
    {

        _estimate += Eigen::Vector3d(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) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据
    
    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }
    
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出
    
    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    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<<"start 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;
    
    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
    
    return 0;
}

　　

ceres代码

CMakeLists.txt 
cmake_minimum_required( VERSION 2.8 )
project( ceres_curve_fitting )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable( curve_fitting main.cpp )
# 与Ceres和OpenCV链接
target_link_libraries( curve_fitting ${CERES_LIBRARIES} ${OpenCV_LIBS} )

　　

main.cpp 
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
    // 残差的计算
    template <typename T>
    bool operator() (
        const T* const abc,     // 模型参数，有3维
        T* residual ) const     // 残差
    {
        residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
        return true;
    }
    const double _x, _y;    // x,y数据
};

int main ( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    // 构建最小二乘问题
    ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
        problem.AddResidualBlock (     // 向问题中添加误差项
        // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( 
                new CURVE_FITTING_COST ( x_data[i], y_data[i] )
            ),
            nullptr,            // 核函数，这里不使用，为空
            abc                 // 待估计参数
        );
    }

    // 配置求解器
    ceres::Solver::Options options;     // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解
    options.minimizer_progress_to_stdout = true;   // 输出到cout

    ceres::Solver::Summary summary;                // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve ( options, &problem, &summary );  // 开始优化
    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<<summary.BriefReport() <<endl;
    cout<<"estimated a,b,c = ";
    for ( auto a:abc ) cout<<a<<" ";
    cout<<endl;

    return 0;
}

　　

 

 

手撕代码

https://liuxiaofei.com.cn/blog/g2o%E4%BC%98%E5%8C%96%E8%A7%A3%E6%9E%90-%E6%89%8B%E5%8A%A8%E5%BE%AE%E5%88%86/

 

代码

#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 <g2o/stuff/sampler.h>
#include <Eigen/Core>
#include <cmath>
#include <chrono>
 
using namespace std;
 
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  // 重置
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }
 
  // 更新，每一轮迭代后更新参数的值Δx。
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(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]);
    const Eigen::Vector3d abc = v->estimate();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
  }
 
  // 计算雅可比矩阵，和上一篇高斯牛顿法里面的求解方式是一样的。
  virtual void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }
 
  virtual bool read(istream &in) {}
 
  virtual bool write(ostream &out) const {}
 
public:
  double _x;  // x 值， y 值为 _measurement
};
 
int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  g2o::Sampler::seedRand();
 
  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + g2o::Sampler::gaussRand(0, 0.02));//加上一个高斯误差，来表示测量是不准确的。
  }
 
  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
 
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  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->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  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 << "start optimization" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  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;
 
  // 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;
 
  return 0;
}

　　

 

 

标签：ax2,abc,const,double,data,g2o,include,bx
From： https://www.cnblogs.com/gooutlook/p/17834874.html

相关文章

  • asterisk、pbx、sip介绍
    最近在了解一个网络电话的项目，在网上搜索了一些关于这方便的资料，现在记录一下。（通讯行业是一个非常专业的领域，本人虽然做了很多年的开发，但有理解起来还是很吃力）1、viop：VoIP（VoiceoverInternetProtocol）简而言之就是将模拟信号（Voice）数字化，以数据封包（DataPacket）的形式在IP网络(IP......
  • Configuration failed because libxml-2.0 was not found. Try installing:
     001、linux中R语言中安装“devtools”报错如下：Configurationfailedbecauselibxml-2.0wasnotfound.Tryinstalling: 002、解决方法：[root@pc1~]#yuminstalllibxml2-devel##根据提示安装 。 ......
  • 集成电路（IC）MAX98050ENX、MAX22707AUB、MAX17543ATP、MAX40008ANT高效、低功耗器件产品
    1、MAX98050ENX音频编解码器是一款高性能、低功耗器件，集成了低延迟数字滤波器，用于无线耳戴式设备、头戴式设备和耳机。MAX98050具有一个单声道播放通道，带有一个5频段双四路均衡器和一个高效、全差分混合AB/D类耳机放大器。播放耳机放大器经过优化，可以实现最低输出噪声和静态功耗，同......
  • 形如$Ax^2+Bxy+Cy^2+Dx+Ey+F=0$的圆锥曲线
    ......
  • Azure DevOps 发布.Net项目到Windows IIS站点之pubxml
    通过VS中我们配置的pubxml进行发布trigger:-masterpool:vmImage:'windows-2022'variables:-group:GeexPublish-name:my-user-variablevalue:$[variables.UserName]#usesruntimeexpression-name:my-passed-variablevalue:$[variables.PassWord]......
  • react 公司项目学习 【react+webpack+nw.js + mobx+react-intl 实现的多页面多语言win
    这几天突然要来个react项目；听说还比较复杂；项目组内就两个人会react还在忙，整组主要是用vue；这不，这种‘狗都不干’的事，被安排到我身上了，那就学吧；一、研究代码结构不得不说，这目录结构搞得有点复杂，算是我接触中除了乾坤和electron之外，相当复杂的了，慢慢阅读吧；看懂了，原来是react+web......
  • AX2012 正则表达式返回所有匹配的Match对象
    staticvoidTestMatches(Args_args){System.Text.RegularExpressions.MatchCollectionregExMatchs;System.Text.RegularExpressions.MatchregExMatch;System.Collections.IEnumeratormatchesEnum;Str1260......
  • qbxt 突破营 Day7 T3
    小葱想要吃糖，小葱将拿出来的N颗糖排成一排，第\(i\)颗糖的美味值为\(a_i\)。小葱很喜欢吃糖，所以小葱会从\(N\)颗糖选择不超过\(K\)段不相交的区间的糖果吃掉。但是小葱同学不希望别人吃到和他美味度差不多的糖，所以对于一颗没被吃掉的糖，小葱希望这颗糖美味度比他吃的糖的美味度最大......
  • qbxt 突破营 Day7 T2
    小葱将买来的糖放进了冰箱冷藏，但是小葱想吃糖了，小葱希望把自己想吃的糖从冰箱里面拿出来。具体来说，小葱同学的冰箱是一棵\(N\)个点的树，每个点有一颗糖，第\(i\)个点的糖的美味值是\(a_i\)。小葱每次取糖会从根节点出发，指定一个目标节点\(p\)，走到\(p\)点并且把这条路径上的所有糖取......
  • qbxt 突破营 Day1 T4
    考虑经典的俄罗斯方块游戏，二维平面上有若干个积木，他们会受重力的影响下落并堆叠。注意，积木只会竖直下落，如果下落过程中碰到了别的积木那么就会停下。例如下图：不同颜色的块代表了不同的积木，这些积木下落之后会形如下图：积木的形状可以任意的，可能跟传统的俄罗斯方块有一些不同，比......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99