非线性优化

Reference:

1. 高翔，张涛 《视觉SLAM十四讲》

1. 线性系统和非线性系统

如果从系统状态空间表达式来观察，线性系统和非线性系统最明显的区别方法就是线性系统遵从叠加原理，而非线性系统不然。
所谓叠加原理举个例子就是：
$f(x)=2x,f(y)=2y,f(x+y)=2(x+y)=2x+2y=f(x)+f(y)$

举个反例：
$f(x)=2x^2,f(y)=2y^2,f(x)+f(y)=2(x^2+y^2)$，但 $f(x+y)=2(x+y{)}^2$，两个显然不等。

换句话说，线性系统的表达式中只有状态变量的一次项，高次、三角函数以及常数项都没有，只要有任意一个非线性环节就是非线性系统。

2. 状态估计问题

2.1 最大后验与最大似然

经典的 SLAM 模型通常由一个运动方程和一个观测方程构成，如下所示：
$$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}\end{array}$$

这里的 $x_k$ 是相机的位姿，位姿变量可以由 $T_k\in \text{SE(3)}$ 表达。运动方程与输入的具体形式有关，但在视觉 SLAM 中没有特殊性（和普通的机器人、车辆情况一样），先暂且不谈。假设使用针孔模型，那么这里的观测方程由针孔相机模型给定。假设在 $x_k$ 处对路标 $y_j$ 进行了一次观测 （$j$ 在这里是路标点的序号），对应到图像上的像素位置 $z_{k,j}$，那么，观测方程可以表示成（说白了，第一个公式为相机的位姿变换公式，第二个为拍摄到的图像像素位置变化公式，很好理解）：
$$s{z}_{k,j}=K(R_k{y}_j+t_k).$$

其中 $K$ 为相机内参，$s$ 为像素点的距离(深度)，也是 $(R_k{y}_j+t_k)$ 的第三个分量。如果使用变换矩阵 $T_k$ 描述位姿。那么路标点 $y_i$ 必须以齐次坐标来描述，计算完成后要转换为费其次坐标。

现在，考虑数据受噪声影响后会发生什么改变。在运动和观测方程中，我们通常假设两个噪声项 $w_k,v_{k,j}$ 满足零均值的高斯分布，像这样：
$${\mathbf{w}}_k\sim \mathcal{N}\left(0,{\mathbf{R}}_k\right),{\mathbf{v}}_{k,j}\sim \mathcal{N}\left(0,{\mathbf{Q}}_{k,j}\right)$$

其中 $\mathcal{N}$ 表示高斯分布，$0$ 表示零均值，${\mathbf{R}}_k$、${\mathbf{Q}}_{k,j}$ 为协方差。在这些噪声的影响下，我们希望通过带噪声的数据 $z$ 和 $u$ 推断位姿 $x$ 和地图 $y$（以及它们的概率分布），这构成了一个状态估计问题。

处理这个状态估计问题的方法大致分为两种。由于在 SLAM 过程中，这些数据是随时间逐渐到来的，所以，我们应该持有一个当前时刻的估计状态，然后用新的数据来更新它。这种方式称为增量/渐进(incremental)的方法，或者叫滤波器。在历史很长一段时间内，研究者们使用滤波器，尤其是扩展卡尔曼滤波器及其衍生方法求解它。另一种方式，则是把数据“攒”起来一并处理，这种方式称为批量(batch)的方法。例如，我们可以把 $0$ 到 $k$ 时刻所有的输入和观测数据都放在一起，那么，在这样的输入和观测下，如何估计整个 $0$ 到 $k$ 时刻的轨迹与地图呢？

这两种不同的处理方式引出了很多不同的估计手段。大体来说，增量方法只关心当前时刻的估计状态 $x_k$，而对之前的状态则不多考虑；相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器，而成为当前视觉 SLAM 的主流方法，极端情况下，我们可以让机器人或无人机收集所有时刻的数据，再带回计算中心统一处理，这也正是 SfM(Structure from Motion) 的主流做法。当然，这种极端情况显然是不实时的，不符合 SLAM 的运动场景。所以在 SLAM 中，实用的方法通常是一些折衷的手段。例如，我们固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化，如滑动窗口估计法。

理论上，批量方法更容易介绍。同时，理解了批量方法也更容易理解增量的方法。所以，本节重点介绍以非线性优化为主的批量优化方法。由于考虑的是批量方法，考虑从 $1$ 到 $N$ 的所有时刻，并假设有 $M$ 个路标点。定义所有时刻的机器人位姿和路标点坐标为：
$$x=\{x_1,...,x_N\},y=\{y_1,...,y_N\}$$

