SLAM－－ceres库－－曲线逼近示例－

#include <ceres/ceres.h>
#include <iostream>
#include <chrono>
#include <opencv2/core/core.hpp>using namespace std;//代价函数的计算模型
struct CURVE_FITTING_COST
{// 仿函数，使用方式　CURVE_FITTING_COST(const T* const abc, T* residual)// 传入的是真实值（x-x_data[i],y-y_data[i]）CURVE_FITTING_COST (double x, double y) : _x (x), _y (y) {}//残差计算//操作符()重载template <typename T>bool operator() (const T* const abc, T* residual) const  //residual残差{//y-exp(ax^2+bx+c)residual[0] = T(_y) - ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);return true;}const double _x, _y;  //x,y数据
};int main(int argc, char** argv)
{//a,b,c是曲线真实模型的参数值，要求一条与该曲线逼近的曲线的参数a,b,c估计值double a = 1.0, b = 2.0, c = 1.0;  //计算结果：　estimate a, b, c = 0.891943 2.17039 0.944142//double a = 1.0, b = 5.0, c = 2.0; // 计算结果： estimate a, b, c = 0.989318 5.0169 1.99377int N = 100;  //１００个数据点double w_sigma = 1.0;  //噪声值cv::RNG rng;         //OpenCV随机数产生器double abc[3] = {0.0, 0.0, 0.0}; //abc的参数估计值，都初始化为0.0vector<double> x_data, y_data; //根据方程y-exp(ax^2+bx+c)＋w　生成１００个（ｘ,ｙ）数据，传入到x_data, y_data中。//假设有一条满足该方程的曲线cout << "generating data: " << endl;for(int i = 0; i < N; i++){double x = i/100.0;//把ｘ（取０～１中的１００个ｘ坐标）压入vector队列的x_data[]中x_data.push_back(x);//对应的找到与ｘ对应的１００个ｙ,压入vector队列的y_data[]中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(//向问题中添加误差项。// 使用自动求导ceres::AutoDiffCostFunction，// 将之前的代价函数结构体<CURVE_FITTING_COST, 1, 3>传入：// 解释：// 第一个1是误差项（是标量）输出维度，即残差的维度，第二个３是输入维度，即待寻优参数abc的维度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;  //输出到coutceres::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() << endl;//输出结果cout << summary.BriefReport() << endl;cout << "estimate a, b, c = ";for (auto a:abc)cout << a << " ";cout << endl;return 0;
}

SLAM－－ceres库－－曲线逼近示例－－啰里啰嗦的代码解析相关推荐

SLAM－－ORB特征点提取－－啰里啰嗦的代码解析
#include <iostream> #include <opencv2/core/core.hpp> #include <opencv2/features2d/fea ...
一步步学习操作系统（1）——参照ucos，在STM32上实现一个简单的多任务（“啰里啰嗦版”)...
该篇为"啰里啰嗦版",另有相应的"精简版"供参考 "不到长城非好汉:不做OS,枉为程序员" OS之于程序员,如同梵蒂冈之于天主教徒,那永远都 ...
【四足机器人--摆动相足端位置速度轨迹规划】（4.1）FootSwingTrajectory（bezier曲线计算脚的摆动轨迹）代码解析
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.FootSwingTrajectory(bezier曲线)的内容 ...
2021啰里啰嗦的个人总结
一年的时间说长不长说短不短,到年底的时候,年初的很多事情的细节都忘了,幸好自己平时在石墨文档中记日志,可以翻一翻日志看看自己这一年都干了些什么事情. 按照时间顺序,先回顾一下2021年干了些什么: 1 ...
C# 多态性 (啰里啰嗦)
使用多态性,可以动态地定义调用的方法, 而不是在编译期间定义. 编译器创建一个虚拟方法表 (vtable),其中列出了可以在运行期间调用的方法, 它根据运行期间的类型调用方法. public stat ...
啰里啰嗦学IOS CoreDate入门
创建项目需要把Use Core Date勾上把Main.storyboard删除. 点击如图文件. 点击下面的Add Entity 点击 + 添加属性和设置类型选中Entity,修改为Perso ...
slam优化库，优化方法，G2o Ceres的学习
文章目录 ch6 - ceres.g2o等前言 1.G2o **1.1 代码 :[G2o: exp(ax^2+bx+c)]** **1.2 理论-原理方面:** **1.3 G2O常见函数分析** ...
SLAM Cartographer（13）基于Ceres库的扫描匹配器
SLAM Cartographer(13)基于Ceres库的扫描匹配器 1. 扫描匹配器 2. 残差计算 2.1. 平移残差 2.2. 角度残差 2.3. 占据空间残差 1. 扫描匹配器通过< ...
在windows系统中使用Ceres非线性优化库：（一）安装Ceres库
(一)安装Ceres库 1.用vcpkg安装Ceres库 1.1.安装vcpkg 1.2.安装Ceres 1.3.配置C ...

SLAM－－ceres库－－曲线逼近示例－－啰里啰嗦的代码解析

SLAM－－ceres库－－曲线逼近示例－－啰里啰嗦的代码解析相关推荐

最新文章

热门文章