高斯牛顿法

将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} )

 

转载于:https://www.cnblogs.com/112358nizhipeng/p/9724460.html