《视觉SLAM十四讲》学习笔记：第6讲非线性优化

《视觉SLAM十四讲》学习笔记：第6讲非线性优化

前言：本学习笔记将记录《视觉SLAM十四将》中一些重要的知识点，并对书中一些比较难的知识点添加上一些笔者个人的理解，以供笔者本人复习并与各位同学一起交流学习。本笔记结构将与原书结构一致，如果某一目录下面没有任何笔记，则代表笔者认为该小节内容相对来说没有过多重点知识。

本节目标
理论
1.理解最小二乘法的含义和处理方式。
2.理解Gauss-Newton，Levenburg-Marquate等下降策略。
实践
1.学习Ceres库和g2o库的基本使用方法。

本章意义：
从有噪声的数据中进行准确的状态估计。

6.1 状态估计问题

6.1.1 最大后验与最大似然
首先回顾一下讨论的经典$SLAM$模型。它由一个运动方程和一个观测方程构成，如下图所示：

其中$x_k$是相机的位姿。可以使用变换矩阵或李代数表示。至于观测方程，对于单目相机，也即是针孔相机模型。其具体参数化形式如下，位姿变量$x_k$可以由$T_k$或$exp({\xi }_k^{\wedge })$表达，两者是等价的。运动方程在视觉SLAM中没有特殊性，现讨论观测方程，假设$x_k$处对路标$y_j$进行了一次观测，对应到图像上的像素位置$z_{k,j}$，那么，观测方程可以表示成：

其中$K$为相机内参，$s$为像素点的距离。
倘若考虑到数据受到噪声的影响，在运动和观测方程中，我们通常假设两个噪声项$w_k，v_{k,j}$满足零均值的高斯分布：

在这些噪声的影响下，我们希望通过带噪声的数据$z$和$u$，推断位姿$x$和地图$y$（以及它们的概率分布），这构成了一个状态估计问题。历史上扩展卡尔曼滤波作为求解SLAM的一个工具，但是卡尔曼滤波只关心当前时刻状态估计$x_k$，而对之前的状态则不多考虑。而在SLAM过程中，数据是随着时间逐渐到来的，因此，近年来普遍使用的非线性优化方法，使用所有时刻采集到的数据进行状态估计，并被认为优于传统的滤波器，因此，非线性优化方法已经成为当前视觉SLAM的主流方法。
在非线性优化中，我们把所有待估计的变量放在一个“状态变量”中：

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

这里的$u$和$z$也是对所有数据的统称。特别地，当我们没有概率运动的传感器，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计$P(x|z)$的条件概率分布。如果忽略图像在时间上的联系，把它们看作一堆彼此没有关系的图片，该问题也称为 Structure from Motion（SfM），即如何从许多图像中重建三维空间结构 。在这种情况下， SLAM 可以看作是图像具有时间先后顺序的，需要实时求解一个 SfM 问题。为了估计状态变量的条件分布，利用贝叶斯法则，有：

贝叶斯法则左侧通常称为后验概率。它右侧$P(z|x)$称为似然，另一部分$P(x)$称为先验。直接求后验分布时困难的，但是求一个状态最优估计，使得在该状态下，后验概率最大化，则是可行的：

由于贝叶斯法则的分母部分与待估计的状态$x$无关，因而可以忽略。贝叶斯法则告诉我们，求解最大后验概率，相当于最大化似然和先验的乘积。然而，由于不知道机器人位姿大概在什么地方，此时就没有了先验。那么，可以求解$x$的最大似然估计：

直观地说，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计，可以理解成：“在什么样的状态下，最可能产生现在观测到的数据 ”。
6.1.2 最小二乘的引出
在高斯分布的假设下，最大似然能够有较简单的形式。回顾观测模型，对于某一次观测：

由于我们假设了噪声项$v_k\sim N(0,Q_{k,j})$，所以观测数据的条件概率为：

它依然是一个高斯分布。为了计算使它最大化的$x_k,y_j$，我们往往使用最小化负对数的方式，来求一个高斯分布的最大似然。
高斯分布在负对数下有较好的数学形式。考虑一个任意的高维高斯分布$x\sim N(\mu ,\Sigma )$，它的概率密度函数展开形式为：

取它的负对数，则变为：

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

该式等价于最小化噪声项（即误差）的平方（$\Sigma$范数意义下）。因此，对于所有的运动和任意的观测，我们定义数据与估计值之间的误差：

并求该误差的平方之和：

这就得到了一个总体意义下的最小二乘问题。我们明白它的最优解等价于状态的最大似然估计。直观来讲，由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程中时，它们并不会完美的成立。这时候把状态的估计值进行微调，使整体误差下降一些，一般会到达一个极小值。这就是一个典型非线性优化的过程。
观察（6.12），可以发现SLAM中的最小二乘问题具有一些特定的结构：
1.首先，整个问题的目标函数由许多个误差的（加权的）平方和组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。每个误差项是一个小规模的约束，因此可以进行线性近似，最后再把这个误差项的小雅可比矩阵块放到整体的雅可比矩阵中。由于这种做法，我们称每个误差项对应的优化变量为参数块（Parameter Block） 。
2.整体误差由很多小型误差项之和组成的问题，其增量方程的求解会具有一定的稀疏性，使得它们在大规模时亦可求解。
3.其次，如果使用李代数表示，则该问题是无约束的最小二乘问题。但如果用旋转矩阵（变换矩阵）描述位姿，则会引入旋转矩阵自身的约束（旋转矩阵必须是正交阵且行列式为 1）。额外的约束会使优化变得更困难。这体现了李代数的优势。
4.最后，我们使用了平方形式（二范数）度量误差，它是直观的，相当于欧氏空间中距离的平方。但它也存在着一些问题，并且不是唯一的度量方式。我们亦可使用其他的范数构建优化问题。
下面将讨论如何求解这个最小二乘问题。

