非线性优化问题基本形式概述

非线性优化问题以及在视觉SLAM中的应用

1.0 最小二乘基础概念

• 定义

$$ 找到一个 n 维的变量 ${\mathbf{x}}^{\ast }\in {\mathbb{R}}^n$ , 使得损失函数 $F(\mathbf{x})$ 取局部最小值：

$$F(\mathbf{x})=\frac{1}{2}\sum _{i=1}^m{(f_i(\mathbf{x}))}^2$$

$$其中 $f_i$ 是残差函数, 比如测量值和预测值之间的差, 且有 $m\ge n$ 。 部最小值指对任意 $\Vert \mathbf{x}-{\mathbf{x}}^{\ast }\Vert <\delta$ 有 $F\left({\mathbf{x}}^{\ast }\right)\le F(\mathbf{x})$
$$损失函数泰勒展开，假设损失函数 $F(\mathbf{x})$ 是可导并且平滑的, 因此, 二阶泰勒展开:

$$F(\mathbf{x}+\mathrm{\Delta }\mathbf{x})=F(\mathbf{x})+\mathbf{J}\mathrm{\Delta }\mathbf{x}+\frac{1}{2}\mathrm{\Delta }{\mathbf{x}}^{\mathrm{\top }}\mathbf{H}\mathrm{\Delta }\mathbf{x}+O(\Vert \mathrm{\Delta }\mathbf{x}{\Vert }^3)$$

这里要着重注意一下，这里的 $\mathbf{J}$ 和 $\mathbf{H}$ 都是每一个残差项的雅可比堆叠(计算)而来，实际上对于初学者来说并不直观，后面我们会以一个曲线拟合和 $BA$ 问题来详细分析一下如何通过连加来推算到 $\mathbf{J}$ 和 $\mathbf{H}$

$$其中 $\mathbf{J}$ 和 $\mathbf{H}$ 分别为损失函数 $F$ 对变量 $x$ 的一阶导和二阶导矩阵，也就是我们通常所说的雅可比矩阵和海塞矩阵。(这里的 $\mathbf{x}$ 包含了所有待优化的变量，在视觉SLAM问题中，一般是相机的 Pose 和已经三角化的点的坐标或者逆深度，且由于相机一般能观测到的3D点的个数是有限的，因此其雅可比矩阵也就是稀疏的，只有两个地方的雅可比求导是不为0的，参考14讲P247，那么 $J_{i,j}^T{J}_{i,j}$，则只有四个地方是不为0的)。

• 损失函数泰勒展开的性质

$$ 忽略泰勒展开的高阶项,损失函数变成了二次函数,可以轻易得到如下性质：

1. 如果在点 $x_s$ 处有导数为 $0$ ,则称这个点为稳定点。
2. 在点 $x_s$ 处对应的 Hessian 为 $\mathbf{H}$：
   • 如果是正定矩阵，即它的特征值都大于 $0$，则在 $x_s$ 处有 $F(x)$ 为局部最小值。
   • 如果是复定矩阵，即它的特征值都小于 $0$，则在 $x_s$ 处有 $F(x)$ 为局部最大值。
   • 如果是不定矩阵，即它的特征值大于 $0$ 也有小于 $0$ 的,则 $x_s$ 处为鞍点。

• 求解方法主要有以下两种：
  • 直接求解：线性最小二乘（这里不再赘述，为线性代数的内容，超定方程的通解为 $x=(A^{T}A{)}^{-1}Ab$）
  • 迭代下降法：适用于线性和非线性最小二乘

2.0 迭代下降求解方式

• 迭代法初衷：

  找到一个下降方向使得损失函数随着 $x$ 的迭代逐渐减少直到 $x^{\ast }$。

  $$F(x_{k+1})<F(x_k)$$

  分两个步骤；第一，找到下降方向单位向量 $d$ ，第二，确定下降的步长 $a$。

  假设 $a$ 足够的小，又因为 $d$ 为单位向量，因此可以将 $ad$ 看作是一个微小的变化量 $\mathrm{△}x$，我们可以对损失函数进行一阶泰勒展开：

  $$F(\mathbf{x}+a\mathbf{d})\approx F(\mathbf{x})+a\mathbf{J}\mathbf{d}$$

  只需要寻找下降方向，满足：

  $$\mathbf{Jd}<0$$

  通过 line search 的方法找到下降的步长：$a^{\ast }=argmi{n}_{a>0}[F(x+ad)]$

