紧接上次的手写高斯牛顿,今天顺便将LM算法也进行了手写实现,并且自己基于eigen的高斯牛顿进行了对比,可以很明显的看到,LM的算法收敛更快,精度也略胜一筹,这次高博的书不够用了,参考网上伪代码进行实现.同学们有什么问题都可以讨论,理论部分的细节后面会补上,最近确实有点忙.

伪代码

程序源码

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include"opencv2/opencv.hpp"
#include"opencv4/opencv2/opencv.hpp"
#include<iostream>
#include<vector>
using namespace Eigen;
using namespace std;
using namespace cv;
//生成测试数据
void makeTheTestNum(vector<double> &xSet ,vector<double> &ySet);
//LM算法结算
void LM(const vector<double> &xSet ,const vector<double> &ySet ,double &a,double &b,double &c);void makeTheTestNum(vector<double> &xSet ,vector<double> &ySet){RNG rng;   double noise = rng.gaussian(1);//设定值double a = 2;double b = 1;double c = 1;for(int i = 0;i <100;i++){   double x = i/100.0;//注意这个.0,不然出来全是0double fx = exp(a*x*x + b*x + c)+noise;xSet.push_back(x);ySet.push_back(fx);}cout<<xSet.size()<<endl;if(xSet.size() != ySet.size())cout<<"data is bad!"<<endl;}void LM(const vector<double> &xSet ,const vector<double> &ySet ,double &a,double &b,double &c){bool flag = 0;double cost =0.0;double lastcost = 0.0;int maxtimes = 1000;double v = 2;double rho = 0;double tao = 1e-10;//获得初值Matrix3d H = Matrix3d::Zero();Vector3d g = Vector3d::Zero();Vector3d J;//装填数据for(int j = 0;j< xSet.size();j++){  double x = xSet[j];double y = ySet[j];// cout<<"x" <<x<<endl;// cout<<"y" <<y<<endl;double e = y - exp(a*x*x + b*x + c);J[0] =  -exp(a*x*x + b*x + c)*x*x;J[1] = -exp(a*x*x + b*x + c)*x;J[2] = -exp(a*x*x + b*x + c)*1;Matrix3d tempH = J*J.transpose(); H +=tempH;g += -J*e;cost +=e*e;}//设置这个u的初值double u = tao*H.maxCoeff();cout<<"init u :"<<u<<endl;cout<<"H init"<<H.matrix()<<endl;cout<<"J init"<<J.matrix()<<endl;cout<<"g init"<<g.matrix()<<endl;Matrix3d I = MatrixXd::Identity(3, 3);for(int i =0; i< maxtimes;i++){//使用eigen解算线性方程组,此处和GN的方程多了一个u*I,这就是LM的关键Matrix3d A = H+u*I;Vector3d delta_abc = A.ldlt().solve(g);//cout<<"delta_abc"<<delta_abc<<endl;//Vector3d delta_abc = H.ldlt().solve(g);if(delta_abc.norm()<1e-12){   flag =1;break;}cout<<"delta_abc"<<delta_abc.transpose()<<endl;//判断是否发散if(isnan(delta_abc[0])||isnan(delta_abc[1])||isnan(delta_abc[2])){   flag =0;break;}a += delta_abc[0];b += delta_abc[1];c += delta_abc[2];double cost_new = 0;for(int j = 0;j< xSet.size();j++){  double x = xSet[j];double y = ySet[j];double e = y - exp(a*x*x + b*x + c);cost_new +=e*e;}rho = (cost - cost_new)/(delta_abc.transpose()*(u*delta_abc+g));//LM的工作if(rho >0 ){cost= 0;//注意初始化两个H和g,如果不是0会有很多奇怪的错误H = Matrix3d::Zero();g = Vector3d::Zero();J = Vector3d::Zero();//装填数据for(int j = 0;j< xSet.size();j++){  double x = xSet[j];double y = ySet[j];// cout<<"x" <<x<<endl;// cout<<"y" <<y<<endl;double e = y - exp(a*x*x + b*x + c);J[0] =  -exp(a*x*x + b*x + c)*x*x;J[1] = -exp(a*x*x + b*x + c)*x;J[2] = -exp(a*x*x + b*x + c)*1;Matrix3d tempH = J*J.transpose(); H +=tempH;g += -J*e;cost +=e*e;}if(delta_abc.norm()<1e-12||cost<1e-12){   flag =1;break;}//更新u和v,缩小范围,更接近高斯牛顿u = u*max(0.3333333,(1-(2*rho-1)*(2*rho-1)*(2*rho-1)));v=2;}else{//不满足,扩大范围,更接近最速下降u = u * v;v = 2 * v;}cout<<"第"<<i<<"次:"<<endl;cout<<"a : "<<a<<endl;cout<<"b : "<<b<<endl;cout<<"c : "<<c<<endl;lastcost = cost;cout<<"delta_abc"<<delta_abc.norm()<<endl;}if(flag){cout<<"已收敛,结果为:"<<endl;cout<<"final a : "<<a<<endl;cout<<"final b : "<<b<<endl;cout<<"final c : "<<c<<endl;}else{cout<<"发散了QAQ,最后一次结果"<<endl;cout<<"final a : "<<a<<endl;cout<<"final b : "<<b<<endl;cout<<"final c : "<<c<<endl;       }}int main(){cout.precision(9);//设置初始值double a = -3;double b = -1;double c = 7;//观测值vector<double> xSet ;vector<double> ySet;makeTheTestNum(xSet ,ySet);LM(xSet, ySet, a, b, c);return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( guassNewton )set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )# Eigen
find_package(Eigen3 3.3 REQUIRED)
include_directories(${Eigen3_INCLUDE_DIRS})
# OpenCV
find_package(OpenCV 3.1 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})add_executable( guassNewton  LM.cc )
target_link_libraries(guassNewton   ${OpenCV_LIBS} )

