VSLAM学习(二) 非线性优化

目录
VSLAM学习(一) 三维运动、相机模型、SLAM模型
VSLAM学习(二) 非线性优化
VSLAM学习(三) 单目相机位姿估计
VSLAM学习(四) Bundle Adjustment

一、前置知识点

1.1 最小二乘项

最小二乘是一种求参数优化损失的方法，不了解的可以去看我的另一篇博文：【线性回归与最小二乘】

例如，
多维高斯分布概率密度: $\mathbf{x}\sim N(\mathbf{\mu },\mathbf{\Sigma })$，其中$\mathbf{x},\mathbf{\mu }\in {\mathbb{R}}^n,\mathbf{\Sigma }\in {\mathbb{R}}^{n\times n}$

$$f(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^{n}det(\mathbf{\Sigma })}}\mathrm{e}\mathrm{x}\mathrm{p}(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu }))$$

在优化中一般使用其负对数形式：

$$-ln[f(\mathbf{x})]=\mathbf{C}+\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })$$
其中二次型$\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })$称为马氏距离，就是一种最小二乘项。
要求$f(\mathbf{x})$的最大值，即是要求马氏距离的最小值。

1.2 牛顿迭代法

牛顿法是一种在实数域和复数域上近似求解方程的方法，原理大致如下

以最简单的一元函数为例，若要求一元方程$f(x)=0$的解，可先随机选取一个初值值$x_0$，但显然$f(x_0)\ne 0$，那么我们对函数$f(x)$关于$x_0$进行一阶泰勒展开：

$$f(x_1)\approx f(x_0)+f^{\prime }(x_0)(x_1-x_0)$$

我们希望找到$f(x_1)=0$，即

$$f(x_0)+f^{\prime }(x_0)(x_1-x_0)=0$$
所以有
$$x_1=x_0-\frac{f(x_0)}{f^{\prime }(x_0)}$$
此为一轮迭代
之后可以重复以上方法往下找$x_2,x_3,\cdot \cdot \cdot$，直到误差允许的范围内为止
过程大致如下图所示

牛顿迭代法 我的思考:

假如所求方程为$x^2-1=0$，那么此时的迭代公式为

$$x_1=x_0-\frac{f(x_0)}{f^{\prime }(x_0)}=x_0-\frac{x_0^2-1}{2x_0}=\frac{x_0}{2}+\frac{1}{2x_0}$$

但若我取初值$x_0=0$，会导迭代时$x_1\to \infty$，这样牛顿法是不是就失效了？

二、状态估计问题

回顾滤波器中的贝叶斯法则：

$$P(x│z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x)$$

现在目标是要去求解状态变量(这里将待求的位姿信息和路标信息全部整合到一个变量中) x = { x 1 , ⋅ ⋅ ⋅ , x n , y 1 , ⋅ ⋅ ⋅ , y m } \boldsymbol x={\{x_1,···,x_n,y_1,···,y_m\}} x={x1​,⋅⋅⋅,xn​,y1​,⋅⋅⋅,ym​}的值

由于$P(x│z)$比较难直接表示，所以要借助两种办法

1. 最大后验估计 (Maximize a Posterior)

$${\mathbf{x}}_{MAP}^{\ast }=\arg \max P(x|z)=\arg \max P(z|x)P(x)$$
2) 最大似然估计 (Maximize Likelhood Estimation)

$${\mathbf{x}}_{MLE}^{\ast }=\arg \max P(z|x)$$

来求出$\mathbf{x}$的状况

先以最简单的形式来举例，假设噪声都是高斯噪声：
$${\mathbf{v}}_{k,j}\sim N(0,{\mathbf{Q}}_{k,j})$$
根据观测方程
$${\mathbf{z}}_{k,j}=h({\mathbf{y}}_j,{\mathbf{x}}_k)+{\mathbf{v}}_{k,j}$$
于是
$$\begin{gathered} \mathbf{z}-h(\mathbf{x})\sim N(0,\mathbf{Q}) \\ or \\ \mathbf{z}\sim N(h(\mathbf{x}),\mathbf{Q}) \\ or \\ \mathrm{P}(\mathbf{z}|\mathbf{x})=N(h(\mathbf{x}),\mathbf{Q}) \end{gathered}$$

求解最大似然估计：

$$\begin{array}{rl}{\mathbf{x}}^{\ast }& =\arg \max P(z|x)\\ & =\arg \min \left(\right(\mathbf{z}-h(\mathbf{x}){)}^T{\mathbf{Q}}^{-1}(\mathbf{z}-h(\mathbf{x})))\\ & =\arg \min ({\mathbf{v}}^T{\mathbf{Q}}^{-1}\mathbf{v})\end{array}$$
说明求后验概率最大化等价于求误差的最小二乘

