非线性优化

引入

一个经典的SLAM模型由一个运动方程和一个观测方程构成

如下式所示：$\pmb{x}_k=f(\pmb{x}_{k-1},\pmb{u}_k)+\pmb{w}_k\\\pmb{z}_{k,j}=h(\pmb{y}_j,\pmb{x}_k)+\pmb{v}_{k,j}$其中$\pmb{u}_k$是运动传感器的读数或者输入；$\pmb{w}_k$是该过程中加入的噪声；$f$是一般函数，用来描述这个过程的；$\pmb{x}$是相机的位姿，可用SE(3)来描述；$\pmb{y}$是路标点；$\pmb{z}$是观测数据；$\pmb{v}_{k,j}$是这次观测里的噪声。由于观测所用的传感器形式很多，这里的观测数据$\pmb{z}$和观测方程$h$也有很多形式。
现假设观测方程由针孔模型给定，在$\pmb{x}_k$处对路标$\pmb{y}_j$进行了一次观测，对应到图像上的像素位置为$\pmb{z}_{k,j}$，则观测方程可以表示为：$s\pmb{z}_{k,j}=\pmb{K}(\pmb{R}_k\pmb{y}_j+\pmb{t}_K)$其中$\pmb{K}$为相机内参，$s$为像素点距离，也是$(\pmb{R}_k\pmb{y}_j+\pmb{t}_k)$的第三个分量，若用变换矩阵$\pmb{T}_k$来描述位姿，则路标$\pmb{y}_j$要用齐次坐标。

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

简单回顾下相机内参的相关知识\
相机内参数是与相机自身特性相关的参数，比如相机的焦距、像素大小等，这些在相机出厂后是固定的。由于镜头的安装精度，形状，传感器上的像素等因素，使镜头的光轴不再穿过图像的正中间。内参矩阵一般是如下形式：$\begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0&0&1 \end{pmatrix}$其中$f_x,f_y$表示$u,v$轴的缩放，$c_x,c_y$表示原点平移的距离。

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

要知道数据在受到噪声影响后会发生什么变化，通常会假设两个噪声项$\pmb{w}_k,\pmb{v}_{k,j}$，使其满足零均值的高斯分布，即：$\pmb{w}_k\sim N(0,\pmb{R}_k)\quad \pmb{v}_k\sim N(0,\pmb{Q}_{k,j})$其中$\pmb{R}_k,\pmb{Q}_{k,j}$为协方差矩阵。
通过带噪声的数据$\pmb{z}$和$\pmb{u}$来推断位姿$\pmb{x}$和地图$\pmb{y}$（及其概率分布），构成一个状态估计问题，即：$P(\pmb{x},\pmb{y}|\pmb{z},\pmb{u})$用贝叶斯法则为：$P(\pmb{x},\pmb{y}|\pmb{z},\pmb{u})=\frac{P(\pmb{z},\pmb{u}|\pmb{x},\pmb{y})P(\pmb{x},\pmb{y})}{P(\pmb{z},\pmb{u})}\varpropto P(\pmb{z},\pmb{u}|\pmb{x},\pmb{y})P(\pmb{x},\pmb{y})$这就是一个求MAP的问题：$(\pmb{x},\pmb{y})^*_{MAP}=arg\max P(\pmb{x},\pmb{y}|\pmb{z},\pmb{u}) = arg\max P(\pmb{z},\pmb{u}|\pmb{x},\pmb{y})P(\pmb{x},\pmb{y})$如果我们不知道先验的话，就变成了求MLE的问题，即：$(\pmb{x},\pmb{y})^*_{MLE}=arg\max P(\pmb{z},\pmb{u}|\pmb{x},\pmb{y})$根据上面，我们假设了噪声项为零均值的高斯分布，所以观测数据的条件概率为：$P(\pmb{z}_{j,k}|\pmb{x}_k,\pmb{y}_j)=N(h(\pmb{y}_j,\pmb{x}_k),\pmb{Q}_{k,j})$(前提：观测是高斯分布的)
然后就是常见的展开取负对数，求最大变成求最小：$-\ln(P(\pmb{x}))=\frac{1}{2}\ln((2\pi)^N\det(\Sigma))+\frac{1}{2}(\pmb{x}-\pmb{\mu})^T\Sigma^{-1}(\pmb{x}-\pmb{\mu})$右边第一项与$\pmb{x}$无关，就不用管了，只关注第二项那个二次型。
将SLAM的观测模型带入得：