2.1 最速下降法: 适用于迭代的开始阶段

适用于迭代的开始阶段

$$ 从下降方向的条件(单位向量)可以知道: $\mathbf{Jd}\mathbf{=}\mathbf{|}\mathbf{|}\mathbf{J}\mathbf{|}\mathbf{|}cos\theta$ ，其中 $\theta$ 表示的是下降方向和梯度方向的夹角. 当 $\theta =\pi$ 有:

这里为什么能写成向量的内积运算,笔者在刚开始看起来还认为是两个矩阵相乘法,却直接写成了内积运算，仔细思索发现 $d$ 其实上是一个和 $x$ 相同维度的单位向量，其纬度为 $n\times 1$ ，而雅可比矩阵

$$\mathbf{d}\mathbf{=}\frac{\mathbf{-}{\mathbf{J}}^{\mathbf{T}}}{\mathbf{|}\mathbf{|}\mathbf{J}\mathbf{|}\mathbf{|}}$$

$$即梯度的负方向为最速下降方向。缺点:最优值附近震荡，收敛慢。

2.2 牛顿法: 适用于最优值附近

在局部最优点 $x^{\ast }$ 附近,如果 $x+∆x$ 是最优解,则损失函数对 $∆x$ 的导数等于 $0$,对公式 (2) 取一阶导有:

$$\begin{array}{rl}\frac{\partial }{\partial \mathrm{\Delta }x}(F(\mathbf{x})+\mathbf{J}\mathrm{\Delta }\mathbf{x}+\frac{1}{2}\mathrm{\Delta }{\mathbf{x}}^{\mathrm{\top }}\mathbf{H}\mathrm{\Delta }\mathbf{x})& ={\mathbf{J}}^{\mathbf{T}}+\mathbf{H}\mathrm{\Delta }x=0\end{array}$$

得到:$∆x=-{\mathbf{H}}^{\mathbf{-}\mathbf{1}}{\mathbf{J}}^{\mathbf{T}}$ 。缺点:二阶导矩阵计算复杂。

这里我们其实既可以看作是多个残差的分量相加后组成的 $H$，也可以看作是每个残差单独的 $H$。

2.3 阻尼法：防止 $\mathrm{\Delta }x$ 的模过大

将损失函数的二阶泰勒展开记作：

$$F(\mathbf{x}+\mathrm{\Delta }x)\approx L(\mathrm{\Delta }x)=F(\mathbf{x})+\mathbf{J}\mathrm{\Delta }\mathbf{x}+\frac{1}{2}\mathrm{\Delta }{\mathbf{x}}^{\mathrm{\top }}\mathbf{H}\mathrm{\Delta }\mathbf{x}$$

求以下函数的最小化:

$$\mathrm{\Delta }x=arg\underset{\mathrm{\Delta }x}{min}\{L\left(\mathrm{\Delta }x\right)+\frac{1}{2}\mu \mathrm{\Delta }{x}^T\mathrm{\Delta }x\}$$

其中,$\mu \ge 0$ 为阻尼因子, $\frac{1}{2}\mu \mathrm{\Delta }{x}^T\mathrm{\Delta }x$是惩罚项。对新的损失函数求一阶导,并令其等于 $0$ 有:

$$\begin{gathered} {\mathbf{L}}^{\mathbf{‘}}\mathbf{(}\mathbf{\Delta }\mathbf{x}\mathbf{)}+\mu \mathbf{\Delta }\mathbf{x}=0 \\ (\mathbf{H}+\mu \mathbf{I})\mathrm{\Delta }x=-{\mathbf{J}}^{\mathbf{T}} \end{gathered}$$

2.4 Gauss-Newton 和 LM

残差函数 $f(x)$ 为非线性函数，对其进行一阶泰勒近似有：

$$f(x+\mathrm{\Delta }x)\approx \ell (\mathrm{\Delta }x)\equiv f(x)+J\mathrm{\Delta }x$$

带入损失函数：

