高斯牛顿迭代估计参数-视觉slam14讲 实践

人家都写好了，自己主要是理解一下这个代码吧，用雅克比矩阵代表需要优化的参数：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;
}