同样的，用不带下标的 $u$ 表示所有时刻的输入，$z$ 表示所有时刻的观测数据。那么我们说，对机器人状态的估计，从概率学的观点来看，就是已知 输入数据 $u$ 和 观测数据 $z$ 的条件下，求状态 $x,y$ 的条件概率分布(这种写法比较重要)：
$$P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})$$

特别地，当我们不知道控制输入，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 $P(x,y|z)$ 的条件概率分布，此问题也称为 SfM，即如何从许多图像中重建三维空间结构。

为了估计状态变量的条件分布，利用贝叶斯法则，有：
$$P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})=\frac{P(\mathbf{x},\mathbf{y},\mathbf{z},\mathbf{u})}{P(\mathbf{z},\mathbf{u})}=\frac{P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})}{P(\mathbf{z},\mathbf{u})}\propto P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y}).$$

贝叶斯法则左侧称为后验概率，右侧的 $P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})$ 称为似然(Likehood)，另一部分 $P(x,y)$ 称为先验(Prior)。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化则是可行的：
$${(\mathbf{x},\mathbf{y})}^{\ast }{}_{MAP}=\arg \max P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})=\arg \max P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y}).$$

请注意贝叶斯法则的分母部分 $P(\mathbf{z},\mathbf{u})$ 与待估计的状态 $x$, $y$ 无关，因为可以忽略。贝叶斯法则告诉我们，求解最大后验概率等价于最大化似然和先验的乘积。当然，我们也可以说，对不起，我不知道机器人位姿或路标大概在什么地方，此时就没有了先验。那么，可以求解最大似然估计(Maximize Likelihood Estimation, MLE):
$$(\mathbf{x},\mathbf{y}{)}^{\ast }{}_{MLE}=\arg \max P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y}).$$

直观地讲，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计可以理解成：“在什么样的状态下，可能产生现在观测到的数据($\mathbf{z},\mathbf{u}$)”。这就是最大似然估计的直观意义。

2.2 最小二乘的引出

那么，如何求最大似然估计呢？我们说，在高斯分布的假设下，最大似然能够有较简单的形式。回顾观测模型，对于某一次观测：
$${\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}$$

由于我们假设了噪声项 ${\mathbf{v}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_{k,j}\right)$，所以观测数据的条件概率为(即概率中心点为 $h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)$)：
$$P\left({\mathbf{z}}_{j,k}\mid {\mathbf{x}}_k,{\mathbf{y}}_j\right)=\mathcal{N}\left(h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right),{\mathbf{Q}}_{k,j}\right).$$

它依然是一个高斯分布。考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。

我们知道高斯分布负对数下有较好的数学形式。考虑任意高维高斯分布 $\mathbf{x}\sim N(\mathbf{\mu },\mathbf{\Sigma })$，它的概率密度函数展开形式为(注意这里说的是任意的高斯分布)：
$$P(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right).$$

对其取负对数，则变为：
$$-\ln (P(\mathbf{x}))=\frac{1}{2}\ln \left((2\pi {)}^N\det (\mathbf{\Sigma })\right)+\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu }).$$

因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的 $\mathbf{x}$ 时，第一项与 $\mathbf{x}$ 无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入 SLAM 的观测模型，相当于在求(跟本小节末尾公式的观测模型是一样的)：
$$({\mathbf{x}}_{\mathbf{k}},{\mathbf{y}}_{\mathbf{j}}{)}^{\ast }=\arg \max \mathcal{N}\left(h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right),{\mathbf{Q}}_{k,j}\right)=\arg \min \left({\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)}^{\mathcal{T}}{\mathbf{Q}}_{k,j}^{-1}\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)\right).$$

我们发现，该式等价于最小化噪声项（即误差）的一个二次型。这个二次型称为马哈拉诺比斯距离(Mahalanobis distance)，又叫马氏距离。它可以看成由 $Q_{k,j}^{-1}$ 加权之后的欧氏距离(二范数)，这里 $Q_{k,j}^{-1}$ 也叫作信息矩阵，即高斯分布协方差矩阵之逆。

现在我们考虑批量时刻的数据。通常假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间是独立的，各个观测之间是独立的，并且输入和观测也是独立的。于是我们可以对联合分布进行因式分解：
$$P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})=\prod _{k}P({\mathbf{u}}_{\mathbf{k}}|{\mathbf{x}}_{\mathbf{k}-1},{\mathbf{x}}_{\mathbf{k}})\prod _{k,j}P({\mathbf{z}}_{\mathbf{k},\mathbf{j}}|{\mathbf{x}}_{\mathbf{k}},{\mathbf{y}}_{\mathbf{j}})$$

这说明我们可以独立的处理各个时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：
$$\begin{array}{l}{\mathbf{e}}_{\mathbf{u},k}={\mathbf{x}}_k-f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)\\ {\mathbf{e}}_{\mathbf{z},j,k}={\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right),\end{array}$$

