[C++知识库]高斯牛顿法在具体工程中的应用——C++版

高斯牛顿法在具体工程中的应用——C++版

说明：本文章没有用大量篇幅来讲述高斯牛顿的原理和数学中的应用，而是用具体的代码来说明，具体是怎么应用的。
如果对高斯牛顿法的原理比较感兴趣，可以阅读以下链接中的内容：
https://zhuanlan.zhihu.com/p/42383070

概述：高斯牛顿法解决的是工程中的非线性最小二乘问题，如SLAM。具体代码如下：

首先我们需要定义以下参数：

 //设定的真实参数的值
 double ar = 1.0, br = 2.0, cr = 1.0;  
 // 估计参数值，每一次迭代的时候，设定的参数值是不一样的，这里是初始化参数       
 double ae = 2.0, be = -1.0, ce = 5.0;
 //设置的数据集的点数共100个数组        
 int N = 100;   
 //设置的噪声值，值越低表明对实际的影响越小                             
 double w_sigma = 0.01;   
 //调用CV库里的rng函数，随机值                
 cv::RNG rng;                                 

其次，我们要设置一个数据集，每一个问题的数据集是不一样的，自己设置

//定义的100个数据点，数据集
vector<double> x_data, y_data;      
  for (int i = 0; i < N; i++) 
  {
  	  //定义的输入xi
      double x = i / 100.0;
      //push.函数的作用：将一个新的元素加到vector的最后面，位置为当前最后一个元素的下一个元素
      x_data.push_back(x);
      //返回的随机数的平均值为零，标准偏差为指定的 sigma （高斯分布的特征）
      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和上一次迭代的cost
    for (int iter = 0; iter < iterations; iter++) {
        //定义一个海塞矩阵，
        Matrix3d H = Matrix3d::Zero();   // Hessian = J^T J in Gauss-Newton，这里的J为行向量，我们最终想得到的是一个矩阵
        //定义一个偏置矩阵
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;
        //每一次的迭代，都有一个对应的参数值，也就是a,b,c的值
        for (int i = 0; i < N; i++) {
            //xi,yi为定义的数据集，yi在这里为实际值，xi为输入值
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
           double error = 0;   // 第i个数据点的计算误差
           //每一个error的大小，一共100个
           //yi为观测值
           //exp(ae * xi * xi + be * xi + ce）为理论值拟合函数
           error=yi - (exp(ae * xi * xi + be * xi + ce));
           // error = 0; // 填写计算error的表达式
            Vector3d J; // 雅可比矩阵：函数对参数求偏导
           //求出每一个数值的Jacbian矩阵，对a,b,c参数分别求偏导
            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);
            cout<<"J"<<i<<":  "<<J.transpose()<<endl;
            //hessian矩阵用累加法，GN近似的H
            H += J * J.transpose(); 
            cout<<"H:"<<H.transpose()<<endl;
            b += J*(-error) ;
            cout<<"b: "<<b.transpose()<<endl;
            //代价函数
            cost += error * error;
        }
        // 求解线性方程 Hx=b，建议用ldlt
        Vector3d dx;
        cout<<"H:"<< H<<endl;
        cout<<"B:"<< b<<endl;
        //H的逆矩阵再乘以b
        dx = H.ldlt().solve(b);
        if (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 << "total cost: " << cost << endl;
    }
    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}