$$\begin{array}{rl}F(\mathbf{x}+\mathrm{\Delta }\mathbf{x})\approx L(\mathrm{\Delta }\mathbf{x})& \equiv \frac{1}{2}\ell (\mathrm{\Delta }\mathbf{x}{)}^{\mathrm{\top }}\ell (\mathrm{\Delta }\mathbf{x})\\ & =\frac{1}{2}{\mathbf{f}}^{\mathrm{\top }}\mathbf{f}+\mathrm{\Delta }{\mathbf{x}}^{\mathrm{\top }}{\mathbf{J}}^{\mathrm{\top }}\mathbf{f}+\frac{1}{2}\mathrm{\Delta }{\mathbf{x}}^{\mathrm{\top }}{\mathbf{J}}^{\mathrm{\top }}\mathbf{J}\mathrm{\Delta }\mathbf{x}\\ & =F(\mathbf{x})+\mathrm{\Delta }{\mathbf{x}}^{\mathrm{\top }}{\mathbf{J}}^{\mathrm{\top }}\mathbf{f}+\frac{1}{2}\mathrm{\Delta }{\mathbf{x}}^{\mathrm{\top }}{\mathbf{J}}^{\mathrm{\top }}\mathbf{J}\mathrm{\Delta }\mathbf{x}\end{array}$$

这里我们假设暂时只关注其中的一项（其实也可以是所有损失项的叠加，最终形式是一样的）。在 $x$ 处进行的泰勒展开，则认为 $x$ 是已知的，现在的损失函数是一个关于 $\mathrm{\Delta }x$ 的函数，其最小值，则令关于 $\mathrm{\Delta }x$ 的导数为 $0$ 即可。可以得到：

$$\mathbf{(}{\mathbf{J}}^{\mathbf{T}}\mathbf{J}\mathbf{)}\mathrm{\Delta }{x}_{gn}=-{\mathbf{J}}^{\mathbf{T}}\mathbf{f}$$

上面这个形式就是我们在论文或者各种SLAM问题中经常见到的形式了，$\mathbf{H}\mathbf{\Delta }\mathbf{x}\mathbf{=}\mathbf{b}$，也叫做 normal equation

曲线拟合理解

现在我们通过非线性最小二乘来进行线性拟合实验，将理论应用于实际中去。假设曲线方程为：

$$y=\exp (a{x}^2+bx+c)$$

其中设 $a=1,b=2,c=3$ 。

现在加入噪声项生成带有高斯分布的噪声数据，当然不是高斯分布的数据也是可以的，但是在自己实验的时候尽量不要出现外点数据，因为我们并没有处理外点数据的策略。则生成数据的方程为：

$$y=\exp (a{x}^2+bx+c)+w$$

其中 $w$ 为符合高斯分布的噪声数据。

通过如下程序生成观测数据：

double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
int N = 100;                                 					// 数据点
double w_sigma = 1.0;                      	 	 // 噪声Sigma值
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));
}

接下来我们关心雅可比如何计算，误差项 $f_i(a,b,c)$ 可以写成如下形式：

$$f_i(a,b,c)=y_i-exp(a_e{x}_i^2+b_e{x}_i+c_e)$$

我们知道这两项相减是绝对不可能相等的，因为在生成数据的时候加入了高斯噪声。我们这里有 $N$ 个观测，即 $i\in (1-100)$，我们将其写成连加的形式

$$F(X)=\sum _{i=1}^N{(y_i-exp(a_e{x}_i^2+b_e{x}_i+c_e))}^2$$

该式中右边就是残差项的具体形式，我们对其进行平方，防止负的残差和正的残差抵消的情况，前面我们已经说过可以将残差项通过一阶泰勒展开进行近似，然后进行平方得到每一个残差项的具体形式：

$f(x+\mathrm{\Delta }x)\approx f(x)+J(x)\mathrm{\Delta }x$

$$\begin{array}{rl}\frac{1}{2}\Vert f(\mathit{x})+\mathit{J}(\mathit{x})\mathrm{\Delta }\mathit{x}{\Vert }^2& =\frac{1}{2}(f(\mathit{x})+\mathit{J}(\mathit{x})\mathrm{\Delta }\mathit{x}{)}^T(f(\mathit{x})+\mathit{J}(\mathit{x})\mathrm{\Delta }\mathit{x})\\ & =\frac{1}{2}(\Vert f(\mathit{x}){\Vert }_2^2+2f(\mathit{x}{)}^T\mathit{J}(\mathit{x})\mathrm{\Delta }\mathit{x}+\mathrm{\Delta }{\mathit{x}}^T\mathit{J}(\mathit{x}{)}^T\mathit{J}(\mathit{x})\mathrm{\Delta }\mathit{x})\\ & =\mathrm{\Omega }(\mathrm{\Delta }x)\end{array}$$

此时由于某时刻的观测已知，因此误差项是一个关于 $\mathrm{\Delta }x$ 的二次函数，求该项的最小值只要让关于 $\mathrm{\Delta }x$ 的导数为 $0$ 即可。求导后可得：