那么，最小化所有时刻估计值与真实读数之间的马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和(分别为运动方程和观测模型的)：
$$\min J(\mathbf{x},\mathbf{y})=\sum _k{\mathbf{e}}_{\mathbf{u},k}^T{\mathbf{R}}_k^{-1}{\mathbf{e}}_{\mathbf{u},k}+\sum _k\sum _j{\mathbf{e}}_{z,k,j}^T{\mathbf{Q}}_{k,j}^{-1}{\mathbf{e}}_{z,k,j}.$$

这样就得到了一个最小二乘问题(Least Square Problem)，它的解等价于状态的最大似然估计。直观上看，由于噪声的存在，当我们把估计的轨迹于地图代入 SLAM 的运动、观测方程中时，它们并不会完美地成立。这时怎么办呢？我们对状态的估计值进行微调，使得整体的误差下降一些。当然，这个下降也有限度，它一般会达到一个极小值。这就是一个典型的非线性优化的过程。

仔细观察上式，我们发现 SLAM 中的最小二乘问题具有一些特定的结构：

• 首先，整个问题的目标函数由许多个误差的(加权的)二次型组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。例如，运动误差只与 $x_{k-1},x_k$ 有关，观测误差只与 $x_k,y_j$ 有关。这种关系会让整个问题由一种稀疏的形式。
• 其次，如果使用李代数表示增量，则该问题是无约束的最小二乘问题。但如果用旋转矩阵/变换矩阵描述位姿，则会引入旋转矩阵自身的约束，即需在问题中加入 s.t. $R^{T}R=I$ 且 $det(R)=1$ 这样令人头大的条件。额外的约束会使优化变得更困难。这体现了李代数的优势。
• 最后，我们使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。例如，某次的观测非常准确，那么协方差矩阵就会“小”，而信息矩阵就会“大”(说白了就是协方差矩阵的逆变大了)，所以这个误差项会在整个问题中占有较高的权重。

现在，我们介绍如何使用这个最小二乘问题，这需要一些非线性优化的基本知识。特别地，我们要针对这样一个通用的无约束非线性最小二乘问题，探讨它是如何求解的。

2.3 例子：批量状态估计