对于slam模型的两个方程的噪声项(之后记为误差项)

$$\begin{array}{rl}{\mathbf{e}}_{\mathbf{u},k}& ={\mathbf{x}}_k-f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)\\ {\mathbf{e}}_{\mathbf{z},k,j}& ={\mathbf{z}}_{k,j}-h({\mathbf{y}}_j,{\mathbf{x}}_k)\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}}_{\mathbf{z},k,j}^T{\mathbf{Q}}_{k,j}^{-1}{\mathbf{e}}_{\mathbf{z},k,j}$$

三、非线性最小二乘

对于求解最小二乘问题的极值相当于也是利用迭代的思想去逼近最优解

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

3.1 一阶梯度法（最速下降法）

将目标函数$F(\mathbf{x})$在$x_k$附近作一阶泰勒展开

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

其中$\mathbf{J}$为Jacobian矩阵

$$\begin{array}{rl}\mathbf{J}(\mathbf{x})& ={\mathrm{D}}_{\mathbf{x}}F(\mathbf{x})={\nabla }_{\mathbf{x}}^{\mathrm{T}}F(\mathbf{x})=\frac{\partial F(\mathbf{x})}{\partial {\mathbf{x}}^{\mathrm{T}}}\\ & =(\frac{\partial F(\mathbf{x})}{\partial x_1},\frac{\partial F(\mathbf{x})}{\partial x_2},\cdot \cdot \cdot ,\frac{\partial F(\mathbf{x})}{\partial x_n})\end{array}$$

取梯度反方向，做梯度下降

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

这只是确定了方向，一般情况下，还需确定一个步长
记步长为$\alpha \ge 0$，根据迭代

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\alpha \Delta {\mathbf{x}}^{\ast }$$

所以目标就是求

$$\underset{\alpha }{\min }F({\mathbf{x}}_{k+1})=\underset{\alpha }{\min }F({\mathbf{x}}_k+\alpha \Delta {\mathbf{x}}^{\ast })$$

可以直接求驻点来解得$\alpha$

$$\begin{gathered} \phi (\alpha )=F({\mathbf{x}}_k+\alpha \Delta {\mathbf{x}}^{\ast }) \\ {\phi }^{\prime }(\alpha )=0 \end{gathered}$$

方法特点：
贪心算法，两次寻路方向正交，产生ZigZag现象（走锯齿路线），导致迭代次数增加

3.2 二阶梯度法（牛顿法）

将目标函数$F(\mathbf{x})$在$x$附近作二阶泰勒展开

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

其中$\mathbf{H}$为Hessian矩阵

$$\begin{array}{rl}\mathbf{H}(\mathbf{x})& ={\nabla }_{\mathbf{x}}^{2}F(\mathbf{x})==\frac{{\partial }^{2}F(\mathbf{x})}{\partial \mathbf{x}\partial {\mathbf{x}}^{\mathrm{T}}}\\ & =\left(\begin{array}{ccc}\frac{{\partial }^{2}F(\mathbf{x})}{\partial x_1^2}& \dots & \frac{{\partial }^{2}F(\mathbf{x})}{\partial x_1\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{{\partial }^{2}F(\mathbf{x})}{\partial x_n\partial x_1}& \dots & \frac{{\partial }^{2}F(\mathbf{x})}{\partial x_n^2}\end{array}\right)\end{array}$$

要求函数$F(\mathbf{x})$的极值，也就是求驻点，即$F^{\prime }(\mathbf{x})=0$的点
将上面的二阶泰勒展开式对$\Delta \mathbf{x}$求导，得

$$F^{\prime }(\mathbf{x}+\Delta \mathbf{x})\approx {\mathbf{J}}^{\mathrm{T}}(\mathbf{x})+\mathbf{H}(\mathbf{x})\Delta \mathbf{x}$$

• 注意对$\Delta \mathbf{x}$求导的意思，也就是$\mathbf{x}$是常数。

令一阶导数等于0，得

$$\mathbf{H}\Delta \mathbf{x}=-{\mathbf{J}}^{\mathrm{T}}$$

若$H$可逆，则可得到牛顿迭代公式为

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k-{\mathbf{H}}^{-1}{\mathbf{J}}^{\mathrm{T}}$$

• 额外补充知识点
  　对于一阶梯度$\mathbf{J}=0$的点，
  　若Hessian矩阵$\mathbf{H}$是正定的(所有特征值都是正的)，则该临界点是局部极小点
  　若Hessian矩阵$\mathbf{H}$是负定的(所有特征值都是负的)，则该临界点是局部极大点
  　若Hessian矩阵$\mathbf{H}$是不定的(特征值有正有负)，则该临界点是鞍点

方法缺点：
求解Hessian矩阵$\mathbf{H}$过于复杂

3.3 高斯-牛顿法

牛顿法是直接对二范数$\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }_2^2$进行泰勒展开
而高斯-牛顿法是先对函数作泰勒展开，之后再求二范数

