自学SLAM（7）非线性优化实践：曲线拟合问题（使用ceres库和SLAM常用的g2o库）

news2024/12/26 16:07:34

前言

在这里插入图片描述

本次文章针对的是第四个视屏中的实践问题

肯定会有部分方法没有说到，比如高斯牛顿法，后面我会把此次视屏对应的作业写好，然后补充到此次博客！！

文章目录

  • 前言
  • 1.曲线拟合题目：
  • 2.非线性最小二乘
    • 2.1 黄金分割法(0.618法)
    • 2.2 最速下降法
  • 3.ceres库实现曲线拟合题目
    • 3.1 安装ceres
    • 3.2 代码及运行
  • 4.g2o库实现曲线拟合题目
    • 4.1 安装g2o
    • 4.2 代码及运行

1.曲线拟合题目：

设有曲线满⾜以下⽅程：
y = exp(ax2 + bx + c) + w
其中 a; b; c 为曲线参数， w 为噪声。现有 N个数据点 (x, y)，希望通过此 N 个点来拟合 a; b; c。实验中取N = 100。那么，定义误差为 ei = yi −exp(ax2 i + bxi + c)，于是 (a，b， c)的最优解可通过解以下最⼩⼆乘获得：在这里插入图片描述

使用ceres库和g2o库完成该题目

2.非线性最小二乘

![在这里插入图片描述](https://img-blog.csdnimg.cn/42930a0b98ce46b1b2ab40eb3f0c0d1

这里涉及到很多非线性优化的知识，如果你对这些知识没有概念，一定要去中国最好的大学——哔哩哔哩大学先自学一下，什么是凸函数，凸规划，海塞矩阵，黄金分割法(0.618法)，最速下降法，牛顿法，高斯牛顿法等等，否则这一块的知识你将无法理解！！！！

我会对某些方法做一些简单的说明：

在这里插入图片描述

2.1 黄金分割法(0.618法)

在这里插入图片描述

2.2 最速下降法

在这里插入图片描述

3.ceres库实现曲线拟合题目

Ceres库来自谷歌。是一个广泛使用的最小二乘问题求解库。

最小二乘问题：最小化观测数据的高维高斯分布形式的误差的平方，就可以得到最优的位姿状态。对于不方便求解的最小二乘问题，可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。此时，只要找到迭代点的梯度方向即可，而无须寻找全局导函数为零的情况。（这个和神经网络里面的梯度下降方式一样欸！殊途同归的感觉）
在这里插入图片描述

3.1 安装ceres

//安装依赖项
sudo apt-get install liblapack-dev libsuitesparse-dev libgflags-dev 
sudo apt-get install libgoogle-glog-dev libgtest-dev
sudo apt-get install libcxsparse3

wget ceres-solver.org/ceres-solver-1.14.0.tar.gz//下载后解压

//安装
cd ceres-solver-1.14.0
sudo mkdir build
cd build
cmake ..
make
sudo make install

3.2 代码及运行

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;
}

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} )

运行：

cd ceres_curve_fitting
mkdir build
cd build
cmake ..
make
./curve_fitting

在这里插入图片描述

最后一行是估计值，估计值的准确度取决于噪音，也就是代码中(main.cpp)的 w_sigma,噪声越小越接近于实际值a=1，b=2，c=1

4.g2o库实现曲线拟合题目

4.1 安装g2o

这个必须用高博再3rdparty中给的，不然后面会一直出错!

//依赖项
sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-dev
sudo apt-get install qtdeclarative5-dev
sudo apt-get install qt5-qmake
sudo apt-get install libqglviewer-dev
//安装
cd g2o
mkdir build
cd build
sudo ldconfig
cmake ..
make
sudo make install

4.2 代码及运行

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;
}

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
)

运行：

cd g2o_curve_fitting
mkdir build
cd build
cmake ..
make
./curve_fitting

在这里插入图片描述
运行出的结果和ceres一样
在这里插入图片描述

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1171329.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

网络基础扫盲-初识网络

网络基础扫盲-初识网络

博客内容&#xff1a;初识网络 文章目录 一、OSI七层网络模型二、TCP/IP四层模型1、MAC地址与IP地址 前言 在以前网络不够发之前&#xff0c;各个实验室进行一些研究时需要进行数据的交流&#xff0c;但是那时车马很慢&#xff0c;一生只够跑几次&#xff0c;所以就有人研究了网…
阅读更多...
Reshape.XL 1.2 for Excel插件 Crack

