视觉SLAM十四讲第六讲笔记

视觉SLAM十四讲第六讲笔记

我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。 然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。尽管相机可以非常好地符合针孔模型，但遗憾的是，我们得到的数据通常是受各种未知噪声影响的。即使我们有着高精度的相机，运动方程和观测方程也只能近似的成立。 由于在 SLAM 问题中，同一个点往往会被一个相机在不同的时间内多次观测，同一个相机在每个时刻观测到的点也不止一个。所以我们要知道 如何处理噪声数据。

一、状态估计的问题

1.最大后验与最大似然

SLAM模型是由一个状态方程和运动方程所构成:
$$\{\begin{array}{c}{x}_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j}\end{array}$$
这里的$x_k$是相机的位姿，我们可以使用变换矩阵或者李代数表示它。假设在$x_k$处对路标$y_j$进行了一次观测，对应到图像上的像素位置$z_{k,j}$，那么，根据上一讲的内容，观测方程可以表示成:

$$s{z}_{k,j}=Kexp({ϵ}^{\wedge })y_j$$

其中$K$是相机内参,s为像素点的距离。同时这里的$z_{k,j}$和$y_j$都必须以齐次坐标来描述，且中间有一次齐次到非齐次的转换。
如果考虑噪声的影响，在运动和观测方程中，我们通常假设两个噪声项$w_k$,$v_{k,j}$满足零均值的高斯分布:

$$w_k\sim N(0,R_k),v_k\sim N(0,Q_{k,j})$$

我们希望通过带噪声的数据$z$和$u$，推断位姿$x$和地图$y$(以及它们的概率分布)，这构成了一个状态估计问题。在非线性优化中，我们把所有待估计的变量放在一个“状态变量”中:
$$x=\{x_1,...,x_N,y_1,...,y_M\}$$

对 机器人状态的估计，就是求已知输入数据$u$和观测数据$z$的条件下，计算状态$x$的条件概率分布：

$$P(x|z,u)$$
特别地，当我们没有测量运动的传感器，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计$P(x|z)$的条件概率分布。如果忽略图像在时间上的联系，把它们看作一堆彼此没有关系的图片，该问题也称为Structure from Motion(SfM)，即如何从许多图像中重建三维空间结构。

SLAM可以看作是图像具有时间先后顺序的，需要实时求解一个SfM问题。 为了估计状态变量的条件分布，利用贝叶斯法则，有:
$$P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x)$$
贝叶斯法则左侧通常称为后验概率。它右侧的$P(z|x)$称为似然，另一部分$P(x)$称为先验。求一个状态最优估计，就是最大化这个后验概率。

$$x_{MAP}^{\ast }=\mathrm{argmax}P(x|z)=\mathrm{argmax}P(z|x)P(x)$$

求解最大后验概率，也就是相当于最大化似然和先验的乘积。 如果我们不知道机器人大概在什么位置，也就是没有了先验$P(x)$。那么，求解的就是最大似然估计(Maximize Likelihood Estimation, MLE):
$$x_{MAP}^{\ast }=\mathrm{argmax}P(x|z)=\mathrm{argmax}P(z|x)$$

所以最大似然估计，可以理解成:“在什么样的状态下，最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。

2. 最小二乘法

用来求解最大似然估计。 观测方程:
$$z_{k,j}=h(y_j,x_k)+v_{k,j}$$
由于我们假设了噪声的项$v_k\sim N(0,Q_{k,j})$， 所以观测数据的条件概率为:
$$P(z_{j,k}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})$$

它依然是一个高斯分布。为了计算使它最大化的$x_k,y_j$，我们往往使用最小化负对数的方式，来求一个高斯分布的最大似然。

考虑一个任意的高维高斯分布$x\sim N(\mu ,\Sigma )$，它的概率密度函数展开形式为:
$$P(x)=\frac{1}{\sqrt{(2\pi {)}^{N}det(\Sigma )}}exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))$$
取它的负对数:
$$-ln(P(x))=\frac{1}{2}ln((2\pi {)}^{N}det(\Sigma ))+\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )$$

对原分布求最大化相当于对负对数求最小化。 在最小化上式的$x$时，第一项与$x$无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。

代入到SLAM模型中:
$$x^{\ast }=\mathrm{argmin}((z_{k,j}-h(x_k,y_j){)}^T{Q}_{k,j}^{-1}(z_{k,j}-h(x_k,y_j)))$$

该式等价于最小化噪声项(即误差)的平方($\Sigma$范数意义下)。误差定义如下:
$$\begin{gathered} e_{v,k}=x_k-f(x_{k-1},u_k) \\ {e}_{y,j,k}=z_{k,j}-h(x_k,y_j) \end{gathered}$$
该误差的平方之和:
$$J(x)=\sum _k{e}_{v,k}^T{R}_k^{-1}{e}_{v,k}+\sum _k\sum _j{e}_{y,k,j}^T{Q}_{k,j}^{-1}{e}_{y,k,j}$$
这就得到了一个总体意义下的最小二乘问题(Least Square Problem)。它的最优解等价于状态的最大似然估计。

二、非线性最小二乘

最小二乘问题:

$$\underset{x}{\min }\frac{1}{2}||f(x)|{|}_2^2$$

这里自变量$x\in {\mathbb{R}}^n$， $f$是任意一个非线性函数，我们设它有$m$维:$f(x)\in {\mathbb{R}}^m$ 。下面讨论如何求解这样一个优化问题。最直观就是求导，让导数等于0。对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。也就是梯度下降法。

接下来的问题是，增量$\Delta x_k$如何确定？

1. 一阶和二阶梯度法

