c++实现高斯牛顿法(Gauss-Newton method)原理

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>using namespace std;
using namespace Eigen;/** 程序说明：为了验证Gauss-Newton法寻找梯度增量的效果，我们假设已知真实的参数（在实际应用中我们要做的就是估计、优化这些参数）* 1.先利用真实参数生成真实的x,y数据* 2.将估计的参数代入Gauss-Newton模型进行优化* 3.优化后的结果与真实参数进行比较*/
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值cv::RNG rng;                                 // OpenCV随机数产生器vector<double> 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));}/**************************开始Gauss-Newton迭代****************************/int iterations = 100;    // 迭代次数double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的costfor (int iter = 0; iter < iterations; iter++) {Matrix3d H = Matrix3d::Zero();             // Hessian = J^T J in Gauss-NewtonVector3d b = Vector3d::Zero();             // biascost = 0;for (int i = 0; i < N; i++) {double xi = x_data[i], yi = y_data[i];  // 第i个数据点//========注意这里不是error ^ 2，Gauss-Newton明确指出是对error进行一阶泰勒展开，可以试着取消以下注释，进行计算，优化结果会比较差=======//double error = ( yi - exp(ae * xi * xi + be * xi + ce) ) * ( yi - exp(ae * xi * xi + be * xi + ce) );//Vector3d J; // 雅可比矩阵//J[0] = -2 * (yi - exp(ae * xi * xi + be * xi + ce)) * exp(ae * xi * xi + be * xi + ce) * xi * xi;  // de/da//J[1] = -2 * (yi - exp(ae * xi * xi + be * xi + ce)) * exp(ae * xi * xi + be * xi + ce) * xi;       // de/db//J[2] = -2 * (yi - exp(ae * xi * xi + be * xi + ce)) * exp(ae * xi * xi + be * xi + ce) ;           // de/dc//============================================================================================================double error = yi - exp(ae * xi * xi + be * xi + ce);Vector3d J; // 雅可比矩阵，实值标量函数对行向量求偏导，结果也为行向量，所以在这是雅可比矩阵是3维向量。J[0] = -exp(ae * xi * xi + be * xi + ce) * xi * xi;  // d(error)/d(ae),公式可看下图。J[1] = -exp(ae * xi * xi + be * xi + ce) * xi;       // d(error)/d(be)J[2] = -exp(ae * xi * xi + be * xi + ce) ;           // d(error)/d(ce)H += J * J.transpose(); // GN近似的Hb += -error * J;cost += error * error;}// 求解线性方程 Hx=b，建议用ldltVector3d dx;dx = H.ldlt().solve(b);//true if arg is a NaN(not a number), false otherwise if (isnan(dx[0])) {cout << "result is nan!" << endl;break;}if (iter > 0 && cost > lastCost) {// 误差增长了，说明近似的不够好cout << "cost: " << cost << ", last cost: " << lastCost << endl;break;}// 因为要寻找使error最小的ae,be,ce，所以要不断更新ae,be,ce估计值实现向最优的ae,be,ce值的逼近ae += dx[0];be += dx[1];ce += dx[2];lastCost = cost;cout << "total cost: " << cost << endl;}cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;return 0;
}

c++实现高斯牛顿法(Gauss-Newton method)原理相关推荐

高斯牛顿(Gauss Newton)、列文伯格-马夸尔特(Levenberg-Marquardt)最优化算法与VSLAM
转载请说明出处:http://blog.csdn.net/zhubaohua_bupt/article/details/74973347 在VSLAM优化部分,我们多次谈到,构建一个关于待优化位姿的误 ...
牛顿法（Newton Method）的原理和实现步骤
牛顿法的法的目的牛顿法不仅可以用来求解函数的极值问题,还可以用来求解方程的根,二者在本质上是一个问题,因为求解函数极值的思路是寻找导数为0的点,这就是求解方程. 牛顿法的法的原理一元函数的情况根 ...
牛顿法 Newton Method
上一次我们讨论了具有 Q-线性收敛性的普通的 gradient descent 方法,今天我们要介绍一种收敛速度更快的算法:Newton Method(或者叫 Newton's Method). 可能 ...
牛顿法(Newton‘s method)和拟牛顿法(quasi Newton method)
简述在看伊恩·古德费洛的深度学习,4.3节基于梯度的优化方法时提到仅使用梯度信息的优化算法称为一阶优化算法 ,如梯度下降. 使用Hessian矩阵的优化算法称为二阶最优化算法 ,如牛顿法. 牛 ...
数值分析原理课程实验——（高斯）Gauss列主元消去法
高斯(Gauss)列主元消去法方法概要待求问题程序流程程序代码 /*Matlab函数 function Result = Gauss(n, A, b)for k = 1:n-1max = ab ...
最优化学习牛顿法（Newton’s method）
牛顿法(Newton's method) 牛顿法(Newton's method) 收敛性分析 ∃ η > 0 \exists \eta>0 ∃η>0 图示和例子优点和缺陷全部笔 ...
牛顿法、梯度下降法、高斯牛顿法、Levenberg-Marquardt算法
何为梯度? 一般解释: f(x)在x0的梯度:就是f(x)变化最快的方向举个例子,f()是一座山,站在半山腰, 往x方向走1米,高度上升0.4米,也就是说x方向上的偏导是 0.4 往y方向走1米,高 ...
牛顿法，高斯-牛顿法
牛顿法(Newton's method) 假如已知函数 f(x)f(x)f(x),想要求 f(x)=0f(x)=0f(x)=0 的解(或者叫根). 牛顿法(Newton's method)大致的思想是 ...
最优化八：高斯牛顿法、LM法
梯度法:,负梯度方向牛顿法:,A为Hession矩阵高斯牛顿法:,为的解 LM法:,为的解 1 高斯牛顿法(Gauss-Newton) 针对优化问题求解x使得f(x)取得最小值,采用高斯牛顿法,步 ...
非线性最小二乘法之Gauss Newton、L-M、Dog-Leg9
最优化理论·非线性最小二乘最优化理论·非线性最小二乘_努力努力努力-CSDN博客最快下降法理解: a*b = |a|*|b|*cos() 最小二乘问题通常的最小二乘问题都可以表示为: Gaus ...

c++实现高斯牛顿法(Gauss-Newton method)原理

c++实现高斯牛顿法(Gauss-Newton method)原理相关推荐

最新文章

热门文章