$$\begin{aligned} (\pmb{x}_k,\pmb{y}_j)^* &= arg\max N(h(\pmb{y}_j,\pmb{x}_k),\pmb{Q}_{k,j})\\ &= arg\min ((\pmb{z}_{k,j}-h(\pmb{x}_k,\pmb{y}_j))^T\pmb{Q}^{-1}_{k,j}(\pmb{z}_{k,j}-h(\pmb{x}_k,\pmb{y}_j))) \end{aligned}$$

在处理批量数据时我们通常是假设各个时刻的输入$\pmb{u}$和观测$\pmb{z}$相互独立，即是有：

$$P(z,u|x,y)=\prod_k P(u_k|x_{k-1},x_k)\prod_{k,j}P(z_{k,j}|x_k,y_j)$$然后我们定义相关误差，运动误差为：$$e_{u,k}=x_k-f(x_{k-1},u_k)$$观测误差为：$$e_{z,j,k}=z_{k,j}-h(x_k,y_j)$$用跟上面同样的方法也能写出$P(u_k|x_{k-1},x_{k})$的最小二乘表达式。现在我们要让这两个误差最小，使用误差平方和定义一个cost function，如下：$$\min J(x,y)=\sum_ke^T_{u,k}R^{-1}_ke_{u,k}+\sum_k\sum_je^T_{z,k,j}Q^{-1}_{k,j}e_{z,k,j}$$这就是我们要处理的最小二乘问题，其实就是两个MLE结合。这个明显是非线性的最小二乘问题，没法直接计算，现介绍以下几种迭代方法。 # 非线性最小二乘问题 现讨论最广泛的情况，即以$f(x)$作为二乘项，我们要解决以下这个问题：$$\min_{x}\frac{1}{2}||f(x)||^2 \quad x \in R^n $$当函数$f$很简单时，直接求导等于0，比较各个极值点即可；但当$f$很复杂，求导不容易计算时，可用迭代的方法。 迭代的基本思想是： > 1. 给定初值$x_0$；\ > 2. 对于第$k$次迭代，寻找一个增量$\Delta x_k$，使得$||f(x_k+\Delta x_k)||_2^2$达到极小值；\ > 3. 若$\Delta x_k$足够小，则停止；\ > 4. 否则，令$x_{k+1}=x_k+\Delta x_k$，返回步骤2。 现在的问题是如何确定这个增量$\Delta x_k$。 ## 一阶/ 二阶梯度法 这就是常见的Gradient Descent。 首先根据泰勒展开（对平方展开）可写为：$$||f(x+\Delta x)||^2_2 \approx ||f(x)||_2^2 + J(x)\Delta x+\frac{1}{2}\Delta x^TH\Delta x$$其中$J,H$分别为雅可比和海森矩阵。 ### 一阶（最速下降法，Steepest Method） 即只保留一阶梯度：$$\min_{\Delta x}||f(x)||_2^2+J\Delta x$$增量方向为$\Delta x^*=-J^T(x)$，通常还要计算步长。 #### 二阶（牛顿法） 即同时保留一阶和二阶：$$\Delta x^8=arg\min ||f(x)||_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^TH\Delta x$$令上式关于$\Delta x$的导数为0则有：$$H\Delta x=-J^T$$

方法分析（优缺点）

最速下降和牛顿虽然直观，但两者均有一些缺点：

1. 最速下降法由于过于贪婪会碰到zigzag问题，如下图：
   
   每次迭代均是往梯度下降最快的方向进行，对于简单的问题依旧需要很多次迭代才能达到最优点。
2. 牛顿法由于是二阶的，可以沿着曲线直接到最优点，但却需要计算复杂的海森矩阵。

所以这两种方法都不太实用，我们希望可以在不计算海森矩阵的前提下实现二阶梯度，就有了以下两种方法

Gauss-Newton

