将f(x)进行一阶泰勒展开,f为误差函数。
$f\left(x+\Delta{x}\right) \thickapprox f\left(x\right) + J\left(x\right)\Delta{x}$
为使$\lVert{f\left(x+\Delta{x}\right)}\rVert^{2}$达到最小。
为求$\Delta{x}$,需解一个线性的最小二乘问题。
$\Delta{x}^{\ast} =\mathop{min}\limits_{\Delta{x}} \frac{1}{2}\lVert{ f\left(x\right) + J\left(x\right)\Delta{x} }\rVert^{2}$
求极值时,对$\Delta{x}$求导,并令导数为零。
$\frac{1}{2}\lVert{f\left(x+\Delta{x}\right)}\rVert^{2}$
$=\frac{1}{2} \left( f\left(x\right) + J\left(x\right)\Delta{x} \right)^{T} \left( f\left(x\right) + J\left(x\right)\Delta{x} \right)$
$=\frac{1}{2}\left[ f\left(x\right)^{T}f\left(x\right) + \Delta{x}^{T}J\left(x\right)^{T}f\left(x\right) + f\left( x\right)^{T}J\left(x\right)\Delta{x} + \Delta{x}^{T}J\left(x\right)^{T}J\left(x\right)\Delta{x}\right]$
由于$\frac{\partial{x^TA}}{\partial{x}}=A,\frac{\partial{Ax}}{\partial{x}}=A^{T},\frac{\partial{x^TAx}}{\partial{x}}= \left(A^T+A\right)x$
矩阵求偏导参考:http://xuehy.github.io/blog/2014/04/18/2014-04-18-matrixcalc/
所以对以上式子求$\Delta{x}$偏导,并令其为零可得:
求解的变量是$\Delta{x}$,因此是一个线性方程组,为高斯牛顿方程。
写成$H\Delta{x}=g$($J^{T}J$为牛顿法中二阶Hessian矩阵近似)
从算法步骤中可以看到,增量方程的求解占据着主要地位。
Levenberg-Marquardt算法(含伪代码):http://users.ics.forth.gr/~lourakis/levmar/levmar.pdf
作业:
1 #include <iostream> 2 #include <opencv2/opencv.hpp> 3 #include <Eigen/Core> 4 #include <Eigen/Dense> 5 #include <chrono> 6 7 using namespace std; 8 using namespace Eigen; 9 10 int main(int argc, char **argv) { 11 double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值 12 13 double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值,初始值 14 int N = 100; // 数据点 15 double w_sigma = 1.0; // 噪声Sigma值 16 cv::RNG rng; // OpenCV随机数产生器 17 18 vector<double> x_data, y_data;// 生成一组真实的数据,存入x_data,y_data 19 for (int i = 0; i < N; i++) 20 { 21 double x = i / 100.0; 22 x_data.push_back(x); 23 y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma)); 24 } 25 26 // 开始Gauss-Newton迭代 27 int iterations = 100; // 迭代次数 28 double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost 29 30 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 31 32 for (int iter = 0; iter < iterations; iter++) { 33 34 Matrix3d H = Matrix3d::Zero(); // Hessian = J^T J in Gauss-Newton 35 Vector3d b = Vector3d::Zero(); // bias 36 cost = 0; 37 38 for (int i = 0; i < N; i++) 39 { 40 double xi = x_data[i], yi = y_data[i]; // 第i个数据点 41 42 43 // start your code here 44 45 double error = 0; // 第i个数据点的计算误差 46 error = exp(ae * xi * xi + be * xi + ce) - yi; // 填写计算error的表达式 47 Vector3d J; // 雅可比矩阵 48 49 J[0] = xi*xi*exp(ae*xi*xi + be * xi + ce); // de/da 50 J[1] = xi*exp(ae*xi*xi + be * xi + ce); // de/db 51 J[2] = exp(ae*xi*xi + be * xi + ce); // de/dc 52 53 H += J * J.transpose(); // GN近似的H 54 b += -error * J; 55 // end your code here 56 57 58 cost += error * error; //误差平方和 59 } 60 61 // 求解线性方程 Hx=b,建议用ldlt 62 // start your code here 63 Vector3d dx; 64 dx = H.ldlt().solve(b); 65 // end your code here 66 67 if (isnan(dx[0])) 68 { 69 cout << "result is nan!" << endl; 70 break; 71 } 72 if (iter > 0 && cost > lastCost) { 73 // 误差增长了,说明近似的不够好 74 cout << "cost: " << cost << ", last cost: " << lastCost << endl; 75 break; 76 } 77 // 更新abc估计值 78 ae += dx[0]; 79 be += dx[1]; 80 ce += dx[2]; 81 82 lastCost = cost; 83 84 cout << "total cost: " << cost << endl; 85 } 86 87 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 88 89 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2-t1); 90 91 cout<<"solve time cost = "<<time_used.count()<<" second."<<endl; 92 93 cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl; 94 return 0; 95 }
CMakeLists.txt
1 cmake_minimum_required( VERSION 2.8 ) 2 project( newton ) 3 4 # 添加c++ 11标准支持 5 set( CMAKE_CXX_FLAGS "-std=c++11" ) 6 7 # 寻找OpenCV库 8 find_package( OpenCV REQUIRED ) 9 # 添加头文件 10 include_directories( ${OpenCV_INCLUDE_DIRS} ) 11 12 include_directories("/usr/include/eigen3") 13 14 15 add_executable( newton gaussnewton.cpp ) 16 # 链接OpenCV库 17 target_link_libraries( newton ${OpenCV_LIBS} )