6.2 非线性最小二乘

虽然在李代数章节中已经求得其导数形式，但往往不方便直接求解这个最小二乘问题，因此，可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤可列写如下：

这让求解导函数为零的问题，变成了一个不断寻找梯度并下降的过程。直到某个时刻增量非常小，无法再使函数下降。此时算法收敛，目标达到了一个极小，我们完成了寻找极小值的过程。在这个过程中，我们只要找到迭代点的梯度方向即可，而无需寻找全局导函数为零的情况。
而增量$\Delta x_k$的确定主要有两种方法。
6.2.1 一阶和二阶梯度法
这两种方法分别用了一阶和二阶泰勒展开，但是都有各自的局限，一阶梯度法（最速下降法）过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的$H$矩阵，这在问题规模较大时非常困难，我们通常倾向于避免$H$的计算。此处不过多介绍，此处稍微极少一下两类更加实用的方法：高斯牛顿法和列文伯格——马夸尔特方法。
6.2.2 Gauss-Newton
Gauss Newton是最优化算法里面最简单的方法之一。它的思想是将$f(x)$进行一阶的泰勒展开（请注意不是目标函数$f(x{)}^2$）：

这里$J(x)$为$f(x)$关于$x$的导数，实际上是一个$m\times n$的矩阵，也是一个雅可比矩阵。根据前面的框架，当前的目标是为了寻找下降矢量$\Delta x$，使得$||f(x+\Delta x)|{|}^2$达到最小。为了求$\Delta x$，我们需要解一个线性的最小二乘问题：

根据极值条件，将上述目标函数对$\Delta x$求导，并令导数为零。由于这里考虑的是$\Delta x$的导数（而不是$x$），我们最后将得到一个线性的方程。为此，先展开目标函数的平方项：

求上式关于$\Delta x$的导数，并令其为零：

可以得到如下方程组：

注意：这里要求解的变量是$\Delta x$，因此这是一个线性方程组，我们称它为增量方程，也可以称为高斯牛顿方程或者正规方程。我们把左边的系数定义为$H$，右边定义为$g$，那么上式变为：

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

从算法步骤中可以看到，增量方程的求解占据着主要地位。原则上，它要求我们所用的近似$H$矩阵是可逆的，但实际数据计算得到的$J^{T}J$却只有半正定性。也就是说，使用Gauss Newton方法时，可能出现$J^{T}J$为奇异矩阵或者病态的情况，此时增量的稳定性较差，导致算法不收敛。更严重的是，就算我们假设$ H$ 非奇异也非病态，如果我们求出来的步长 $∆x$ 太大，也会导致我们采用的局部近似
(6.19) 不够准确，这样一来我们甚至都无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的。
而Levenberg-Marquadt 方法在一定程度上修正了这些问题。
6.2.3 Levenberg-Marquadt
由于 Gauss-Newton 方法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自然地想到应该给$∆x$ 添加一个信赖区域（Trust Region），不能让它太大而使得近似不准确。非线性优化种有一系列这类方法，这类方法也被称之为信赖区域方法 (Trust Region Method)。在信赖区域里边，我们认为近似是有效的；出了这个区域，近似可能会出问题。
一个比较好的确定信赖区域的范围是根据我们的近似模型跟实际函数之间的差异来确定这个范围：如果差异小，我们就让范围尽可能大；如果差异大，我们就缩小这个近似范围。因此，考虑使用

如果$\rho$接近于1，则近似是好的。如果$\rho$太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果$\rho$比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。
于是，可以得到一个比Gauss Newton更好的框架：

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

这里$\lambda$为Lagrange乘子。类似于Gauss-Newton中的做法，展开后，核心问题仍是计算增量的线性方程：

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

我们看到，当参数$\lambda$ 比较小时， $H$ 占主要地位，这说明二次近似模型在该范围内是比较好的， L-M 方法更接近于 G-N 法。另一方面，当 $\lambda$比较大时， $\lambda I$ 占据主要地位， L-M
更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。 L-M 的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量 $∆x$。
总而言之，非线性优化问题的框架，分为 Line Search 和 Trust Region 两类。 Line Search 先固定搜索方向，然后在该方向寻找步长，以最速下降法和 Gauss-Newton 法为代表。而 Trust Region 则先固定搜索区域，再考虑找该区域内的最优点。此类方法以 L-M 为代表。实际问题中，我们通常选择 G-N 或 L-M 之一作为梯度下降策略。
6.2.4 小结
1.实际上非线性优化的所有迭代求解方案，都需要用户来提供一个良好的初始值。由于目标函数太复杂，导致在求解空间上的变化难以琢磨，对问题提供不同的初始值往往会导致不同的计算结果。这种情况是非线性优化的通病：大多数算法都容易陷入局部极小值。因此，无论是哪类科学问题，我们提供初始值都应该有科学依据，例如视觉 SLAM 问题中，我们会用 ICP， PnP 之类的算法提供优化初始值。总之，一个良好的初始值对最优化问题非常重要！
2.在视觉SLAM线性增量方程的矩阵通常是稀疏的，这也为求解提供了便利。