$$\begin{gathered} f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x} \\ \Delta {\mathbf{x}}^{\ast }=\arg \underset{\Delta \mathbf{x}}{\min }\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}{\Vert }^2 \end{gathered}$$

展开待优化式：

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

求导求驻点

$$\begin{gathered} {\mathbf{J}}^{\mathrm{T}}f+{\mathbf{J}}^{\mathrm{T}}\mathbf{J}\Delta \mathbf{x}=0 \\ {\mathbf{J}}^{\mathrm{T}}\mathbf{J}\Delta \mathbf{x}=-\mathbf{J}f \end{gathered}$$

记${\mathbf{J}}^{\mathrm{T}}\mathbf{J}=\mathbf{H},-{\mathbf{J}}^{\mathrm{T}}f=\mathbf{g}$，即得到增量方程

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

计算${\mathbf{J}}^{\mathrm{T}}\mathbf{J}$比牛顿法计算Hessian矩阵要容易的多，但是${\mathbf{J}}^{\mathrm{T}}\mathbf{J}$是半正定的，不一定可逆

3.4 Levenberg-Marquardt方法

上面两种优化都属于线性搜索(Line Search)方式
而这里的LM方法属于置信区域(Trust Region)模式

记

$$\rho =\frac{f(\mathbf{x}+\Delta \mathbf{x})-f(\mathbf{x})}{\mathbf{J}(x)\Delta \mathbf{x}}$$

即实际下降与一阶近似下降的比值

• 比值越大，说明一阶近似越可靠，还可加大近似范围
• 比值越小，说明一阶近似越不可靠，需要减小近似范围

LM方法确定迭代增量的时候，将增量约束与此范围之内：
$$\underset{\Delta \mathbf{x}}{\min }\frac{1}{2}\Vert f(\mathbf{x}+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}){\Vert }^2,s.t.\Vert \mathbf{D}\Delta \mathbf{x}{\Vert }^2\le \mu$$
其中$\mu$是信赖区域半径（初始随机取值），$\mathbf{D}$是区域形状

确定增量前，先要计算$\rho$

• 若$\rho >\frac{3}{4}$，则放大范围，$\mu =2\mu$
• 若$\rho <\frac{1}{4}$，则缩小范围，$\mu =\frac{1}{2}\mu$

对于区域形状$\mathbf{D}$，Levenberg和Marquardt提出的有所不同

• Levenberg提出$\mathbf{D}=\mathbf{I}$，相当于把$\Delta \mathbf{x}$约束在一个球中
• Marquardt提出$\mathbf{D}$取成非负对角阵，通常使用${\mathbf{J}}^{\mathrm{T}}\mathbf{J}$对角元素的平方根，相当于把$\Delta \mathbf{x}$约束在一个椭球中

所以整个问题相当于求解一个条件极值
可以使用拉格朗日乘数法

$$\underset{\Delta \mathbf{x}}{\min }\frac{1}{2}(\Vert f(\mathbf{x}+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}){\Vert }^2+\lambda \Vert \mathbf{D}\Delta \mathbf{x}{\Vert }^2)$$

还是求解增量方程
有

$$\begin{gathered} ({\mathbf{J}}^{\mathrm{T}}\mathbf{J}+\lambda {\mathbf{D}}^{\mathrm{T}}\mathbf{D})\Delta \mathbf{x}=-\mathbf{J}f \\ or \\ (\mathbf{H}+\lambda {\mathbf{D}}^{\mathrm{T}}\mathbf{D})\Delta \mathbf{x}=\mathbf{g} \end{gathered}$$

可以看出，就是比G-N方法多了一个$\lambda {\mathbf{D}}^{\mathrm{T}}\mathbf{D}$

以Levenberg为例($\mathbf{D}=\mathbf{I}$)
增量方程即为

$$(\mathbf{H}+\lambda \mathbf{I})\Delta \mathbf{x}=\mathbf{g}$$
这相当于给矩阵$\mathbf{H}$增加了它的正定性，并且：

• 当$\lambda$很小的时候，$\mathbf{H}$占主体地位，LM方法接近高斯牛顿法
• 当$\lambda$很大的时候，$\lambda \mathbf{I}$占主体地位，LM方法接近于最速下降法

所以可以将LM方法看做是一阶法与二阶法的一个混合折中