《视觉SLAM十四讲》-- 非线性优化

05 非线性优化

5.1 批量状态估计与最大后验估计

（1）两种状态估计方法：

• 增量/渐进式（滤波器）：数据是随时间逐渐到来的；

• 批量式：一次给定所有的数据，估计所有的变量。

（2）经典的 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}\text{\hspace{1em}(5-1)}$$

定义所有时刻的机器人位姿和路标点坐标

x = { x 1 , … , x N } , y = { y 1 , … , y M } \boldsymbol{x}=\left\{\boldsymbol{x}_{1}, \ldots, \boldsymbol{x}_{N}\right\}, \quad \boldsymbol{y}=\left\{\boldsymbol{y}_{1}, \ldots, \boldsymbol{y}_{M}\right\} x={x1​,…,xN​},y={y1​,…,yM​}

用 $\mathbf{u}$ 表示所有时刻的输入， $\mathbf{z}$ 表示所有时刻的观测数据。在已知输入和观测数据的条件下，求机器人状态 $\mathbf{x}$ 和 $\mathbf{y}$ 的概率分布，即

$$P(\mathbf{x},\mathbf{y}|\mathbf{u},\mathbf{z})$$

根据贝叶斯法则，

$$\underset{\text{后验 }}{\underbrace{P(\mathbf{x},\mathbf{y}\mid \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 \underset{\text{似然 }}{\underbrace{P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})}}\underset{\text{先验 }}{\underbrace{P(\mathbf{x},\mathbf{y})}}\text{\hspace{1em}(5-2)}$$

那么，求解最大后验概率等价于最大化似然概率和先验概率的乘积。但是，有时我们并不知道机器人位姿或路标大概在什么地方，也就不知道先验概率，上式就简化为求解最大似然概率

$$(\mathbf{x},\mathbf{y}{)}_{MLE}^{\ast }=\mathrm{argmax}P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})$$

直观地讲，似然是指“在现在的位姿下，可能产生怎样的观测数据”，但由于观测数据已知，则上式可理解为在什么样的状态下，最可能产生观测到的数据。

5.2 最小二乘的引出

（1）在高斯分布的假设下，最大似然有较简单的形式。对于某一次观测

$${\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)$，所以观测数据的条件概率为

$$P({\mathbf{z}}_{k,j}|{\mathbf{y}}_j,{\mathbf{x}}_k)=N(h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right),{\mathbf{Q}}_{k,j})$$

依然满足高斯分布。

（2）对于任意维高斯分布 $\mathbf{x}\sim \mathcal{N}\left(\mathbf{\mu },\mathbf{\Sigma }\right)$，则其概率密度函数为

$$P(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp (-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\Sigma }^{-1}(\mathbf{x}-\mathbf{\mu }))\text{\hspace{1em}(5-3)}$$

其中，$N$ 为向量 $\mathbf{x}$ 的维度； $\det (\mathbf{\Sigma })$ 表示求 $\mathbf{\Sigma }$ 的行列式。

取其负对数，则变为

$$-\ln (P(\mathbf{x}))=\frac{1}{2}\ln ((2\pi {)}^N\det (\mathbf{\Sigma }))+\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\Sigma }^{-1}(\mathbf{x}-\mathbf{\mu })\text{\hspace{1em}(5-4)}$$

对原函数求最大化相当于对负对数求最小化。显然与第一项无关，故只需考虑最小化右侧的二次型项，则

$$\begin{array}{rl}{\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)}^{\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)}^{\mathrm{T}}{\mathbf{Q}}_{k,j}^{-1}\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)\right)\end{array}\text{\hspace{1em}(5-5)}$$

机器人状态 $\mathbf{x}$、 $\mathbf{y}$ 等于使二次型项最小时的值。

（3）假设各个时刻的输入和观测都是相互独立的，那么，我们对似然概率进行因式分解，得

$$P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})=\prod _{k}P\left({\mathbf{u}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{x}}_k\right)\prod _{k,j}P\left({\mathbf{z}}_{k,j}\mid {\mathbf{x}}_k,{\mathbf{y}}_j\right)\text{\hspace{1em}(5-6)}$$

