【数学与算法】非线性最小二乘法的解法【最速梯度下降法】、【牛顿法】、【高斯牛顿法】、【LM算法】

关于非线性优化的问题，可以推荐观看视觉SLAM十四讲视频的第六讲 非线性优化。

如果不明白线性和非线性，可参考这篇博客：线性最小二乘和非线性最小二乘

这篇博客的后面有讲到几种优化方法(最速梯度下降法、牛顿法、高斯牛顿法，LM算法)，很容易记住，不像其他的公式推导那么生硬：Bundle Adjustment—即最小化重投影误差（高翔slam—第七讲）

这篇博客也很棒非线性优化（高翔slam—第六讲 ）

下图摘自非线性优化

---

非线性最小二乘:

所谓非线性，就是f(x)无法表示为的线性关系，而是某种非线性关系。

这让求解导函数为零的问题变成了一个不断寻找下降增量 ∆xk 的问题。以下就是寻找增量的方法。

下面是对平方展开的：

一阶梯度法(梯度下降法):

不需要求步长(设定固定步长)叫梯度下降法，
每一步迭代都需要求最优步长的叫做最速下降法。

二阶梯度法(又叫牛顿法):

牛顿法与高斯牛顿法的不同之处：

牛顿法是直接对误差的平方$||f(x+\Delta x)|{|}_2^2$ 在x处进行泰勒展开。
高斯牛顿法是对误差函数$f(x+\Delta x)$在x处进行泰勒展开后得到$f(x+∆x)=f(x)+J(x)∆x$，再对$f(x+∆x)=f(x)+J(x)∆x$进行平方操作。

评价一阶梯度与二阶梯度：

一阶梯度法(梯度下降法)，太慢了，因为它每次都会找到最陡的方向进行下降。如果是最速下降法(比梯度下降法每一步迭代多了求最优步长)，前后两次迭代的梯度向量方向是正交的，也就是说一直走的是直角，故就算很简单的也会走很多步，即过于贪婪(zigzag 问题)，过于贪心，容易走出锯齿路线，反而增加了迭代次数；

而二阶梯度法(牛顿法)，迭代次数少，对高阶表现良好，但不可避免的我们需要计算目标函数的 H 矩阵，这在问题规模较大时非常困难，我们通常倾向于避免 H 的计算。

高斯牛顿法:

令$H=J(x)J(x{)}^T，g=-J(x)f(x)$，则高斯牛顿方程变为：
$$H∆x=g$$

重中之重，牛顿法和高斯牛顿法的对比：对比牛顿法中 $H\ast ∆x=-J$，高斯牛顿法用$J(x)J(x{)}^T$作为牛顿法中Hessian矩阵的近似，从而省略了$H$的计算。

求解高斯牛顿方程是整个优化问题的核心所在，如果能解出该方程，则高斯牛顿法的步骤如下：

为了求解增量方程，需要解出$H^{-1}$，这需要$H$矩阵可逆。但是实际上$H$只有半正定，也就是$H$可能会是奇异矩阵或ill-condition的情况，此时增量的稳定性较差，导致算法不收敛。就算H非奇异也非病态，但是如果求出来的步长 $∆x$ 太大，也无法保证能迭代收敛。

牛顿法与高斯牛顿法的不同之处：

牛顿法是直接对误差的平方$||f(x+\Delta x)|{|}_2^2$ 在x处进行泰勒展开。
高斯牛顿法是对误差函数$f(x+\Delta x)$在x处进行泰勒展开后得到$f(x+∆x)=f(x)+J(x)∆x$，再对$f(x+∆x)=f(x)+J(x)∆x$进行平方操作。

列文伯格-马夸尔特(Levenberg-Marquadt):

---

例题：使用高斯牛顿法进行曲线拟合

#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

//g++ test.cpp `pkg-config opencv --libs --cflags` -std=c++11 -o test

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;                        // 噪声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));
  }
  
  // 开始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 W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;
    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();
      b += -inv_sigma * inv_sigma * error * J;
      cost += error * error;
    }
    
    // 求解线性方程 Hx=b
    Vector3d 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 << ", break." << endl;
      break;
    }
    
    ae += dx[0];
    be += dx[1];
    ce += dx[2];
    lastCost = cost;
    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }
  
  
  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}