人家都写好了,自己主要是理解一下这个代码吧,用雅克比矩阵代表需要优化的参数:J 。 一个海森矩阵和一个b矩阵用来迭代。 在每次迭代过程中, 优化的方向是 由 HX=b ,求解出来的x
给定的。 之中H和 b的计算,可以简单地由 J矩阵得到,也就是:
H+= inv_sigma * inv_sigma * J * J.transpose();
b+= -inv_sigma* inv_sigma * error * J;
然后通过
dx = H.ldlt().solve(b)
得到δx 。 这就是我们更新的方向,所有的估计值都进行更新,进行如下动作:
ae+=dx[0];
be+=dx[1];
ce+=dx[2];
最终判断不收敛或者迭代次数用光,则跳出就好了。整体代码如下:
/*************************************************************************
> File Name : gaussNewton.cpp
> Author : enjoy5512
> Mail : enjoy5512@163.com
> Created Time : 2021年07月30日 星期五 08时59分03秒
************************************************************************/
#include<iostream>
#include<opencv2/opencv.hpp>
#include<Eigen/Core>
#include<Eigen/Dense>
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;
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; //opencv的随机数产生器
//源数据
vector<double> x_data, y_data;
//生成源数据
for (int i=0; i<N; i++){
double temp = i/100;
x_data.push_back(temp);
y_data.push_back(exp(ar*temp*temp + br*temp + cr) + rng.gaussian(w_sigma * w_sigma));
}
//开始进行 guassNewton迭代
int iteration = 100;
double cost=0, last_cost=0;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
for(int iter=0;iter<iteration;iter++){
Matrix3d H = Matrix3d::Zero();
Vector3d b = Vector3d::Zero();
cost=0;
for(int i=0;i<N;i++){
double xi=x_data[i];
double yi=y_data[i];
double error = yi - exp(ae*xi*xi + be*xi +ce);
Vector3d J;
J[0] = -xi*xi*exp(ae*xi*xi + be*xi +ce);
J[1] = -xi*exp(ae*xi*xi + be*xi + ce);
J[2] = -exp(ae*xi*xi + be*xi +ce);
H+= inv_sigma * inv_sigma * J * J.transpose();
b+= -inv_sigma* inv_sigma * error * J;
cost+=error*error;
}
Vector3d dx = H.ldlt().solve(b);
if(isnan(dx[0])){
cout<< "result is nan" <<endl;
break;
}
if(iter > 0 &&cost>last_cost){
cout<< "在第" <<iter<<"轮误差不降反增!"<<endl;
cout<< cost <<" -> "<< last_cost<<endl;
break;
}
ae+=dx[0];
be+=dx[1];
ce+=dx[2];
last_cost = cost;
cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
"\t\testimated 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<<"共耗时: "<< time_used.count() <<" seconds" <<endl;
cout<<"真实值为: "<< ar<<" "<<br <<" "<<cr<<endl;
cout<<"估计值为: "<< ae <<" "<<be<<" "<<ce<<endl;
return 0;
}