slam十四讲-ch6-非线性优化(包含手写高斯牛顿、使用g2o库、使用ceres库三种方法的源码详细注释)
一、自写高斯-牛顿法
该程序是要进行一个非线性优化,对非线性函数的系数进行优化
y=exp(ax2+bx+c)
给定初始的系数 ae,be,ce(估计的) ar,br,cr(真实的)
源码如下:
//
// Created by wenbo on 2020/10/28.
//
//该程序是手写高斯牛顿法,对一个简单的非线性函数进行优化
//y=exp(ax2+bx+c)
#include<iostream>
#include<opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>
using namespace std;
using namespace Eigen;
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;//求个倒数,??//利用opencv随机数产生器cv::RNG rng;vector<double> x_data,y_data;//定义容器分别来接受x ,y的数据//得到真实值for(int i=0;i<N;i++){double x=i/100.0;//得到数据点 0,,,,,,,x_data.push_back(x);//再把数据点(自变量)放回容器x_datay_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma*w_sigma));//w_sigma*w_sigma是噪声方差//这里把每个自变量所对应的真实值(y_data)算了出来,用作观测值}//高斯牛顿迭代int iters=100;//迭代次数double cost=0,lastcost=0;//本次迭代和上次迭代,最终的目标函数,要将误差进行平方chrono::steady_clock::time_point t1=chrono::steady_clock::now();////理一下思路//要算出J 矩阵 利用J 矩阵 算出H矩阵//之后解线性方程组 Hx=b//判断最后的cost满足调节没有,满足就不用迭代//要定义误差函数 yi=y_data[i] error=yi-exp(ae*x*x+be*x+ce)//用真实值减去估计值得到误差函数//J矩阵是误差函数对各个进行优化的系数的导数所组成的向量//H=(协方差矩阵转置)*J*J.transopose()//协方差转置来源于随机噪声//b=-(协方差矩阵转置)*error*J//cost=error*errorfor(int iter=0;iter<iters;iter++){Matrix3d H=Matrix3d::Zero();Vector3d b=Vector3d::Zero();cost=0;for(int i=0;i<N;i++){double xi=x_data[i], yi=y_data[i];//拿到对应的数据点double error =yi-exp(ae*xi*xi+be*xi+ce);//得到误差含糊是Eigen::Vector3d J; // 雅可比矩阵J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/daJ[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/dbJ[2] = -exp(ae * xi * xi + be * xi + ce); // de/dcH += inv_sigma * inv_sigma * J * J.transpose();b += -inv_sigma * inv_sigma * error * J;cost += error * error;}//求解线性方程组 Hx=bVector3d dx=H.ldlt().solve(b);//利用ldlt分解//也可用QR分解//Vector3d dx=H.colPivHouseholderQr().solve(b);//利用Qr分解//isnan() 判断是不是NAN值(not a number非法数字)// 标准库中定义了一个宏:NAN来表示非法数字。// 比如负数开方、负数求对数、0.0/0.0、0.0* INFINITY(无穷大)、INFINITY/INFINITY、INFINITY-INFINITY// 以上操作都会得到NAN。// 注意:如果是整数0/0会产生操作异常//isinf()测试某个浮点数是否是无限大,其中INF表示无穷大if(isnan(dx[0])){cout<<"result is nan"<<endl;break;}//如果优化结束,就应该退出迭代if(iter>0&&cost>=lastcost)//判断迭代结束{cout<<"cost="<<cost<<">="<<"lastcost="<<lastcost<<",break!"<<endl;break;}//否则继续优化系数ae+=dx[0];be+=dx[1];ce+=dx[2];lastcost=cost;cout<<"cost:"<<cost<<",\t\t update: "<<dx.transpose()<<"\t\t estimated params: "<<ae<<","<<be<<","<<ce<<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<<"cost_time="<<time_used.count()<<endl;cout<<"estimated abc ="<<ae<<","<<be<<","<<ce<<endl;return 0;
}
运行结果如下:
cost:3.19575e+06, update: 0.0455771 0.078164 -0.985329 estimated params: 2.04558,-0.921836,4.01467
cost:376785, update: 0.065762 0.224972 -0.962521 estimated params: 2.11134,-0.696864,3.05215
cost:35673.6, update: -0.0670241 0.617616 -0.907497 estimated params: 2.04432,-0.0792484,2.14465
cost:2195.01, update: -0.522767 1.19192 -0.756452 estimated params: 1.52155,1.11267,1.3882
cost:174.853, update: -0.537502 0.909933 -0.386395 estimated params: 0.984045,2.0226,1.00181
cost:102.78, update: -0.0919666 0.147331 -0.0573675 estimated params: 0.892079,2.16994,0.944438
cost:101.937, update: -0.00117081 0.00196749 -0.00081055 estimated params: 0.890908,2.1719,0.943628
cost:101.937, update: 3.4312e-06 -4.28555e-06 1.08348e-06 estimated params: 0.890912,2.1719,0.943629
cost:101.937, update: -2.01204e-08 2.68928e-08 -7.86602e-09 estimated params: 0.890912,2.1719,0.943629
cost=101.937>=lastcost=101.937,break!
cost_time=0.000228831
estimated abc =0.890912,2.1719,0.943629
2、利用g2o进行非线性优化
该程序是要进行一个非线性优化,对非线性函数的系数进行优化
y=exp(ax2+bx+c)
给定初始的系数 ae,be,ce(估计的) ar,br,cr(真实的)
源码如下:
//
//
//g2o
#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<3,Eigen::Vector3d>//定义顶点类。继承g2o的父类(基础顶点类)
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW//这个必须写,我也不知道为什么//重置,估计系数(待优化的系数)重写虚函数// override保留字表示当前函数重写了基类的虚函数virtual void setToOriginImpl() override{_estimate<<0,0,0;//设置估计的系数}//更新估计系数(待优化的系数)重写虚函数virtual void oplusImpl(const double *update) override{_estimate += Eigen::Vector3d(update);}// 存盘和读盘:留空virtual bool read(istream &in) {}//virtual bool write(ostream &out) const {}
};//误差模板(边) 模板参数:观测值(真实值 y)维度,类型。链接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW//这个必须写,我也不知道为什么//有参构造CurveFittingEdge(double x): BaseUnaryEdge(), _x(x) {}//这里的构造函数里面还调用了BaseUnaryEdge的构造函数//计算曲线模型误差 重写虚函数virtual void computeError() override{const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);const Eigen::Vector3d abc = v->estimate();//abc待优化的系数//误差函数 =y-exp(ax2+bx+c) 这里面的abc都是待优化的系数_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();//计算误差_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 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;cv::RNG rng; // OpenCV随机数产生器vector<double> x_data, y_data; // 数据//真实值,// 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) + rng.gaussian(w_sigma * w_sigma));}// 构建图优化,先设定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)); // 信息矩阵: 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); //迭代次数10chrono::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;
}
运行结果如下:
start optimization
solve time cost = 0.00582179 seconds.
estimated model: 2 -1 5
iteration= 0 chi2= 3195746.523015 time= 2.9264e-05 cumTime= 2.9264e-05 edges= 100 schur= 0
iteration= 1 chi2= 3195746.523015 time= 2.7616e-05 cumTime= 5.688e-05 edges= 100 schur= 0
iteration= 2 chi2= 3195746.523015 time= 2.8487e-05 cumTime= 8.5367e-05 edges= 100 schur= 0
iteration= 3 chi2= 3195746.523015 time= 5.7382e-05 cumTime= 0.000142749 edges= 100 schur= 0
iteration= 4 chi2= 3195746.523015 time= 3.061e-05 cumTime= 0.000173359 edges= 100 schur= 0
iteration= 5 chi2= 3195746.523015 time= 2.9421e-05 cumTime= 0.00020278 edges= 100 schur= 0
iteration= 6 chi2= 3195746.523015 time= 2.0262e-05 cumTime= 0.000223042 edges= 100 schur= 0
iteration= 7 chi2= 3195746.523015 time= 2.1139e-05 cumTime= 0.000244181 edges= 100 schur= 0
iteration= 8 chi2= 3195746.523015 time= 2.026e-05 cumTime= 0.000264441 edges= 100 schur= 0
iteration= 9 chi2= 3195746.523015 time= 2.5783e-05 cumTime= 0.000290224 edges= 100 schur= 0
3、利用ceres进行非线性优化
源码如下:
//
//
#include<iostream>
#include<opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
using namespace std;
//利用ceres进行非线性优化(曲线拟合)
//步骤:
//1、定义内个参数块,位每个参数分配double数组来存储变量的值
//2、定义残差块的计算方式
//3、残差块往往也需要定义雅克比的计算方式
//4、把左右的参数块和残差块加入Ceres定义的Problem的对象中,调用solve函数求解即可//代价函数的计算模型,这里面计算的是误差(残差)函数 Ceres应该会对这个误差函数进行平方,得到目标函数
struct CURVE_FITTING_COST
{CURVE_FITTING_COST(double x,double y):_x(x),_y(y) {}//初始化数据 这里面的_x _y 是真实的数据,_y也就是观测值//残差计算 y-exp(ax2+bx+c) ,其中这个式子里面的a b c值咱们的估计值,也就是希望优化的系数//重载()运算符,()运算符能够利用其传入的参数abc,residual,算出残差,返回true//我个人理解是,在构建Problem的时候,这个重载被集成在Problem里了template<typename T>bool operator ()(const T *const abc, T *residual)const{// y-exp(ax^2+bx+c),其中这个式子里面的a b 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;
};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;//估计参数值,对这组数进行优化double abc[3]={ae,be,ce};int N=100;//数据的个数double w_sigma=1.0;//噪声sigma的值double inv_sigma=1.0/w_sigma; //求个倒数,??//利用opencv随机数产生器cv::RNG rng;vector<double> x_data,y_data;//定义容器分别来接受x ,y的数据//得到真实值for(int i=0;i<N;i++){double x=i/100.0;//得到数据点 0,,,,,,,x_data.push_back(x);//再把数据点(自变量)放回容器x_datay_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma*w_sigma));//w_sigma*w_sigma是噪声方差//这里把每个自变量所对应的真实值(y_data)算了出来,用作观测值}//构建最小二乘问题,也就是非线性优化问题ceres::Problem 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用来配置options.linear_solver_type=ceres::DENSE_NORMAL_CHOLESKY;//利用DENSE_NORMAL_CHOLESKY求解增量方差 Hx=boptions.minimizer_progress_to_stdout=true;//允许 输出到coutceres::Solver::Summary summary;//优化信息chrono::steady_clock::time_point t1 = chrono::steady_clock::now();ceres::Solve(options, &problem, &summary); // 开始优化 将配置信息和要求解的问题放进去,把优化的信息放在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<<" ";//遍历abccout<<endl;return 0;
}
运行结果如下:
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time0 1.597873e+06 0.00e+00 3.52e+06 0.00e+00 0.00e+00 1.00e+04 0 6.29e-05 1.43e-041 1.884440e+05 1.41e+06 4.86e+05 9.88e-01 8.82e-01 1.81e+04 1 1.48e-04 1.81e-032 1.784821e+04 1.71e+05 6.78e+04 9.89e-01 9.06e-01 3.87e+04 1 6.89e-05 2.02e-033 1.099631e+03 1.67e+04 8.58e+03 1.10e+00 9.41e-01 1.16e+05 1 8.80e-05 2.25e-034 8.784938e+01 1.01e+03 6.53e+02 1.51e+00 9.67e-01 3.48e+05 1 4.10e-05 2.42e-035 5.141230e+01 3.64e+01 2.72e+01 1.13e+00 9.90e-01 1.05e+06 1 7.10e-05 2.61e-036 5.096862e+01 4.44e-01 4.27e-01 1.89e-01 9.98e-01 3.14e+06 1 6.60e-05 2.82e-037 5.096851e+01 1.10e-04 9.53e-04 2.84e-03 9.99e-01 9.41e+06 1 6.51e-05 3.03e-03
solve time cost = 0.00320557 seconds.
Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
estimated a,b,c 0.890908 2.1719 0.943628
slam十四讲-ch6-非线性优化(包含手写高斯牛顿、使用g2o库、使用ceres库三种方法的源码详细注释)相关推荐
- 视觉SLAM十四讲CH6代码解析及课后习题详解
gaussNewton.cpp #include <iostream> #include <chrono> #include <opencv2/opencv.hpp> ...
- 视觉slam十四讲ch6曲线拟合 代码注释(笔记版)
1 #include <opencv2/core/core.hpp> 2 #include <ceres/ceres.h> 3 #include <chrono> ...
- 史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)
本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证{\color{blue}{文末正下方中心}提供了本人 \co ...
- 【《视觉SLAM十四讲》前ch2-ch6实践全过程和遇到的问题及解决办法】
文章目录 前言 一.运行环境配置 1.在虚拟机上安装Ubuntu14.04 2.方便Ubuntu使用 二.<十四讲>的实践部分过程与问题 1.Ubuntu下安装包的两种方法 2.编译高翔的 ...
- 视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习 ...
- 《视觉SLAM十四讲》手写高斯牛顿—笔记记录
<视觉SLAM十四讲>手写高斯牛顿-笔记记录 我们的最终目的:使用高斯牛顿法,拟合参数abc 我们的实际小目标:求解增量方程得到ΔX(有了Δx就可以不停的迭代Eabc使得拟合Rabc啦) ...
- 视觉SLAM十四讲学习笔记-第六讲学习笔记总结(1)---非线性优化原理
第六讲学习笔记如下: 视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_ ...
- 视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习 ...
- 视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习 ...
最新文章
- java 千分位格式话_Java 字符串小数转成千分位格式
- Python入门100题 | 第028题
- stdthread(3)detach
- 浅谈 Android 自定义锁屏页的发车姿势
- 【二叉树】【144. 二叉树的前序遍历】【中等】
- 消息映射的服务器的设计与实现
- 一个数据包大小是多少k_算法交流: 6046 数据包的调度机制 【2.6基本算法之动态规划】...
- Modularity(模块化-ES6)
- 工作中:如何在实际工作中处理 NULL,并给出一些指南
- XML DOM学习笔记(JS)
- linux 英汉词典程序shell+postgresql版
- 设计模式之GOF23代理模式01
- vue实现点击复制文本功能
- Linux页高速缓存与文件读写
- 2021-11-30 网工基础(三)物理层、数据链路层、VRP系统等基础
- 傻瓜式制作纯净版win10启动盘
- Python | 爬虫抓取智联招聘(基础版)
- docker容器挂载权限问题 导致日志文件不生成
- 【STM32F407】Note_01 STM32 编程环境搭建 -- Keil与VS code组合
- 2020-06-09:给定一个无序数组,里面数都是成双数的,只有一个数是成单数的,求这个数?
热门文章
- css只设置背景图片半透明,css3实现背景图片半透明内容不透明的方法示例
- 微信公众平台开发培训
- activiti:initiator的作用及其使用
- 西门子 PLC S7单边通信
- pycharm 中 pydev debugger: CRITICAL WARNING: This version of python seems to be incorrectly compiled
- python电子书在线阅读-Python编程快速上手 让繁琐工作自动化
- 前端iframe标签介绍及使用
- 项目进度没有把控好,被领导足足骂了10多分钟,吭都不敢吭一声
- 改进YOLOv7系列:首发最新结合Global Context Modeling结构(附YOLOv5改进),目标检测高效涨点
- 忽视警告_不要忽视下雨的风险2