视觉slam十四讲ch6曲线拟合 代码注释(笔记版)

  1 #include <opencv2/core/core.hpp>
  2 #include <ceres/ceres.h>
  3 #include <chrono>
  4
  5 using namespace std;
  6
  7 // 代价函数的计算模型
  8 struct CURVE_FITTING_COST
  9 {
 10     CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
 11     // 残差的计算
 12     template <typename T>
 13     bool operator() (
 14         const T* const abc,     // 模型参数,有3维 当没有必要分类的时候 就用一个数组来存储未知的系数,方便管理,而不是设3个变量,之后在()重载函数的形式参数个数变为3个
 15         T* residual ) const     // 残差
 16     {
 17         residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
 18         return true;
 19     }
 20     const double _x, _y;    // x,y数据
 21 };
 22
 23 int main ( int argc, char** argv )
 24 {
 25     double a=1.0, b=2.0, c=1.0;         // 真实参数值
 26     int N=100;                          // 数据点
 27     double w_sigma=1.0;                 // 噪声Sigma值(根号下方差)
 28     cv::RNG rng;                        // OpenCV随机数产生器
 29     double abc[3] = {0.8,2.1,0.9};      // abc参数的估计值 (修改初始值 下面求解迭代过程会不同)
 30
 31     vector<double> x_data, y_data;      // 数据
 32
 33     /*生成符合曲线的样本*/
 34     cout<<"generating data: "<<endl;   //下面是从真实的曲线中取得样本数据
 35     for ( int i=0; i<N; i++ )
 36     {
 37         double x = i/100.0;
 38         x_data.push_back ( x );
 39         y_data.push_back (
 40             exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
 41         );
 42         //cout<<x_data[i]<<" "<<y_data[i]<<endl;//输出生成数据
 43     }
 44
 45     // 构建最小二乘问题
 46     ceres::Problem problem;
 47     for ( int i=0; i<N; i++ )
 48     {
 49         /* 第一个参数 CostFunction* : 描述最小二乘的基本形式即代价函数 例如书上的116页fi(.)的形式
 50          * 第二个参数 LossFunction* : 描述核函数的形式 例如书上的ρi(.)
 51          * 第三个参数 double* :       待估计参数(用数组存储)
 52          * 这里仅仅重载了三个参数的函数,如果上面的double abc[3]改为三个double a=0 ,b=0,c = 0;
 53          * 此时AddResidualBlock函数的参数除了前面的CostFunction LossFunction 外后面就必须加上三个参数 分别输入&a,&b,&c
 54          * 那么此时下面的 ceres::AutoDiffCostFunction<>模板参数就变为了 <CURVE_FITTING_COST,1,1,1,1>后面三个1代表有几类未知参数
 55          * 我们修改为了a b c三个变量,所以这里代表了3类,之后需要在自己写的CURVE_FITTING_COST类中的operator()函数中,
 56          * 把形式参数变为了const T* const a, const T* const b, const T* const c ,T* residual
 57          * 上面修改的方法与本例程实际上一样,只不过修改的这种方式显得乱,实际上我们在用的时候,一般都是残差种类有几个,那么后面的分类 就分几类
 58          * 比如后面讲的重投影误差,此事就分两类 一类是相机9维变量,一类是点的3维变量,然而残差项变为了2维
 59          *
 60          * (1): 修改后的写法(当然自己定义的代价函数要对应修改重载函数的形式参数,对应修改内部的残差的计算):
 61          *      ceres::CostFunction* cost_function
 62          *              = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 1 ,1 ,1>(
 63          *                  new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
 64          *      problem.AddResidualBlock(cost_function,nullptr,&a,&b,&c);
 65          * 修改后的代价函数的计算模型:
 66          *   struct CURVE_FITTING_COST
 67          *   {
 68          *       CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
 69          *       // 残差的计算
 70          *       template <typename T>
 71          *       bool operator() (
 72          *          const T* const a,
 73          *          const T* const b,
 74          *          const T* const c,
 75          *          T* residual   ) const     // 残差
 76          *       {
 77          *           residual[0] = T ( _y ) - ceres::exp ( a[0]*T ( _x ) *T ( _x ) + b[0]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c)
 78          *           return true;
 79          *       }
 80          *       const double _x, _y;    // x,y数据
 81          *   };//代价类结束
 82          *
 83          *
 84          * (2): 本例程下面的语句通常拆开来写(看起来方便些):
 85          * ceres::CostFunction* cost_function
 86          *              = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
 87          *                  new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
 88          * problem.AddResidualBlock(cost_function,nullptr,abc)
 89          * */
 90         problem.AddResidualBlock (     // 向问题中添加误差项
 91         // 使用自动求导,模板参数:误差类型,Dimension of residual(输出维度 表示有几类残差,本例程中就一类残差项目,所以为1),输入维度,维数要与前面struct中一致
 92                 /*这里1 代表*/
 93             new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
 94                 new CURVE_FITTING_COST ( x_data[i], y_data[i] )// x_data[i], y_data[i] 代表输入的获得的试验数据
 95             ),
 96             nullptr,            // 核函数,这里不使用,为空  这里是LossFunction的位置
 97             abc                 // 待估计参数3维
 98         );
 99     }
