最小二乘法曲线拟合（代码注释）

    最小二乘使所有点到曲线的方差最小.利用最小二乘对扫描线上的所有数据点进行拟合,得到一条样条曲线,然后逐点计算每一个点Pi到样条曲线的欧拉距离ei（即点到曲线的最短距离）,ε是距离的阈值，事先给定，如果ei≥ε，则将该点判断为噪点.

该方法最重要的事先拟合样条曲线。

确定曲线类型的方法：根据已知数据点类型初步确定曲线类型，经验观察初步尝试拟合函数类型.

曲线类型选择：直线，二次曲线，三次曲线，对数函数拟合，幂函数拟合，直至方差最小。

直线：f(X1) = aX1 + b;

二次曲线：f(X1) = aX12 + bX1 + c;

对数函数：f(X1) = a + b log(X1);

幂函数： f(X1)  = aX1b

​​​​​​​#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;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;  //计算sigma的倒数，之后用于误差归一化
  cv::RNG rng;                                 // OpenCV随机数产生器

  //产生100组数据，xi,yi(带噪声)
  vector<double> x_data, y_data;
  for (int i = 0; i < N; i++)
  {
    double x = i / 100.0; //相当于x范围是0-1
    x_data.push_back(x);

    //rng.gaussian(val)，表示生成一个服从均值为0，标准差为val的高斯分布的随机数
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 开始Gauss-Newton迭代
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost代价和上一次迭代的cost代价
  for (int iter = 0; iter < iterations; iter++)
  {
      //将矩阵H初始化为3*3零矩阵，表示海塞矩阵，H = J * (sigma * sigma).transpose() * J.transpose()
      Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton

      //将b初始化为3*1零向量，b = -J * (sigma * sigma).transpose() * error，error表示测量方程的残差
      Vector3d b = Vector3d::Zero();             // bias
      cost = 0;

    //遍历所有数据点计算H，b和cost
    for (int i = 0; i < N; i++)
    {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - exp(ae * xi * xi + be * xi + ce);  //观测值-模型计算值
      Vector3d J; // 雅可比矩阵
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
      J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

      H += inv_sigma * inv_sigma * J * J.transpose();  //这里除以sigma是归一化
      b += -inv_sigma * inv_sigma * error * J;

      cost += error * error; //error表示测量方程的残差

      //cout << "N: " << i <<endl;
    }

    // 求解线性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);  //ldlt()表示利用Cholesky分解求dx
    if (isnan(dx[0]))  //isnan()函数判断输入是否为非数字，是非数字返回真，nan全称为not a number
    {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost)  //因为iter要大于0，第1次迭代(iter = 0, cost > lastCost)不执行！
    {
      cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      break;
    }

    //更新优化变量ae，be和ce！
    ae += dx[0];
    be += dx[1];
    ce += dx[2];

    lastCost = cost;

    //cout << "iter total: " << iter << endl;
    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }

  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}