定义真实输入、观测数据与预测输入、观测数据之间的误差

$${{\mathbf{e}}_{\mathbf{u}}}_{,k}={\mathbf{x}}_k-f({\mathbf{u}}_k,{\mathbf{x}}_{k-1})$$
$${{\mathbf{e}}_{\mathbf{z}}}_{,j,k}={\mathbf{z}}_{k,j}-h({\mathbf{x}}_k,{\mathbf{y}}_j)\text{\hspace{1em}(5-7)}$$

那么，式（5-5）可以转化为一个最小二乘问题，即

$$\min J(\mathbf{x},\mathbf{y})=\sum _k{e}_{\mathbf{u},k}^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{e}}_{\mathbf{u},k}+\sum _k\sum _j{\mathbf{e}}_{\mathbf{z},k,j}^{\mathrm{T}}{\mathbf{Q}}_{k,j}^{-1}{\mathbf{e}}_{\mathbf{z},k,j}\text{\hspace{1em}(5-8)}$$

5.3 非线性最小二乘

非线性函数常常难以用求导法得到最值，而采用梯度下降法，即，通过不断迭代 ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，使得 $||f({\mathbf{x}}_k+\Delta {\mathbf{x}}_k)|{|}^2$ 达到极小值
，当 $\Delta \mathbf{x}$ 足够小时，即停止。关键在于增量 $\Delta \mathbf{x}$ 的选取。

过程如下：

——————————————————————————————————————————————————————

• 给定某个初值 ${\mathbf{x}}_0$；

• 对于第 $k$ 次迭代，寻找增量 $\Delta {\mathbf{x}}_k$；

• 当 $\Delta {\mathbf{x}}_k$ 足够小时，即停止；否则，重复第二步，继续寻找。

——————————————————————————————————————————————————————

下面介绍几个常用的优化方法。

5.3.1 一阶和二阶梯度法

考虑最小二乘问题：

$$\underset{x}{\min }F(x)=\frac{1}{2}\Vert f(x){\Vert }_2^2$$

注意 $F(\mathbf{x})$ 和 $f(\mathbf{x})$ 的区别（可以理解为 $f(\mathbf{x})$为残差矩阵）。

参考：https://blog.csdn.net/u014709760/article/details/97576395

对于非线性函数 $F(\mathbf{x})$，考虑第 $k$ 次迭代，将其在 ${\mathbf{x}}_k$ 附近泰勒展开（将 $\Delta {\mathbf{x}}_k$ 看做未知数），

$$F(\mathbf{x})=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{)}^T\Delta {\mathbf{x}}_k\text{\hspace{1em}(5-9)}$$

其中，$\mathbf{J}({\mathbf{x}}_k)$ 是 $F(\mathbf{x})$ 关于 $\mathbf{x}$ 的一阶导数，$\mathbf{H}({\mathbf{x}}_k)$ 是二阶导数。

（1）仅保留一阶导数时，取增量为反向梯度即可，即

$$\Delta {\mathbf{x}}^{\ast }=-\mathbf{J}({\mathbf{x}}_k)$$

也被称为最速下降法。

（2）保留二阶梯度信息。此时增量方程为

$$\Delta {\mathbf{x}}^{\ast }=\arg \min \left(F(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^{\mathrm{T}}\mathbf{H}\Delta \mathbf{x}\right)\text{\hspace{1em}(5-10)}$$

右侧各项分别为 $\Delta \mathbf{x}$ 的零次、一次和二次项，将其对 $\Delta \mathbf{x}$ 求导，并令其为零

$$\mathbf{J}+\mathbf{H}\Delta \mathbf{x}=0\text{\hspace{1em}(5-11)}$$

求解这个方程，即得到增量。此方法也被称为牛顿法。但在实际中，$\mathbf{H}$ 矩阵计算较为困难。

5.3.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}\text{\hspace{1em}(5-12)}$$
其中，$\mathbf{J}(\mathbf{x})$ 是 $f(\mathbf{x})$ 关于 $\mathbf{x}$ 的一阶导数，为 $n\times 1$ 列向量。