100
101     // 配置求解器ceres::Solver (是一个非线性最小二乘的求解器)
102     ceres::Solver::Options options;     // 这里有很多配置项可以填Options类嵌入在Solver类中 ,在Options类中可以设置关于求解器的参数
103     options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解 这里的linear_solver_type 是一个Linear_solver_type的枚举类型的变量
104     options.minimizer_progress_to_stdout = true;   // 为真时 内部错误输出到cout,我们可以看到错误的地方,默认情况下,会输出到日志文件中保存
105
106     ceres::Solver::Summary summary;                // 优化信息
107     chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//记录求解时间间隔
108     //cout<<endl<<"求解前....."<<endl;
109     /*下面函数需要3个参数:
110      * 1、 const Solver::Options& options <----> optione
111      * 2、 Problem* problem               <----> &problem
112      * 3、 Solver::Summary* summary       <----> &summart (即使默认的参数也需要定义该变量 )
113      * 这个函数会输出一些迭代的信息。
114      * */
115     ceres::Solve ( options, &problem, &summary );  // 开始优化
116     //cout<<endl<<"求解后....."<<endl;
117     chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
118     chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
119     cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
120
121     // 输出结果
122     // BriefReport() : A brief one line description of the state of the solver after termination.
123     cout<<summary.BriefReport() <<endl;
124     cout<<"estimated a,b,c = ";
125     /*auto a:abc  或者下面的方式都可以*/
126     for ( auto &a:abc ) cout<<a<<" ";
127     cout<<endl;
128
129     return 0;
130 }

原文地址:https://www.cnblogs.com/newneul/p/8407365.html

时间: 2024-11-08 18:00:05

视觉slam十四讲ch6曲线拟合 代码注释(笔记版)的相关文章

CV学习资料《卷积神经网络与视觉计算》+《深度学习实践计算机视觉》+《视觉SLAM十四讲从理论到实践》电子资料代码分析

视觉和图形学真是一家,基础都一样! 如果学习图像识别,计算机视觉,推荐电子书<视觉SLAM十四讲:从理论到实践>,系统介绍了视觉SLAM(同时定位与地图构建)所需的基本知识与核心算法,既包括数学理论基础,如三维空间的刚体运动.非线性优化,又包括计算机视觉的算法实现,例如多视图几何.回环检测等. 一个周读完了,代码很清晰!Particle Filtering,KF,EKF, Batch Optimization, Lie Group,ICP,LK光流... 尤其惊喜的是文末作者看好的IMU-SL

浅读《视觉SLAM十四讲:从理论到实践》--操作1--初识SLAM

下载<视觉SLAM十四讲:从理论到实践>源码:https://github.com/gaoxiang12/slambook 第二讲:初识SLAM 2.4.2 Hello SLAM(书本P27) 1.从github上下载源码,并解压 Ubuntu上,解压zip,先找到zip文件所在位置,然后运行下面代码,进行解压. unzip slambook-master.zip 解压后,找到ch2文件夹,在文件夹中找到helloSLAM.cpp文件 运行cpp文件 g++ helloSLAM.cpp 如未安

入门实战《深度学习技术图像处理入门》+《视觉SLAM十四讲从理论到实践》

学习图像识别处理,想在数据分析竞赛中取得较高的排名,看了<深度学习技术图像处理入门>电子书,一边看电子书一边做标记,对配套的代码也做了测试,收获颇多. 从机器学习.图像处理的基本概念入手,逐步阐述深度学习图像处理技术的基本原理以及简单的实现. 学习理论后做实验,使用卷积神经网络进行端到端学习,构建深度卷积神经网络,使用循环神经网络改进模型,评估模型,测试模型.最关键的是可以将模型运用于实战之中,将深度学习模型导入到工程中,数据类型转换函数,实施CAM可视化,这是我最需要的. 视觉和图形学真是一

《视觉slam十四讲》之第3讲-实践Eigen库

<视觉slam十四讲>之第3讲-实践Eigen库 Eigen库的安装 sudo apt-get install libeigen3-dev 注:Eigen是一个由纯头文件搭建的线性代数库,头文件安装路径为/usr/include/eigen3/. 实例1:Eigen的基础运算 #include <iostream> #include <ctime> #include <Eigen/Core> // Eigen 部分 #include <Eigen/De

《视觉slam十四讲》-第3讲-三维空间刚体运动

<视觉slam十四讲>-第3讲-三维空间刚体运动 原文地址:https://www.cnblogs.com/tangyuanjie/p/12606386.html

视觉SLAM十四讲随笔

**************************************************************************************************************************************************ch1:clion的使用 断点设置之后,逐行执行 (Step Into) ( F7 ),逐函数执行 (Step Over) ( F8 ),注意0和O在clion中的显示区别,0是中间有个点的 O看起来像0,中

视觉slam十四讲ch5 joinMap.cpp 代码注释(笔记版)

1 #include <iostream> 2 #include <fstream> 3 using namespace std; 4 #include <opencv2/core/core.hpp> 5 #include <opencv2/highgui/highgui.hpp> 6 #include <Eigen/Geometry> 7 #include <boost/format.hpp> // for formating st

《视觉slam十四讲》之第3讲-三维刚体运动

第三讲:三维空间刚体运动 旋转的几种表达方式 向量 关于向量: 注:其中e1,e2,e3为线性空间下的一组基. 向量的内积: 注:向量的内积表示向量间的投影关系. 向量的外积 注:可以使用外积表示向量的旋转. 注:^ 记成一个反对称符号. 旋转矩阵 假设某个单位正交基(e1; e2; e3) ,经过一次欧式变换,变成了(e′ 1; e′ 2; e′ 3),对于同一个向量 a (注意该向量并没有随着坐标系的旋转而发生运动),它在两个坐标系下的坐标为 [a1; a2; a3]T 和 [a′ 1; a

《视觉slam十四讲》-第1讲-全书内容概要

原文地址:https://www.cnblogs.com/tangyuanjie/p/12605748.html