Reshape.XL 1.2 for Excel插件 Crack

特征 插件 Reshape.XL 包括 130 个基本可组合功能。使用它们&#xff0c;您可以快速轻松地进行非常复杂的数据转换和处理。它们的架构和基本定义受到 SQL 和 R 语言的强烈启发。 到目前为止&#xff0c;类似的功能只能通过脚本语言供程序员使用。借助 Reshape.XL 插件&#xf…
阅读更多...
Pyhotn: Mac安装selenium和chromedriver-119

Pyhotn: Mac安装selenium和chromedriver-119

1.0 安装selenium 终端输入&#xff1a; pip install selenium 查看版本&#xff1a; pip show selenium2.0 安装chromedriver 查看chrome版本 网上大多数是&#xff0c;基本到114就停了。 https://registry.npmmirror.com/binary.html?pathchromedriver/ 各种搜索&#…
阅读更多...
Java自学第4课：Java数组，类，对象

Java自学第4课：Java数组，类，对象

1 一维数组的创建和使用 2种创建形式&#xff1a; &#xff08;1&#xff09;先声明&#xff0c;再用new分配内存 &#xff08;2&#xff09;声明的同时分配内存 2种幅值形式 &#xff08;1&#xff09;用new{}赋值 &#xff08;2&#xff09;用{}赋值 如果不使用的话&a…
阅读更多...
【jvm】虚拟机栈

【jvm】虚拟机栈

目录 一、背景二、栈与堆三、声明周期四、作用五、特点&#xff08;优点&#xff09;六、可能出现的异常七、设置栈内存大小八、栈的存储单位九、栈运行原理十、栈帧的内部结构10.1 说明10.2 局部变量表10.3 操作数栈10.4 动态链接10.5 方法返回地址10.6 一些附加信息 十一、代…
阅读更多...
【强化学习】16 ——PPO（Proximal Policy Optimization）

【强化学习】16 ——PPO（Proximal Policy Optimization）

文章目录 前言TRPO的不足PPO特点 PPO-惩罚PPO-截断优势函数估计算法伪代码PPO 代码实践参考 前言 TRPO 算法在很多场景上的应用都很成功&#xff0c;但是我们也发现它的计算过程非常复杂&#xff0c;每一步更新的运算量非常大。于是&#xff0c;TRPO 算法的改进版——PPO 算法…
阅读更多...
【PyQt学习篇 · ⑪】：QPushButton和QCommandLinkButton的使用

【PyQt学习篇 · ⑪】：QPushButton和QCommandLinkButton的使用

文章目录 构造函数菜单设置扁平化默认处理右键菜单QCommandLinkButton的使用 构造函数 QPushButton的构造函数如下&#xff1a; """QPushButton(parent: Optional[QWidget] None)QPushButton(text: Optional[str], parent: Optional[QWidget] None)QPushButt…
阅读更多...
基于动力学模型的机械臂pid控制

基于动力学模型的机械臂pid控制

参考资料&#xff1a; 一、如何实现机械臂的控制 在最常见的对机械臂动力学实现控制的问题中&#xff0c;我们会有一段机械臂末端的期望轨迹S&#xff0c;希望通过对机械臂关节处电机转矩的控制实现末端沿期望轨迹的完美运动。控制问题主要分为镇定和跟踪两种&#xff0c;上面…
阅读更多...
2023/11/4 JAVA学习

2023/11/4 JAVA学习

通过匿名内部类
阅读更多...
verdi技巧分享--合并多个fsdb文件、统计信号边沿

verdi技巧分享--合并多个fsdb文件、统计信号边沿

文章目录 0 前言1 如何显示信号高位的02 统计信号的上升沿、下降沿3 合并信号4 将多个fsdb文件合并成一个 0 前言 分享几个这段时间学到的verdi操作 1 如何显示信号高位的0 这个可能对一些有强迫症的有帮助吧 nand相关的操作&#xff0c;有一些特定的cmd&#xff0c;比如 r…
阅读更多...
什么是工分排队模式？看懂之后，又能学会一招拓客引流技巧？

什么是工分排队模式？看懂之后，又能学会一招拓客引流技巧？

什么是工分排队模式&#xff1f;看懂之后&#xff0c;又能学会一招拓客引流技巧&#xff1f; 背景&#xff1a;当下市场行情呈现出经济平稳快速增长的趋势&#xff0c;但同时也存在物价持续上升的情况。从经济角度来看&#xff0c;当前市场行情呈现出经济平稳快速增长的趋势。这…
阅读更多...
职场被迫内卷，云认证破局