笔者发现在这里举一个简单的例子会更好一些。考虑一个非常简单的离散时间系统：
$$\begin{array}{lc}{\mathbf{x}}_k={\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k,& {\mathbf{w}}_k\sim \mathcal{N}(0,{\mathcal{Q}}_k)\\ {\mathbf{z}}_k={\mathbf{x}}_k+{\mathbf{n}}_k,& {\mathbf{n}}_k\sim \mathcal{N}(0,{\mathcal{R}}_k)\end{array}$$

这可以表达一辆沿 $x$ 轴前进或后退的汽车。第一个公式为运动方程，${\mathbf{u}}_k$ 为输入，${\mathbf{w}}_k$ 为噪声；第二个公式为观测方程，${\mathbf{z}}_k$ 为对汽车位置的测量。取时间 $k=1,...,3$，现希望根据已有的 $\mathbf{x},\mathbf{z}$ 进行状态估计。设初始状态 ${\mathbf{x}}_0$ 已知。下面来推导批量状态的最大似然估计。

首先，令批量状态变量为 $\mathbf{x}=[{\mathbf{x}}_0,{\mathbf{x}}_1,{\mathbf{x}}_2,{\mathbf{x}}_3{]}^T$，令批量观测为 $\mathbf{z}=[{\mathbf{z}}_1,{\mathbf{z}}_2,{\mathbf{z}}_3{]}^T$，按同样方式定义 $\mathbf{u}=[{\mathbf{u}}_1,{\mathbf{u}}_2,{\mathbf{u}}_3{]}^T$。按照先前的推导，我们知道最大似然估计为：
$${\mathbf{x}}_{map}^{\ast }=\arg \max P(\mathbf{x}|\mathbf{u},\mathbf{z})=\arg \max P(\mathbf{u},\mathbf{z}|\mathbf{x})=\prod _{k=1}^{3}P({\mathbf{u}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_k)\prod _{k=1}^{3}P({\mathbf{z}}_k|{\mathbf{x}}_k)$$

对于具体的每一项，比如运动方程，我们知道(已知${\mathbf{x}}_{k-1}$、${\mathbf{x}}_k$，求${\mathbf{u}}_k$的概率，${\mathbf{u}}_k$中心点为${\mathbf{x}}_k-{\mathbf{x}}_{k-1}$。要是没有噪声的话，${\mathbf{u}}_k$就是这样求出来的)：
$$P({\mathbf{u}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_k)=\mathcal{N}({\mathbf{x}}_k-{\mathbf{x}}_{k-1},{\mathcal{Q}}_k),$$

观测方程也是类似的：
$$P({\mathbf{z}}_k|{\mathbf{x}}_k)=\mathcal{N}({\mathbf{x}}_k,{\mathcal{R}}_k).$$

根据这些方法，我们就能够实际的解决上面的批量状态估计问题。根据之前的叙述，可以构建误差变量：
$${\mathbf{e}}_{\mathbf{u},k}={\mathbf{x}}_k-{\mathbf{x}}_{k-1}-{\mathbf{u}}_k,{\mathbf{e}}_{\mathbf{z},k}={\mathbf{z}}_k-{\mathbf{x}}_k$$

于是最小二乘的目标函数为：
$$\min \sum _{k=1}^3{\mathbf{e}}_{\mathbf{u},k}^T{Q}_k^{-1}{\mathbf{e}}_{\mathbf{u},k}+\sum _{k=1}^3{\mathbf{e}}_{\mathbf{z},k}^T{R}_k^{-1}{\mathbf{e}}_{\mathbf{z},k}$$

此外，这个系统是线性系统，我们可以很容易地将它写成向量形式。定义向量 $\mathbf{y}=[\mathbf{u},\mathbf{z}{]}^T$，那么可以写出矩阵 $\mathbf{H}$，使得：
$$y-Hx=e\sim \mathcal{N}(0,\Sigma )$$

那么：
$$\mathbf{H}=\left[\begin{array}{cccc}1& -1& 0& 0\\ 0& 1& -1& 0\\ 0& 0& 1& -1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$$

且 $\mathbf{\Sigma }=\mathrm{diag}\left({\mathbf{Q}}_1,{\mathbf{Q}}_2,{\mathbf{Q}}_3,{\mathbf{R}}_1,{\mathbf{R}}_2,{\mathbf{R}}_3\right)$ 。整个问题可以写成
$${\mathbf{x}}_{\text{map }}^{\ast }=\arg \min {\mathbf{e}}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}\mathbf{e},$$

之后我们将看到，这个问题有唯一的解:
$${\mathbf{x}}_{\text{map }}^{\ast }={\left({\mathbf{H}}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}\mathbf{H}\right)}^{-1}{\mathbf{H}}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}\mathbf{y}$$

3. 非线性最小二乘

先来考虑一个简单的最小二乘问题(注意$F(\mathbf{x})$和$f(\mathbf{x})$)：
$$\underset{x}{\min }F(\mathbf{x})=\frac{1}{2}\Vert f(\mathbf{x}){\Vert }_2^2.$$

其中，自变量 $\mathbf{x}\in {\mathbb{R}}^n$，$f$ 是任意标量非线性函数 $f(\mathbf{x}):{\mathbb{R}}^n\to \mathbb{R}$。注意这里的系数 $\frac{1}{2}$ 是无关紧要的，有些文献上带有这个系数，有些文献则不带，它不会影响之后的结论。下面讨论如何求解这样一个优化问题。显然，如果 $f$ 是这个数学形式上很简单的函数，那么该问题可以用解析形式来求。令目标函数的导数为零，然后求解 $\mathbf{x}$ 的最优值，就和一个求二元函数的极值一样:
$$\frac{\mathrm{d}F}{\mathrm{d}\mathbf{x}}=0.$$

解此方程，就得到了导数为零处的极值。它们可能是极大、极小或鞍点处的值，只要逐个比较它们的函数值大小即可。但是，这个方程是否容易求解呢？这取决于 $f$ 导函数的形式。如果 $f$ 为简单的线性函数，那么这个问题就是简单的线性最小二乘问题，但是有些导函数可能形式复杂，使得该方程可能不容易求解。求解这个方程需要我们知道关于目标函数的全局性质，而通常这是不太可能的。对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤可列写如下:

1. 给定某个初始值 $x_0$ 。
2. 对于第 $k$ 次迭代，寻找一个增量 $\Delta {\mathbf{x}}_k$，使得 ${\Vert f\left({\mathbf{x}}_k+\Delta {\mathbf{x}}_k\right)\Vert }_2^2$ 达到极小值。
3. 若 $\Delta {\mathbf{x}}_k$ 足够小，则停止。
4. 否则，令 ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，返回第 $2$ 步。

这让求解导函数为零的问题，变成了一个不断寻找下降增量 $\Delta {\mathbf{x}}_k$ 的问题，我们将看到，由于可以对 $f$ 进行线性化，增量的计算将简单很多。当函数下降直到增量非常小的时候，就认为算法收敛，目标函数达到了一个极小值。在这个过程中，问题在于如何找到每次迭代点的增量，而这是一个局部的问题，我们只需关心 $f$ 在迭代值处的局部性质而非全局性质。这类方法在最优化、机器学习等领域应用非常广泛。

接下来，我们考察如何寻找这个增量 $\Delta {\mathbf{x}}_k$。这部分知识实际属于数值优化的领域，我们来看一些广泛使用的结果。

3.1 一阶和二阶梯度法

现在考虑第 $k$ 次迭代，假设我们在 ${\mathbf{x}}_{\mathbf{k}}$ 处，想要寻到增量 $\Delta {\mathbf{x}}_{\mathbf{k}}$，那么最直观的方式是将目标函数在 ${\mathbf{x}}_{\mathbf{k}}$ 附近进行泰勒展开：
$$F({\mathbf{x}}_k+\Delta {\mathbf{x}}_k)\approx F({\mathbf{x}}_k)+\mathbf{J}({\mathbf{x}}_k{)}^T\Delta {\mathbf{x}}_k+\frac{1}{2}\Delta {\mathbf{x}}_k^T\mathbf{H}({\mathbf{x}}_k)\Delta {\mathbf{x}}_k.$$

其中 $\mathbf{J}({\mathbf{x}}_k)$ 是 $F(\mathbf{x})$ 关于 $\mathbf{x}$ 的一阶导数(也叫梯度、雅可比(Jacobian)矩阵)，$\mathbf{H}$ 则是二阶导数(海塞 (Hessian) 矩阵)，它们都在 ${\mathbf{x}}_{\mathbf{k}}$ 处取值。我们可以选择保留泰勒展开的一阶或二阶项，那么对应的求解方法则称为一阶梯度或二阶梯度法。如果保留一阶梯度，那么取增量的方向为反向的梯度，即可保证函数下降：
$$\Delta {\mathbf{x}}^{\ast }=-{\mathbf{J}}^T({\mathbf{x}}_k).$$

当然这只是个方向，通常我们还要再指定一个步长 $\lambda$。步长可以根据一定的条件来计算，在机器学习中也有一定经验性质的方法，但我们不展开谈。这种方法被称为最速下降法。它的直观意义非常简单，只要我们沿着反向梯度方向前进，在一阶(线性)的近似下，目标函数必定会下降。

注意，以上讨论都是在第 $k$ 次迭代时进行的，并不涉及其他的迭代信息。所以为了简化符号，后面我们省略下标 $k$，并认为这些讨论对任意一次迭代都成立。

另外，我们可以选择保留二阶梯度信息，此时增量方程为:
$$\Delta {\mathbf{x}}^{\ast }=\arg \min (F(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}).$$

右侧只含 $\Delta \mathbf{x}$ 的的零次、一次和二次项。求右侧等式关于 $\Delta \mathbf{x}$ 的导数并令它为零，得到：
$$\mathbf{J}+\mathbf{H}\Delta \mathbf{x}=0\to \mathbf{H}\Delta \mathbf{x}=-\mathbf{J}.$$

求解这个线性方程，就得到了增量。该方法又称为牛顿法。

我们看到，一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量做最小化即可。事实上，我们用一个一次或二次的函数近似了原函数，然后用近似函数的最小值来猜测原函数的极小值。只要原目标函数局部看起来像一次或二次函数，这类算法就是成立的。不过，这两种方法也存在它们自身的问题。最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数(不是很好用的方法，容易走出之字型，收敛步数比较多。一阶得到的是一个一阶的近似，得到的是一个线性的。)。

而牛顿法则需要计算目标函数的 $\mathbf{H}$ 矩阵，这在问题规模较大时非常困难，我们通常倾向于避免 $H$ 的计算(二阶近似得到的是一个曲线的，所以不会像前面的‘之’字型这样，收敛的会更快。)。对于一般的问题，一些拟牛顿法可以得到较好的结果，而对于最小二乘问题，还有几类更实用的方法：高斯牛顿法和列文伯格---马夸尔特方法。

说白了，就是将 $||f(x+\Delta x)|{|}_2^2$ 在 $x$ 处二阶泰勒展开，得：
$$\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }_2^2\approx \Vert f(\mathbf{x}){\Vert }_2^2+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}(\mathbf{x})\Delta \mathbf{x}$$