一阶近似$f(x)$为：$f(x+\Delta x) \approx f(x)+J(x)\Delta x$其平方误差为：$$
\begin{aligned}
\frac{1}{2}||f(x)+J(x)\Delta x||^2 &=\frac{1}{2}(f(x)+J(x)\Delta x)^T(f(x)+J(x)\Delta x)\\&=\frac{1}{2}(||f(x)||_2^2+2f(x)^TJ(x)\Delta x+\Delta x^TJ(x)^TJ(x)\Delta x)
\end{aligned}

$$令其对$\Delta x$的导数为零得：$$

2J(x)^Tf(x)+2J(x)^TJ(x)\Delta x=0

$$即：$$

J(x)^TJ(x)\Delta x = -J(x)^Tf(x)

$$我们可以把这个式子写为：$$

H\Delta x=g

$$与上面的二阶（牛顿）法做对比，可以发现这里实际是用$J(x)^TJ(x)$近似海森矩阵。 所以这个迭代过程可以写为： >1. 给定初始值$x_0$； >2. 对于第$k$次迭代，求出当前的雅可比矩阵$J(x_k)$和误差$f(x_k)$； >3. 求解增量方程：$H\Delta x=g$，即$\Delta x_k=H^{-1}g$； >4. 若$\Delta x_k$足够小，则停止，否则令$x_{k+1}=x_k+\Delta x_k$，并返回步骤2。 **总结**： 由于海森矩阵不容易算，所以这里用雅可比的平方近似海森矩阵。这种方法简单实用，但$\Delta x_k=H^{-1}g$中无法保证$H$可逆（二次近似不可靠）。 然后Levenberg_Marquadt方法在一定程度上改善了它。 ## Levenberg_Marquadt 与G-N方法不同，这里引进了一个参考值，或者说区域，用于描述近似程度：$$

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

$$这个式子可以理解为：**实际下降 / 近似下降**，若太大，则减小近似范围，若太小，则增加近似范围。 其迭代过程可以描述为： >1. 给定初始值$x_0$以及初始优化半径$\mu$； >2. 对于第$k$次迭代，求解：$$\min_{\Delta x_k}\frac{1}{2}||f(x_k)+J(x_k)\Delta x_k||^2, \quad s.t. ||D\Delta x_k||^2\le\mu$$这里$\mu$是信赖区域的半径，$D$的话可以理解为形状参数，当$D=1$时，表示取一个球，后来普遍令$D$取非负对角矩阵，表示椭球。 >3. 计算$\rho$； >4. 若$\rho>\frac{3}{4}$，则$\mu=2\mu$； >5. 若$\rho<\frac{1}{4}$，则$\mu=0.5\mu$；（这两个都是经验值） >6. 如果$\rho$大于某个阈值，认为近似可行，令$x_{k+1}=x_k+\Delta x_k$ >7. 判断算法是否收敛，如果不收敛则返回步骤2，否则结束。 最后，在上面的步骤中还剩一个问题，就是第二步的带约束的优化问题，依旧用拉格朗日进行计算即可：$$

\min_{\Delta x_k}\frac{1}{2}||f(x_k)+J(x_k)\Delta x_k||^2+\frac{\lambda}{2}||D\Delta x||^2

$$参照G-N展开，增量方程为：$$

(H+\lambda D^TD)\Delta x=g

$在Levenberg方法中，取$D=I$，则：$(H+\lambda I)\Delta x=g$$这里的$H$依旧是$J(x)^TJ(x)$。
对比G-N方法，这里前面加上了一项，相当于在G-N方法的基础上增强了$H$的正定性。

G-N 和 L-M的对比

1. G-N方法属于线性搜索方法：先找到方向，再确定长度；\
2. L-M方法属于信赖区域方法（Trust Region），认为近似只在区域内可靠。\
3. LM相比于GN，能够保证增量方程的正定性，即认为近似只在一定范围内成立，如果近似不好则缩小范围。收敛能够更好一点。\
4. 从增量方程上来看，可以看成一阶和二阶的混合，参数$\lambda$控制两边的权重。以$(H+\lambda I)\Delta x=g$为例，去掉$H$则变成一阶的，去掉$\lambda$项则变成二阶的。\
5. 一般来说，LM的计算量会比GN大一些，但收敛性也更好。所以简单的情况用GN，复杂的用LM。

注：本文参考自《视觉SLAM十四讲》