视觉SLAM实践入门——（7）利用高斯牛顿法进行曲线拟合

考虑满足以下方程的曲线

其中a，b，c是曲线参数，w是高斯噪声，w~(0，σ^2)（服从零均值的高斯分布）。

假设有N个关于x，y的观测数据点，通过根据这些数据求解下面最小二乘问题，求出曲线参数

在求解这个问题时，程序里先根据模型生成x，y的真值，然后在真值中添加高斯分布的噪声，再使用高斯牛顿法从带噪声的数据拟合参数模型

定义误差为：

计算每个误差项对状态变量的导数：

则

高斯牛顿法的增量方程：

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

#define N 100	//数据点

int main(void)
{  
  double ar = 1.0, br = 2.0, cr = 1.0;	//真实参数
  double ae = 2.0, be = -1.0, ce = 5.0;	//估计参数
  double w_sigma = 1.0;		//噪声sigma值
  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 x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar*x*x + br*x + cr) + rng.gaussian(w_sigma * w_sigma));
  }
  
  //高斯牛顿迭代
  int iterations = 50;	//迭代次数
  double cost = 0, lastCost = 0;	//本次迭代和上次迭代
  
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for(int iter = 0; iter < iterations; iter++)
  {
    Matrix3d H = Matrix3d::Zero();
    Vector3d b = Vector3d::Zero();
    cost = 0;
    
   for(int i = 0; i < N; i++)
   {
      double xi = x_data[i], yi = y_data[i];	//从数据集取出数据点
      double err = yi - exp(ae*xi*xi + be*xi + ce);	//误差
      Vector3d J;	//雅可比阵
      
      //因为待估计状态是a，b，c，因此J的各元素是e对a，b，c的偏导
      J[0] = -xi*xi*exp(ae*xi*xi + be*xi + ce);	//误差对a的偏导
      J[1] = -xi*exp(ae*xi*xi + be*xi + ce);	//误差对b的偏导
      J[2] = -exp(ae*xi*xi + be*xi + ce);	//误差对c的偏导
      
      H += inv_sigma*inv_sigma*J*J.transpose();
      b += -inv_sigma*inv_sigma*err*J;
      
      cost += err*err;
   }
   
    //求解Hx=b
    Vector3d dx = H.ldlt().solve(b);	//解△x，各元素表示状态的增量
    if(isnan(dx[0]))	//无解
    {
      cout << iter << ":" << "result is nan!" << endl;
      break;
    }
    
    if(iter > 0 && cost >= lastCost)	//目标函数经过迭代不再下降，已经达到局部极小值
    {
      cout << iter << ":" << "cost:" << cost << ">= last cost:" << lastCost << ",break." << endl;   
      break;
    }
   
    ae += dx[0];
    be += dx[1];
    ce += dx[2];
    
    lastCost = cost;
    
    cout << iter << ":" << "total cost : " << cost << ", \tupdate: " << dx.transpose() << "\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 << "solve time = " << time_used.count() << "s." << endl;
  cout << "estimated a = " << ae << ", b = " << be << ", c = " << ce << endl;
   

  return 0;
}