其中，$\mathbf{J}$ 是 $\Vert f(\mathbf{x}){\Vert }_2^2$ 在 $x$ 的一阶导数(Jocabian矩阵)，$\mathbf{H}(\mathbf{x})$ 是二阶导数(Hessian矩阵)。

• 仅保留一阶项(最速下降法)：
  $\Delta {\mathbf{x}}^{\ast }=-\mathbf{J}(\mathbf{x}{)}^T$
  通常这个梯度会乘一个步长 $\lambda$ ，即 $\Delta {\mathbf{x}}^{\ast }=-\lambda \mathbf{J}(\mathbf{x}{)}^T$ 。

• 保留二阶项(牛顿法)：
  $\Delta x^{\ast }=\mathrm{argmin}\left(\Vert f(x){\Vert }_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H(x)\Delta x\right)$
  右侧对 $\Delta x$ 求导可得：
  $\mathbf{H}(\mathbf{x})\Delta \mathbf{x}=-\mathbf{J}(\mathbf{x}{)}^T$
  

3.2 高斯牛顿法

高斯牛顿法是最优化算法里面最简单的方法之一。它的思想是将 $f(\mathbf{x})$ 进行一阶的泰勒展开(比如说，${\min }_{x}F(\mathbf{x})=\frac{1}{2}\Vert f(\mathbf{x}){\Vert }_2^2$，牛顿法求的是在$F(\mathbf{x})$上的二阶泰勒展开，而高斯牛顿法不用去纠结$f(\mathbf{x}+\Delta \mathbf{x})$在$L_2$上的定义)，请注意这里不是目标函数 $F(\mathbf{x})$ 而是 $f(\mathbf{x})$，否则就变成牛顿法了：
$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}.$$

