视觉SLAM十四讲读书笔记：Ch06非线性优化

Posted on In Views:

这是我系列博客《视觉SLAM十四讲》的读书笔记中的一篇，总结了第六章“非线性优化”的内容。

Ch06 非线性优化

1 状态估计问题

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

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

第一个方程中的为相机的位姿，使用描述。第二个观测方程可以用针孔相机模型。

我们分析一点具体的参数化表达：对于位姿变量$x_k$可以使用$T_k\in\text{SE}(3)$表达，此时假设在$x_k$处对路标$y_j$进行了一次观测，得到对应像素位置为$z_{k,j}$，则观测方程可以写作

其中为相机内参矩阵，为像素点距离。

进一步地，我们考虑噪声，假设运动方程和观测方程中噪声$w_k,v_{k,j}$满足零均值高斯分布

在噪声影响下，我们希望通过带噪声的数据推断位姿和地图（以及它们的概率分布），这构成了一个状态估计问题。

SLAM的状态估计解决方法分为两个线路：
类别 增量/滤波 批量/优化
核心思想 逐帧更新当前状态 累积数据后统一优化
代表算法 EKF-SLAM 及其衍生 BA/图优化、SfM
优点 计算量小，天然在线 全局精度高，误差分布均匀
缺点 历史状态不可回溯，误差易累积 计算量大，极端做法不实时

一种折中实现是滑动窗口估计，仅对最近一段轨迹与地图进行局部优化，兼顾实时性与精度，是当前视觉 SLAM 的主流实现。

这里我们讨论批量方法，考虑1到$N$的所有时刻，假设有$M$个坐标点

同样地，不带下表的u表示所有时刻的输入，z表示所有时刻的观测数据。从概率的角度来看，我们可以将问题描述为：求的条件概率分布 特殊地，如果不知道控制输入，只有图像，那么问题就退化为了，此类问题称为SfM (Structure from Motion)

我们使用贝叶斯定理：

其中称为后验概率，称为似然，称为鲜艳概率。直接求后验困难，但是求一个状态的最优估计，使得在该状态下后验概率最大化，是可行的： 上式称为最大后验估计MAP（Maximum A Posterior）。

如果没有先验，也可以使用最大似然估计MLE（Maximum Likelihood Estimation）

具体的数学内容不再赘述，如有需要可以参考任一模式识别教材的参数估计部分。

1.2 最小二乘法

对于一次观测

由于我们假设了噪声的分布，则容易知道 观测数据也服从也服从一个高斯分布，可以使用最小化负对数的方法来求高斯分布的最大似然，对于

则： 最后这个问题等价于最小化噪声项的一个二次型，为马氏距离（Mahalanobis distance）。

考虑批量数据，我们假设各个时刻的输入和观测是独立的，于是可以对联合概率分布进行因式分解

因此我们可以独立地处理各个时刻的运动和观测，定义各次输入、观测数据与模型之间的误差为：

最小化所有时刻的估计值与真实读数之间的马氏距离，等价于求最大似然估计，因此我们得到了问题： 这就得到了一个最小二乘问题，其等价为最大似然估计。

上面我们得到SLAM的最小二乘法问题有特定的结构：
  1. 目标函数由许多误差的二次型组成。总体的维度很高，但是误差项都是简单的，整个问题呈现出一种稀疏的形式
  2. 如果使用李代数代表增量，则问题就是一个无约束的优化问题；但如果使用旋转矩阵/变化矩阵描述位姿，则会引入旋转矩阵本身的约束，使得优化变得困难。
  3. 使用马氏距离度量误差，误差的分布将影响此项的权重，如果观测很准确，那么协方差矩阵就会很小（直觉上的理解，实际上矩阵的大小不能像标量一样简单），那么他的逆矩阵就会变大。

2 非线性最小二乘

考虑一个简单的最小二乘问题： 其中，为任意标量函数。对于这样的一个优化问题，我们可以令目标函数导数为0： 解这个方程就能得到导数为0处的极值、鞍点的值（如果F是凸函数，那么就能得到最小值）。

可以在确认合适的初始值之后使用迭代的方法来逐步优化，大致步骤如下：

  1. 给定某处初始值
  2. 对于第次迭代，寻找一个增量，使得达到极小值
  3. 若足够小，就停止
  4. 否则令

我们将一个求解全局性质的问题转换为一个关注局部性质的问题（只关注）

2.1 梯度法

将在附近展开： 其中Jocobian矩阵是在的一阶导数、Hessian矩阵是在的二阶导数。

当保留一阶梯度的时候，取增量为反向的梯度： 通常再加个步长，其实就是梯度下降法

如果保留二阶梯度信息 求导可得 这个方式就是牛顿法

2.2 高斯牛顿法

对一阶泰勒展开 为了最小化，我们要求解下面的这个问题： 我们展开目标式子： 对 求导等于0，得到方程组： 这个方程称为变量的增量方程/高斯牛顿方程/正规方程。左边的系数定义为，右边定义为，上式为 高斯牛顿法使用作为牛顿法中Hessian矩阵的近似，省略了计算Hessian矩阵的过程。求解增量方程式式整个优化问题的核心所在。流程可以写为：

  1. 给定某处初始值
  2. 对于第次迭代，求出Jocabian矩阵和误差
  3. 求解增量方程：
  4. 若足够小，就停止；否则令

不过，高斯牛顿法并非总是可靠的。由于它使用 近似 Hessian，这一近似只有在残差较小、问题接近线性时才足够准确；而当模型强烈非线性或初值较差时，迭代方向可能不再是下降方向，甚至导致发散。另外，当 Jacobian 的列接近相关时， 可能变得病态，使得方程难以稳定求解。为改善这些问题，人们提出了多种改进策略，例如在线搜索 (Line Search) 框架下通过调整步长来保证下降，或在信赖域 (Trust Region) 方法中限制更新范围。在非线性最小二乘问题中，应用最为广泛的一种改进就是列文伯格–马夸尔特 (Levenberg–Marquardt, LM) 方法，它在远离最优解时更像梯度下降以保证稳定性，而在接近最优解时又保留了高斯牛顿法的快速收敛特性。

2.3 Levenberg-Marquardt Method, LM

在上面一段话中，我们提到一个方法就是给添加一个范围，称为信赖域，在这个区域内，二阶近似式有效的。这类方法也称为信赖域方法。

为了确定信赖域的范围，一个比较好的方法就是权衡近似模型和实际函数值的差异，如果差异大，那么这个区域显然不是信赖域。我们定义了指标 表示实际函数的下降值和近似下降的值的比值。越近似于1，那么说明近似是良好的，否则我们要缩小近似范围。

  1. 给定初始值以及初始优化半径
  2. 对于第k次迭代，求解带约束的优化问题. 其中是系数矩阵

  1. 计算
  2. 若，则设置
  3. 若，则设置
  4. 若小于某个阈值，则认为近似可行。令

上面提到的系数矩阵D和半径都是经验值。有了之后，我们的自变量的范围就变成了一个高维椭球。对于的选取，可以选取对角阵，也可以选取一个非负数对角阵（如的对角阵元素平方根）。

为了求解上面过程中的约束优化问题，我们可以使用拉格朗日法，求解无约束优化问题 我们求导得到正规方程为： 当选取为单位对角阵的时候，该方程退化为 当偏小的时候，该方法近似为高斯牛顿法；当偏大的时候，该方法近似为梯度下降法