求解增量最直观的方式是将目标函数在$x$附近进行泰勒展开:
$$||f(x+\Delta x)|{|}_2^2\approx ||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x$$

这里$J$是||f(x)||^2 关于$x$的导数(雅可比矩阵)，而$H$则是二阶导数(Hessian 矩阵)。我们可以选择保留泰勒展开的一阶或二阶项，对应的求解方法则为一阶梯度或二阶梯度法。如果保留一阶梯度， 那么增量的方向为:
$$\Delta x^{\ast }=-J^T(x)$$
当然，我们还需要该方向上取一个步长$\lambda$，求得最快的下降方式。这种方法被称为最速下降法。

另一方面，如果保留二阶梯度信息，那么增量方程为:
$$\Delta x^{\ast }=\mathrm{argmin}||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x$$
求右侧等式关于$\Delta x$的导数并令它为零，就得到了增量的解:
$$H\Delta x=-J^T$$

该方法称又为牛顿法。这两种方法也存在它们自身的问题。最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的$H$矩阵，这在问题规模较大时非常困难， 我们通常倾向于避免$H$的计算。

2. Gauss-Newton

它的思想是将$f(x)$进行一阶的泰勒展开(请注意不是目标函数$f(x{)}^2$)

$$f(x+\Delta x)\approx f(x)+J(x)\Delta x$$
$J(x)$是$f(x)$关于$x$的导数，也就是一个$mxn$的矩阵，也是一个雅可比矩阵。当前的目标是为了寻找下降矢量$\Delta X$， 使得$f(x+\Delta x{)}^2$达到最小。为了求$\Delta x$， 我们需要解一个线性的最小二乘问题：

$$\Delta x^{\ast }=\underset{\Delta x}{\mathrm{argmin}}\frac{1}{2}||f(x)+J(x)\Delta x|{|}^2$$

由于这里考虑的是$\Delta x$的导数而不是$x$，我们最后将得到一个线性的方程。为此，先展开目标函数的平方项:
$$\begin{array}{rl}\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{)}^{T}J(x)\Delta x+\Delta x^{T}J(x{)}^{T}J(x)\Delta x)\end{array}$$
对$\Delta x$的导数，使其为0:
$$2J(x{)}^{T}f(x)+2J(x{)}^{T}J(x)\Delta x=0$$

整理可得:
$$J(x{)}^{T}J(x)\Delta x=-J(x{)}^{T}f(x)$$

我们要求解的变量是$\Delta x$， 因此这是一个线性方程组，我们称它为增量方程，也可以称为高斯牛顿方程(Gauss Newton equations)或者正规方程(Normal equations)。

我们把左边的系数定义为$H$，右边定义为$g$，那么上式变为：
$$H\Delta x=g$$

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

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

3.Levenberg-Marquadt

Levenberg-Marquadt方法在一定程度上修正了上面这些问题，一般认为它比Gauss Newton更为鲁棒。 尽管它的收敛速度可能会比Gauss-Newton更慢，被称之为阻尼牛顿法(Damped Newton Method) 由于Gauss-Newton方法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果， 所以我们很自然地想到应该给$\Delta x$添加一个信赖区域(Trust Region)，不能让它太大而使得近似不准确。非线性优化种有一系列这类方法，这类方法也被称之为信赖区域方法(Trust Region Method)。在信赖区域里边，我们认为近似是有效的，出了这个区域，近似可能会出问题。

那么如何确定这个信赖区域的范围呢？一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定这个范围：如果差异小，我们就让范围尽可能大；如果差异大，我 们就缩小这个近似范围。因此， 考虑使用:

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

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

这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。我们把增量限定于一个半径为$\mu$的球中，认为只在这个球内才是有效的。带上$D$之后，这个球可以看成一个椭球。在Levenberg提出的优化方法中，把$D$取成单位阵$I$，相当于直接把$\Delta x$约束在一个球中。 随后，Marqaurdt 提出将$D$取成非负数对角阵——实际中通常用$J^{T}J$的对角元素平方根，使得在梯度小的维度上约束范围更大一些。在 L-M 优化中，我们都需要解式(6.24)那样一个子问题来获得梯度。这是一个带不等式约束的优化问题，我们用Lagrange方法将它转换为一个无约束优化问题:
$$\underset{\Delta x_k}{\min }\frac{1}{2}||f(x_k)+J(x_k)\Delta x_k|{|}^2+\frac{\lambda }{2}||D\Delta x|{|}^2$$

把它展开后，我们发现该问题的核心仍是计算增量的线性方程:
$$(H+\lambda D^{T}D)\Delta x=g$$

可以看到，增量方程相比于Gauss-Newton，多了一项$\lambda D^{T}D$。如果考虑它的简化形式，即D = I，那么相当于求解:
$$(H+\lambda I)\Delta x=g$$

我们看到，当参数$\lambda$比较小时，$H$占主要地位，这说明二次近似模型在该范围内是比较好的， L-M方法更接近于G-N法。另一方面，当$\lambda$比较大时，$\lambda I$占据主要地位，L-M更接近于一阶梯度下降法(即最速下降)，这说明附近的二次近似不够好。L-M的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量。
在实际中，还存在许多其它的方式来求解函数的增量，例如Dog-Leg等方法。我们在这里所介绍的，只是最常见而且最基本的方式，也是视觉 SLAM 中用的最多的方式。总而言之，非线性优化问题的框架，分为Line Search和Trust Region两类。 Line Search先固定搜索方向，然后在该方向寻找步长，以最速下降法和Gauss-Newton法为代表。而Trust Region则先固定搜索区域，再考虑找该区域内的最优点。此类方法以 L-M 为代表。实际问题中，我们通常选择G-N或L-M之一作为梯度下降策略。