此时，我们的问题变为找到 $\Delta \mathbf{x}$ ，使得
$\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}{\Vert }^2$ 最小。将其展开

$$\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}(\Vert f(\mathbf{x}{\Vert }^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})\end{array}\text{\hspace{1em}(5-13)}$$

将 $\Delta \mathbf{x}$ 看做未知数，对其求导，并令其为零：

$$f(\mathbf{x})\mathbf{J}(\mathbf{x})+\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}=0\text{\hspace{1em}(5-14)}$$

得

$$\underset{\mathbf{H}(\mathbf{x})}{\underbrace{\mathbf{J}(\mathbf{x}){\mathbf{J}}^{\mathrm{T}}}}(\mathbf{x})\Delta \mathbf{x}=\underset{\mathbf{g}(\mathbf{x})}{\underbrace{-\mathbf{J}(\mathbf{x})f(\mathbf{x})}}\text{\hspace{1em}(5-15)}$$

即

$$\mathbf{H}(\mathbf{x})\Delta \mathbf{x}=\mathbf{g}(\mathbf{x})\text{\hspace{1em}(5-16)}$$

5.3.3 列文伯格-马夸尔特方法

以上方法都是用一阶或二阶泰勒展开式近似替代原函数，不可避免存在精度问题，因此我们定义一个指标 $\rho$ 来描述近似的好坏程度：

$$\rho =\frac{f(\mathbf{x}+\mathbf{\Delta }\mathbf{x})-f(\mathbf{x})}{\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}}\text{\hspace{1em}(5-17)}$$

其中，分子是实际下降的值，分母是近似下降的值。若 $\rho$ 接近于 1 ，则近似效果好；若 $\rho$ 太小，则说明实际减小的值远小于近似减小的值，即近似效果较差，需缩小近似范围； 若 $\rho$ 太大，则说明实际减小的值大于近似减小的值，则需放大近似范围。

基于此，提出一个改进的高斯牛顿法：

——————————————————————————————————————————————————————

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

• 对于第 $k$ 次迭代，在高斯牛顿法的基础上加上信赖区域，求解：

$$\underset{\Delta {\mathbf{x}}_k}{\min }\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,\text{ s.t. }{\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2⩽\mu \text{\hspace{1em}(5-18)}$$

其中 $\mu$ 是信赖区域的半径，$\mathbf{D}$ 是系数矩阵。

• 计算 $\rho$；

• 若 $\rho >\frac{3}{4}$（需放大近似范围），则设置 $\mu =2\mu$；若 $\rho <\frac{1}{4}$（需缩小近似范围），则设置 $\mu =0.5\mu$；

• 若 $\rho$ 在范围内，则认为近似效果好，令 ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$；

• 若 $\Delta {\mathbf{x}}_k$ 足够小，则停止；否则返回第二步。

——————————————————————————————————————————————————————

对于式（5-18），构造拉格朗日函数

$$\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)\text{\hspace{1em}(5-19)}$$

对 $\Delta \mathbf{x}$ 求导，并令等于零，得

$$(\mathbf{H}+\lambda {\mathbf{D}}^T\mathbf{D})\Delta \mathbf{x}=\mathbf{g}\text{\hspace{1em}(5-20)}$$

这里的 $\mathbf{H}$ 、 $\mathbf{g}$ 与高斯牛顿法中的$\mathbf{H}$ 、 $\mathbf{g}$含义相同。

若将 $\mathbf{D}$ 简化的看做为 $\mathbf{I}$，则上式变为

$$(\mathbf{H}+\lambda \mathbf{I})\Delta \mathbf{x}=\mathbf{g}\text{\hspace{1em}(5-21)}$$

可以看出，当 $\lambda$ 较小时，$\mathbf{H}$ 占主导地位，说明该范围内二次近似模型表现较好，此时，该方法更接近于高斯牛顿法；当 $\lambda$ 较大时，$\lambda \mathbf{I}$ 占主导地位，此时，该方法更接近于一阶梯度法。

5.4 实践：曲线拟合问题