//
// Created by 高翔 on 2017/12/15.
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <math.h>
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;                   // 噪声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个数据点// start your code heredouble ex=exp(ae*xi*xi+be*xi+ce);double error = 0;   // 第i个数据点的计算误差error = yi-ex; // 填写计算error的表达式Vector3d J; // 雅可比矩阵J[0] = -xi*xi*ex;  // de/daJ[1] = -xi*ex;  // de/dbJ[2] = -ex;  // de/dc
H += J * J.transpose(); // GN近似的Hb += -error * J;// end your code here
cost += error * error;}// 求解线性方程 Hx=b，建议用ldlt// start your code here
        Vector3d dx;dx=H.ldlt().solve(b);// end your code hereif (isnan(dx[0])) {cout << "result is nan!" << endl;break;}if (iter > 0 && cost > lastCost) {// 误差增长了，说明近似的不够好cout << "cost: " << cost << ", last cost: " << lastCost << endl;break;}// 更新abc估计值ae += dx[0];be += dx[1];ce += dx[2];lastCost = cost;cout<<"iterate: "<<iter<<endl;cout<<"ae="<<ae<<" be="<<be<<" ce="<<ce<<endl;cout<<"cost= "<<cost<<endl;// end your code here
cout << "total cost: " << cost << endl;}cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;return 0;
}