这里 $\mathbf{J}(\mathbf{x}{)}^T$ 为 $f(\mathbf{x})$ 关于 $\mathbf{x}$ 的导数(高斯法中 $\mathbf{J}({\mathbf{x}}_k)$ 是 $F(\mathbf{x})$ 关于 $\mathbf{x}$ 的一阶导数)，为 $n\times 1$ 的列向量。根据前面的框架，当前的目标是寻找增量 $\Delta \mathbf{x}$，使得 $\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2$ 达到最小。为了求 $\Delta \mathbf{x}$，我们需要解一个线性的最小二乘问题：
$$\Delta {\mathbf{x}}^{\ast }=\arg \underset{\Delta \mathbf{x}}{\min }\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}{\Vert }^2.$$

这个方程与之前有什么不一样呢? 根据极值条件，将上述目标函数对 $\Delta x$ 求导，并令导数为零。为此，先展开目标函数的平方项：
$$\begin{array}{rl}\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}{\Vert }^2& =\frac{1}{2}(f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}{)}^T(f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x})\\ & =\frac{1}{2}\left(\Vert f(\mathbf{x}){\Vert }_2^2+2f(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}+\Delta {\mathbf{x}}^T\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}\right).\end{array}$$

求上式关于 $\Delta \mathbf{x}$ 的导数，并令其为零:
$$\mathbf{J}(\mathbf{x})f(\mathbf{x})+\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}=0.$$

可以得到如下方程组:
$$\mathbf{J}(\mathbf{x}{)}^T\mathbf{J}(\mathbf{x})\Delta \mathbf{x}=-\mathbf{J}(\mathbf{x}{)}^{T}f(\mathbf{x})$$

这个方程是关于变量 $\Delta \mathbf{x}$ 的线性方程组，我们称它为增量方程，也可以称为高斯牛顿方程 (Gauss Newton equations) 或者正规方程 (Normal equations)。我们把左边的系数定义为 $\mathbf{H}$，右边定义为 $\mathbf{g}$，那么上式变为:
$$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}.$$

这里把左侧记作 $\mathbf{H}$ 是有意义的。对比牛顿法可见，Gauss-Newton 用 ${\mathbf{J}}^T\mathbf{J}$ 作为牛顿法中二阶 Hessian 矩阵的近似(注意是近似！)，从而省略了计算 $\mathbf{H}$ 的过程。求解增量方程是整个优化问题的核心所在。如果我们能够顺利解出该方程，那么 Gauss-Newton 的算法步骤可以写成：

1. 给定初始值 ${\mathbf{x}}_0$ 。
2. 对于第 $k$ 次迭代，求出当前的雅可比矩阵 $\mathbf{J}\left({\mathbf{x}}_k\right)$ 和误差 $f\left({\mathbf{x}}_k\right)$ 。
3. 求解增量方程: $\mathbf{H}\Delta {\mathbf{x}}_k=\mathbf{g}$.
4. 若 $\Delta {\mathbf{x}}_k$ 足够小，则停止。否则，令 ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，返回 2。

从算法步骤中可以看到，增量方程的求解占据着主要地位。只要我们能够顺利解出增量，就能保证目标函数能够正确地下降。