运行结果

第0次:
a : -2.42817009
b : -1.21346293
c : 6.01511826
delta_abc1.1586837
delta_abc  1.48927117 -0.567649404 -0.959180392
第1次:
a : -0.93889892
b : -1.78111233
c : 5.05593786
delta_abc1.86015631
delta_abc  3.20365945  -1.27539914 -0.902080513
第2次:
a : 2.26476053
b : -3.05651147
c : 4.15385735
delta_abc3.56424271
delta_abc  2.28598992 -0.697482592 -0.902219971
第3次:
a : 4.55075046
b : -3.75399407
c : 3.25163738
delta_abc2.55464925
delta_abc-0.445211318    1.4096799 -0.963758978
第4次:
a : 4.10553914
b : -2.34431417
c : 2.2878784
delta_abc1.76472148
delta_abc -1.31455441     2.111265 -0.826759985
第5次:
a : 2.79098473
b : -0.233049172
c : 1.46111842
delta_abc2.62088254
delta_abc-0.691169818   1.07445643 -0.400135006
第6次:
a : 2.09981491
b : 0.841407256
c : 1.06098341
delta_abc1.33876075
delta_abc-0.0980877612    0.15582017 -0.0599021543
第7次:
a : 2.00172715
b : 0.997227425
c : 1.00108126
delta_abc0.193621802
delta_abc-0.00172660324  0.00277169756 -0.00108091369
第8次:
a : 2.00000055
b : 0.999999123
c : 1.00000034
delta_abc0.00343974425
delta_abc-5.45923277e-07  8.78403636e-07 -3.43603167e-07
第9次:
a : 2
b : 1
c : 0.999999999
delta_abc1.08981113e-06
已收敛,结果为:
final a : 2
final b : 1
final c : 0.999999999

手写系列之手写LM(Levenberg–Marquardt)算法(基于eigen)相关推荐

LM(Levenberg–Marquardt)算法原理及其python自定义实现
LM算法原理及其python自定义实现 LM(Levenberg–Marquardt)算法原理 LM算法python实现实现步骤: 代码: 运行结果: LM(Levenberg–Marquardt) ...
Levenberg–Marquardt算法学习
本次是对Levenberg–Marquardt的学习总结,是为之后看懂sparse bundle ajdustment打基础.这篇笔记包含如下内容: 回顾高斯牛顿算法,引入LM算法惩罚因子的计算(迭 ...
非线性最小二乘问题与Levenberg–Marquardt算法详解
1 最小二乘问题给定一组连续函数 f:Rn→Rm,m⩾n{\mathbf{f}}:{\mathbb{R}^n} \to {\mathbb{R}^m},{\text{ }}m \geqslant nf ...
Levenberg–Marquardt算法
Levenberg-Marquardt 算法最优估计算法中通常的做法都是写一个代价函数,并迭代计算最小化代价函数. 解决非线性最小二乘问题的方法有高斯牛顿法等,下面介绍Levenberg-Marqu ...
lm opencv 算法_Levenberg–Marquardt算法学习（和matlab的LM算法对比）
回顾高斯牛顿算法,引入LM算法惩罚因子的计算(迭代步子的计算) 完整的算法流程及代码样例 1. 回顾高斯牛顿,引入LM算法根据之前的博文:Gauss-Newton算法学习假设我们研究如 ...
Levenberg–Marquardt algorithm
Levenberg-Marquardt又称莱文伯格-马夸特方法(Levenberg–Marquardt algorithm)能提供数非线性最小化(局部最小)的数值解. 此算法能借由执行时修改参数达到结 ...
【手写系列】写一个迷你版的Tomcat
前言 Tomcat,这只3脚猫,大学的时候就认识了,直到现在工作中,也常会和它打交道.这是一只神奇的猫,今天让我来抽象你,实现你! Tomcat Write MyTomcat Tomcat是非常流行的 ...
【手写系列】纯手写实现一个高可用的RPC
前言在实际后台服务开发中,比如订单服务(开发者A负责)需要调用商品服务(开发者B负责),那么开发者B会和A约定调用API,以接口的形式提供给A.通常都是B把API上传到Maven私服,然后B开始写A ...
【手写系列】理解数据库连接池底层原理之手写实现
前言数据库连接池的基本思想是:为数据库连接建立一个"缓冲池",预先在池中放入一定数量的数据库连接管道,需要时,从池子中取出管道进行使用,操作完毕后,再将管道放入池子中,从而避免了 ...

手写系列之手写LM(Levenberg–Marquardt)算法(基于eigen)

伪代码

程序源码

CMakeLists.txt

运行结果

手写系列之手写LM(Levenberg–Marquardt)算法(基于eigen)相关推荐

最新文章

热门文章