$$\begin{array}{l}2\mathit{J}(\mathit{x}{)}^{T}f(\mathit{x})+2\mathit{J}(\mathit{x}{)}^T\mathit{J}(\mathit{x})\mathrm{\Delta }\mathit{x}=\mathbf{0}\\ \mathit{J}(\mathit{x}{)}^T\mathit{J}(\mathit{x})\mathrm{\Delta }\mathit{x}=-\mathit{J}(\mathit{x}{)}^{T}f(\mathit{x})\end{array}$$

这里我们简单的记：

$$\begin{gathered} \mathit{J}(\mathit{x}{)}^{T}f(\mathit{x})=\eta \\ \mathit{J}(\mathit{x}{)}^T\mathit{J}(\mathit{x})\mathrm{\Delta }\mathit{x}=\mathbf{H}\mathbf{\Delta }\mathbf{x} \end{gathered}$$

即我们常见的形式：

读者要注意到这里的 $b$ 其实就是上面的 $-\eta$

$$\mathbf{H}\mathbf{\Delta }\mathbf{x}\mathbf{=}\mathbf{b}$$

这里我们假设残差项记为 ${\mathbf{e}}_{\mathbf{i}}$ 一共有 $N$ 个观测，则有 $N$ 个残差项。

$$F(X)={\mathbf{e}}_{\mathbf{1}}^{\mathbf{2}}\mathbf{+}{\mathbf{e}}_{\mathbf{2}}^{\mathbf{2}}\mathbf{+}{\mathbf{e}}_{\mathbf{3}}^{\mathbf{2}}\mathbf{+}{\mathbf{e}}_{\mathbf{4}}^{\mathbf{2}}\mathbf{+}{\mathbf{e}}_{\mathbf{5}}^{\mathbf{2}}\mathbf{+}\cdots \mathbf{+}{\mathbf{e}}_{\mathbf{N}}^{\mathbf{2}}$$

整个 $F(X)$ 此时是关于待优化变量的函数，每一项分别用各自的一阶泰勒展开近似，注意这里的每一项由于观测的不同，每一项都是一个不同的函数表达式，但是优化变量都是一样的。得到如下结果：

$$\begin{array}{rl}\frac{1}{2}\Vert f(\mathit{x})+\mathit{J}(\mathit{x})\mathrm{\Delta }\mathit{x}{\Vert }^2& =\mathrm{\Omega }(\mathrm{\Delta }x)\end{array}$$

$$F(X)\approx \mathrm{\Omega }(\mathrm{\Delta }x{)}_1+\mathrm{\Omega }(\mathrm{\Delta }x{)}_2+\mathrm{\Omega }(\mathrm{\Delta }x{)}_3+\mathrm{\Omega }(\mathrm{\Delta }x{)}_4\cdots +\mathrm{\Omega }(\mathrm{\Delta }x{)}_N$$

这里的 $\mathrm{\Delta }x$ 是我们在使用基于迭代下降的方法中所选中的步长和方向，如果 $F(X)$ 在 $\mathrm{\Delta }x$ 为某个值时取得极小值，则 $\mathrm{\Delta }x$无论是在任何一个方向加或者减函数值都会上升，此时这个点则为极小值点，这里的叙述不太数学化，但是大家联想一下极小值的定义，应该是可以理解的，当达到该条件后，那么该点关于 $\mathrm{\Delta }x$ 的导数一定为 $0$ 。所以对此时的$F(X)$求导并让其等于 $0$ 得到：

$${\mathbf{H}}_{\mathbf{1}}\mathbf{\Delta }\mathbf{x}+{\eta }_{\mathbf{1}}+{\mathbf{H}}_{\mathbf{2}}\mathbf{\Delta }\mathbf{x}+{\eta }_{\mathbf{2}}+{\mathbf{H}}_{\mathbf{3}}\mathbf{\Delta }\mathbf{x}+{\eta }_{\mathbf{3}}\cdots +{\mathbf{H}}_{\mathbf{N}}\mathbf{\Delta }\mathbf{x}+{\eta }_{\mathbf{N}}=\mathbf{0}$$

再将该式子变形，将关于 $\mathrm{\Delta }x$ 的项都移动到左边，没有关于 $\mathrm{\Delta }x$ 的移动到右边：