为了求解增量方程，我们需要求解 ${\mathbf{H}}^{-1}$，这需要 $\mathbf{H}$ 矩阵可逆，但实际数据中计算得到的 ${\mathbf{J}}^T\mathbf{J}$ 却只有半正定性。也就是说，在使用高斯牛顿法时，可能出现 ${\mathbf{J}}^T\mathbf{J}$ 为奇异矩阵或者病态 (ill-condition) 的情况，此时增量的稳定性较差，导致算法不收敛。直观地说，原函数在这个点的局部近似不像一个二次函数。更严重的是，就算我们假设 $\mathbf{H}$ 非奇异也非病态，如果我们求出来的步长 $\Delta \mathbf{x}$ 太大，也会导致我们采用的局部近似 $f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}$ 不够准确，这样一来我们甚至无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的。

尽管高斯牛顿法有这些缺点，但它依然算是非线性优化方面一种简单有效的方法，值得我们学习。在非线性优化领域，相当多的算法都可以归结为高斯牛顿法的变种。这些算法都借助了高斯牛顿法的思想并且通过自己的改进修正其缺点。例如一些线搜索方法 (Line Search Method)加入了一个步长 $\alpha$，在确定了 $\Delta \mathbf{x}$ 后进一步找到 $\alpha$ 使得 $\Vert f(\mathbf{x}+\alpha \mathbf{\Delta }\mathbf{x}){\Vert }^2$ 达到最小，而不是简单地令 $\alpha =1$。

列文伯格---马夸尔特方法在一定程度上修正了这些问题。一般认为它比高斯牛顿法更为健壮，它的收敛速度可能比高斯牛顿法更慢，被称之为阻尼牛顿法 (Damped Newton Method)。

3.3 列文伯格—马夸尔特方法

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自然地想到应该给 $\Delta \mathbf{x}$ 添加一个范围，称为信赖区域 (Trust Region)。这个范围定义了在什么情况下二阶近似是有效的，这类方法也被称为信赖区域方法 (Trust Region Method)。在信赖区域里，我们认为近似是有效的；出了这个区域，近似可能会出问题。

那么，如何确定这个信赖区域的范围呢? 一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定：如果差异小，说明近似效果好，我们扩大近似的范围；反之，如果差异大，就缩小近似的范围。我们定义一个指标 $\rho$ 来刻画近似的好坏程度：
$$\rho =\frac{f(\mathbf{x}+\Delta \mathbf{x})-f(\mathbf{x})}{\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}}.$$

$\rho$ 的分子是实际函数下降的值，分母是近似模型下降的值。如果 $\rho$ 接近于 $1$，则近似是好的。如果 $\rho$ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 $\rho$ 比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。
于是，我们构建一个改良版的非线性优化框架，该框架会比高斯牛顿法有更好的效果:

1. 给定初始值 ${\mathbf{x}}_0$，以及初始优化半径 $\mu$ 。

2. 对于第 $k$ 次迭代，在高斯牛顿法的基础上加上信赖区域，求解:
   $$\underset{\Delta {\mathbf{x}}_k}{\min }\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^T\Delta {\mathbf{x}}_k\Vert }^2,\text{ s.t. }{\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2\le \mu ,$$

   这里，$\mu$ 是信赖区域的半径，$D$ 为系数矩阵，将在后文中说明。

3. 按公式 $\rho =\frac{f(\mathbf{x}+\Delta \mathbf{x})-f(\mathbf{x})}{\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}}$ 计算 $\rho$ 。

4. 若 $\rho >\frac{3}{4}$，则 $\mu =2\mu$;

5. 若 $\rho <\frac{1}{4}$，则 $\mu =0.5\mu$;

6. 如果 $\rho$ 大于某阈值，则认为近似可行。令 ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$ 。

7. 判断算法是否收敛。如不收敛则返回 $2$，否则结束。