职场被迫内卷，云认证破局

前言&#xff1a; 2023年作为疫情全面放开的第一年&#xff0c;经济并没有像22年底时我们想象的那样&#xff0c;快速复苏&#xff0c;GDP增长超10%。取而代之的是&#xff0c;2023年经济大环境对各个行业来说&#xff0c;相比22年显的更加艰难&#xff0c;GDP增长预计在5%左右…
阅读更多...
Java数组的定义与常用使用方法

Java数组的定义与常用使用方法

目录 一.什么是数组 二.数组的创建及初始化 数组的创建 数组的初始化 动态初始化&#xff1a; 静态初始化&#xff1a; 【注意】 三.数组的使用 数组中元素访问 遍历数组 四.数组作为方法的参数 参数传基本数据类型 参数传数组类型(引用数据类型) 作为方法的返回…
阅读更多...
飞行器坐标转换

飞行器坐标转换

飞行器坐标转换 坐标系定义方向余弦矩阵 坐标系定义 本文定义的是右手直角坐标系&#xff0c; x − y − z x-y-z x−y−z轴分别为北-天-东。 从 A A A坐标系到 B B B坐标系是分别绕 y − z − x y-z-x y−z−x轴&#xff0c;即天-东-北旋转 ψ − θ − γ \psi-\theta-\gamm…
阅读更多...
【深入理解指针5】

【深入理解指针5】

提示&#xff1a;文章写完后&#xff0c;目录可以自动生成&#xff0c;如何生成可参考右边的帮助文档 目录 前言 1. sizeof和strlen的对比 1.1sizeof 1.2 strlen 1.3 sizeof 和 strlen的对比 2. 数组和指针笔试题解析 2.1 一维数组 2.2 字符数组 2.3 二维数组 3. 指针运算笔试题…
阅读更多...
竞赛 深度学习疫情社交安全距离检测算法 - python opencv cnn

竞赛 深度学习疫情社交安全距离检测算法 - python opencv cnn

文章目录 0 前言1 课题背景2 实现效果3 相关技术3.1 YOLOV43.2 基于 DeepSort 算法的行人跟踪 4 最后 0 前言 &#x1f525; 优质竞赛项目系列&#xff0c;今天要分享的是 &#x1f6a9; **基于深度学习疫情社交安全距离检测算法 ** 该项目较为新颖&#xff0c;适合作为竞赛…
阅读更多...
『亚马逊云科技产品测评』活动征文｜在aws搭建游戏工作室的网盘

『亚马逊云科技产品测评』活动征文｜在aws搭建游戏工作室的网盘

授权声明&#xff1a;本篇文章授权活动官方亚马逊云科技文章转发、改写权&#xff0c;包括不限于在 Developer Centre, 知乎&#xff0c;自媒体平台&#xff0c;第三方开发者媒体等亚马逊云科技官方渠道 目录 前言 方案选择 基础环境准备 部署网盘 1、创建数据目录 2、编…
阅读更多...
硬盘坏道检测修复工具下载，仅支持机械盘

硬盘坏道检测修复工具下载，仅支持机械盘

硬盘坏道检测修复工具下载&#xff0c;仅支持机械盘 下载路径&#xff0c;最下方官网——软件下载——常用工具下载——硬盘坏道修复工具硬盘检测修复工具 【软件试用版下载、软件资讯或技术支持服务可点击文章最下方官网】
阅读更多...
代码随想录算法训练营第23期day39 |62.不同路径、63. 不同路径 II

代码随想录算法训练营第23期day39 |62.不同路径、63. 不同路径 II

目录 一、&#xff08;leetcode 62&#xff09;不同路径 1.动态规划 1&#xff09;确定dp数组&#xff08;dp table&#xff09;以及下标的含义 2&#xff09;确定递推公式 3&#xff09;dp数组的初始化 4&#xff09;确定遍历顺序 5&#xff09;举例推导dp数组 2.数论方…
阅读更多...
虚拟dom及diff算法之 —— h函数和diff函数

虚拟dom及diff算法之 —— h函数和diff函数

新虚拟dom和老虚拟dom进行diff算法&#xff08;精细化比较&#xff09;&#xff0c;算出如何最小量更新&#xff0c;最后反映到真实dom上 diff是发生在虚拟dom上的 模板编译 虚拟dom如何产生 - 渲染函数&#xff08;h函数&#xff09; h函数产生虚拟节点&#xff08;vnode&a…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023