$${\mathbf{H}}_{\mathbf{1}}\mathbf{\Delta }\mathbf{x}+{\mathbf{H}}_{\mathbf{2}}\mathbf{\Delta }\mathbf{x}+{\mathbf{H}}_{\mathbf{3}}\mathbf{\Delta }\mathbf{x}+\cdots +{\mathbf{H}}_{\mathbf{N}}\mathbf{\Delta }\mathbf{x}=-{\eta }_{\mathbf{1}}-{\eta }_{\mathbf{2}}-{\eta }_{\mathbf{3}}\cdots -{\eta }_{\mathbf{N}}$$

其实也就是:

$${\mathbf{H}}_{\mathbf{1}}\mathbf{\Delta }\mathbf{x}+{\mathbf{H}}_{\mathbf{2}}\mathbf{\Delta }\mathbf{x}+{\mathbf{H}}_{\mathbf{3}}\mathbf{\Delta }\mathbf{x}+\cdots +{\mathbf{H}}_{\mathbf{N}}\mathbf{\Delta }\mathbf{x}={\mathbf{b}}_{\mathbf{1}}+{\mathbf{b}}_{\mathbf{2}}+{\mathbf{b}}_{\mathbf{3}}\cdots +{\mathbf{b}}_{\mathbf{N}}$$

写成连加的形式：

$$\mathrm{\Delta }x\sum _{i=1}^{N}H=\sum _{i=1}^{N}b$$

这里我们就通过每一项的一个具体形式来推倒出最后的 H 和 b 是怎么来的了。也就是我们经常在程序中见到的 += 操作的原理：

H +=  J * J.transpose();
b += -J * error;

我们再次回到曲线拟合的题目中去，待优化的变量就三个 $a,b,c$ 则每一个残差项都含有这三个参数，我们称其雅可比为稠密的（虽然只有三个参数，视觉BA问题中由于相机观测的特殊性，其雅可比矩阵是稀疏的），对每一个残差向分别求雅可比，然后求和得到最终的 $H$ 和 $b$ ，然后求解一次 $\mathrm{\Delta }x$ ，Step 一次，根据判断条件选择是否继续进行迭代。每一个残差项对于 $\mathrm{\Delta }x$ 的雅可比为

$$\begin{array}{l}J[0{]}_a=-x_i^2\exp (a_e{x}_i^2-b_e{x}_i-c)\\ J[1{]}_b=-x_i\exp (a_e{x}_i^2-b_e{x}_i-c)\\ J[2{]}_c=-\exp (a_e{x}_i^2-b_e{x}_i-c)\end{array}$$

得到了雅可比，那么剩下的就是迭代求解即可，完整代码如下，来自14讲配套代码：

#include 
#include 
#include 
#include 
#include 

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.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

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  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 +=  J * J.transpose();
      b += -J * error;

      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;
  }

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_castdouble>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

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

• 运行结果如下：

total cost: 3.19575e+06,                update: 0.0455771  0.078164 -0.985329           estimated params: 2.04558,-0.921836,4.01467
total cost: 376785,             update:  0.065762  0.224972 -0.962521           estimated params: 2.11134,-0.696864,3.05215
total cost: 35673.6,            update: -0.0670241   0.617616  -0.907497                estimated params: 2.04432,-0.0792484,2.14465
total cost: 2195.01,            update: -0.522767   1.19192 -0.756452           estimated params: 1.52155,1.11267,1.3882
total cost: 174.853,            update: -0.537502  0.909933 -0.386395           estimated params: 0.984045,2.0226,1.00181
total cost: 102.78,             update: -0.0919666   0.147331 -0.0573675                estimated params: 0.892079,2.16994,0.944438
total cost: 101.937,            update: -0.00117081  0.00196749 -0.00081055             estimated params: 0.890908,2.1719,0.943628
total cost: 101.937,            update:   3.4312e-06 -4.28555e-06  1.08348e-06          estimated params: 0.890912,2.1719,0.943629
total cost: 101.937,            update: -2.01204e-08  2.68928e-08 -7.86602e-09          estimated params: 0.890912,2.1719,0.943629
cost: 101.937>= last cost: 101.937, break.
solve time cost = 0.00440302 seconds. 
estimated abc = 0.890912, 2.1719, 0.943629

• 和真实结果对比，这里的准确度取决于我们噪声方差的大小

	$a$	$b$	$c$
Estimate	$0.890912$	$2.1719$	$0.943629$
Real	$1$	$2$	$1$

下一节我们来讨论一下视觉SLAM中的非线性优化问题的具体形式，以及其 $H$ 和 $b$ 的由来和构建方法。