这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。在式 ${\min }_{\Delta {\mathbf{x}}_k}\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^T\Delta {\mathbf{x}}_k\Vert }^2,\text{ s.t. }{\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2\le \mu$ 中，我们把增量限定于一个半径为 $\mu$ 的球中，认为只在这个球内才是有效的。带上 $\mathbf{D}$ 之后，这个球可以看成一个椭球。在列文伯格提出的优化方法中，把 $\mathbf{D}$ 取成单位阵 $\mathbf{I}$，相当于直接把 $\Delta {\mathbf{x}}_k$ 约束在一个球中。随后，马尔夸特提出将 $\mathbf{D}$ 取成非负数对角阵一一实际中通常用 ${\mathbf{J}}^T\mathbf{J}$ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些。

不论如何，在列文伯格—马夸尔特方法优化中，我们都需要解式 ${\min }_{\Delta {\mathbf{x}}_k}\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^T\Delta {\mathbf{x}}_k\Vert }^2,\text{ s.t. }{\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2\le \mu$ 这样一个子问题来获得梯度。这个子问题是带不等式约束的优化问题，我们用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：
$$\mathcal{L}\left(\Delta {\mathbf{x}}_k,\lambda \right)=\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^{\mathrm{T}}\Delta {\mathbf{x}}_k\Vert }^2+\frac{\lambda }{2}\left({\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2-\mu \right).$$

这里 $\lambda$ 为拉格朗日乘子。类似于高斯牛顿法中的做法，令该拉格朗日函数关于 $\Delta \mathbf{x}$ 的导数为零，它的核心仍是计算增量的线性方程：
$$\left(\mathbf{H}+\lambda {\mathbf{D}}^{\mathrm{T}}\mathbf{D}\right)\Delta {\mathbf{x}}_k=\mathbf{g}.$$

可以看到，相比于高斯牛顿法，增量方程多了一项 $\lambda {\mathbf{D}}^T\mathbf{D}$ 。如果考虑它的简化形式，即 $D=I$，那么相当于求解：
$$(\mathbf{H}+\lambda \mathbf{I})\Delta {\mathbf{x}}_k=\mathbf{g}.$$

我们看到，一方面，当参数 $\lambda$ 比较小时，$\mathbf{H}$ 占主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格一马夸尔特方法更接近于高斯牛顿法。另一方面，当 $\lambda$ 比较大时，$\lambda \mathbf{I}$ 占据主要地位，列文伯格一马夸尔特方法更接近于一阶梯度下降法 (即最速下降)，这说明附近的二次近似不够好。列文伯格一马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量 $\Delta {\mathbf{x}}_0$。

在实际中，还存在许多其他的方式来求解增量，例如 Dog-Leg 等方法。我们在这里所介绍的，只是最常见而且最基本的方法，也是视觉 SLAM 中用得最多的方法。实际问题中，我们通常选择高斯牛顿法或列文伯格一马夸尔特方法中的一种作为梯度下降策略。当问题性质较好时，用高斯牛顿。如果问题接近病态，则用列文伯格一马夸尔特方法。

3.4 小结

这里只罗列了最常见的两种非线性优化方案——高斯牛顿法和列文伯格–马尔夸特方法。我们避开了许多数学性质上的讨论。如果读者对优化感兴趣，可以进一步阅读专门介绍数值优化的书籍（这是一个很大的课题)。以高斯牛顿法和列文伯格–马尔夸特方法为代表的优化方法，在很多开源的优化库都已经实现并提供给用户。最优化是处理许多实际问题的基本数学工具，不光在视觉 SLAM 起着核心作用，在类似于深度学习等其它领域，它也是求解问题的核心方法之一。我们希望读者能够根据自身能力, 去了解更多的最优化算法。

也许你发现了，无论是高斯牛顿法还是列文伯格–马尔夸特方法，在做最优化计算时，都需要提供变量的初始值。你也许会问，这个初始值能否随意设置? 当然不能。实际上非线性优化的所有迭代求解方案，都需要用户来提供一个良好的初始值。由于目标函数太复杂，导致在求解空间上的变化难以预测，对问题提供不同的初始值往往会导致不同的计算结果。这种情况是非线性优化的通病：大多数算法都容易陷入局部极小值。因此，无论是哪类科学问题，我们提供初始值都应该有科学依据，例如视觉 SLAM 问题中，我们会用 ICP、PnP 之类的算法提供优化初始值。总之，一个良好的初始值对最优化问题非常重要！

也许读者还会对上面提到的最优化产生疑问：如何求解线性增量方程组呢? 我们只讲到了增量方程是一个线性方程，但是直接对系数矩阵进行求逆岂不是要进行大量的计算? 当然不是。在视觉 SLAM 算法里，经常遇到 $\Delta \mathbf{x}$ 的维度大到好几百或者上千，如果你是要做大规模的视觉三维重建，就会经常发现这个维度可以轻易达到几十万甚至更高的级别。要对那么大个矩阵进行求逆是大多数处理器无法负担的，因此存在着许多针对线性方程组的数值求解方法。在不同的领域有不同的求解方式，但几乎没有一种方式是直接求系数矩阵的逆，我们会采用矩阵分解的方法来解线性方程，例如 $\mathrm{QR}$ 、Cholesky 等分解方法。这些方法通常在矩阵论等教科书中可以找到，我们不多加介绍。

幸运的是，视觉 SLAM 里，这个矩阵往往有特定的稀疏形式，这为实时求解优化问题提供了可能性。利用稀疏形式的消元、分解，最后再进行求解增量，会让求解的效率大大提高。在很多开源的优化库上，维度为一万多的变量在一般的 PC 上就可以在几秒甚至更短的时间内就被求解出来，其原因也是用了更加高级的数学工具。视觉 SLAM 算法现在能够实时地实现，也是多亏了系数矩阵是稀疏的，如果矩阵是稠密的，恐怕优化这类视觉 SLAM 算法就不会被学界广泛采纳了。