视觉SLAM第六讲

在前面几讲，我们介绍了经典SLAM模型的运动方程和观测方程。现在我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的，那么就需要在有噪声的数据中进行准确的状态估计。

状态估计问题

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

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

$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_{k,j}=h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)+{\mathbf{v}}_{k,j}\end{array}$

相机的位姿变量${\mathbf{x}}_k$可以由${\mathbf{T}}_k\in \mathrm{SE}(3)$表达，观测方程即针孔相机模型，假设在${\mathbf{x}}_k$处对路标${\mathbf{y}}_j$进行了一次观测，对应到图像上的像素位置${\mathbf{z}}_{k,j}$，那么，观测方程可以表示成

$s{\mathbf{z}}_{k,j}=\mathbf{K}({\mathbf{R}}_k{\mathbf{y}}_j+{\mathbf{t}}_k)$

其中$\mathbf{K}$为相机内参，$s$为像素点的距离。

在运动和观测方程中，我们通常假设两个噪声项${\mathbf{w}}_k,{\mathbf{v}}_{k,j}$满足零均值的高斯分布，在这些噪声的影响下，我们希望通过带噪声的数据$\mathbf{z}$和$\mathbf{u}$推断位姿$\mathbf{x}$和地图$\mathbf{y}$（以及它们的概率分布），这构成了一个状态估计问题。

处理这个状态估计问题的方法大致分成两种。

增量方法与批量方法：

• 增量方法：是一种实时处理数据的技术，在SLAM中通过持续更新当前状态估计，逐步融合新数据。这种方法通常使用滤波器（如扩展卡尔曼滤波器EKF）来递归地估计系统状态。
• 批量方法：是一种非实时的处理技术，将数据积累到一定程度后一次性进行处理。这种方法通过对所有观测数据进行全局优化来求解SLAM问题。

大体来说，增量方法仅关心当前时刻的状态估计$x_k$，而对之前的状态则不多考虑；相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器，而成为当前视觉SLAM的主流方法。

在SLAM中，为实现实时性，通常不采用SfM那样将所有数据集中处理的非实时方法。相反，SLAM中采用折中策略，如滑动窗口估计法，通过固定部分历史轨迹，仅对当前时刻附近的轨迹进行优化，从而在保证计算效率的同时维持较高的估计精度。

本讲我们重点介绍以非线性优化为主的批量优化方法，考虑从1到$N$的所有时刻，并假设有$M$个路标点。定义所有时刻的机器人位姿和路标点坐标为：

x = { x 1 , … , x N } ， y = { y 1 , … , y M } x=\left \{ x_{1},\dots ,x_{N} \right \} ，y=\left \{ y_{1},\dots ,y_{M} \right \} x={x1​,…,xN​}，y={y1​,…,yM​}

对机器人的状态估计，从概率学的观点来看，就是已知输入数据$\mathbf{u}$和观测数据$\mathbf{z}$的条件下，求状态$\mathbf{x},\mathbf{y}$的条件概率分布：

$P\left(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u}\right)$

特别地，当我们不知道控制输入，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计$P\left(\mathbf{x},\mathbf{y}\mid \mathbf{z}\right)$的条件概率分布，此问题也称为Structure from Motion（SfM），即如何从许多图像中重建三维空间结构。

为了估计状态变量的条件分布，利用贝叶斯法则，有：

$P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})=\frac{P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})}{P(\mathbf{z},\mathbf{u})}\propto \underset{\text{似然 }}{\underbrace{P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})}}\underset{\text{先验 }}{\underbrace{P(\mathbf{x},\mathbf{y})}}$

贝叶斯法则左侧称为后验概率，右侧的$P\left(\mathbf{z}\mid \mathbf{x}\right)$称为似然（Likehood），另一部分$P\left(\mathbf{x}\right)$称为先验（Prior）。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化（MaximizeaPosterior，MAP），则是可行的：

$(\mathbf{x},\mathbf{y}{)}_{\mathrm{MAP}}^{\ast }=\arg \max P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})=\arg \max P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})$

注意贝叶斯法则的分母与状态$\mathbf{x},\mathbf{y}$无关，因此在最大后验概率估计（MAP）中可以忽略，求解MAP等价于最大化似然和先验的乘积。当先验信息缺失时，MAP估计简化为最大似然估计（MLE）。

$(\mathbf{x},\mathbf{y}{)}_{\mathrm{MLE}}^{\ast }=\arg \max P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})$

最小二乘的引出

那么如何求最大似然估计呢？我们说，在高斯分布的假设下，最大似然能够有较简单的形式。回顾观测模型，对于某一次观测：

${\mathbf{z}}_{k,j}=h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)+{\mathbf{v}}_{k,j}$

由于我们假设了噪声项${\mathbf{v}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_{k,j}\right)$，所以观测数据的条件概率为：

$P\left({\mathbf{z}}_{k,j}\mid {\mathbf{x}}_k,{\mathbf{y}}_j\right)=\mathcal{N}\left(h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right),{\mathbf{Q}}_{k,j}\right)$

考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。

考虑任意高维高斯分布$\mathbf{x}\sim \mathcal{N}\left(\mu ,\mathbf{\Sigma }\right)$，它的概率密度函数展开形式为：

$P(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)$

对其取负对数，则变为：

$-\ln (P(\mathbf{x}))=\frac{1}{2}\ln \left((2\pi {)}^N\det (\mathbf{\Sigma })\right)+\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })$

因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的$\mathbf{x}$时，第一项与$\mathbf{x}$无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：

$\begin{array}{rl}{\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)}^{\ast }& =\arg \max \mathcal{N}\left(h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right),{\mathbf{Q}}_{k,j}\right)\\ & =\arg \min \left({\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)}^{\mathrm{T}}{\mathbf{Q}}_{k,j}^{-1}\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)\right)\end{array}$

该式等价于最小化误差的二次型，称为马哈拉诺比斯距离，即加权的欧氏距离，其中权重由信息矩阵${\mathbf{Q}}_{k,j}^{-1}$（高斯分布协方差矩阵的逆）确定。

在批量处理数据时，假设各时刻的输入和观测相互独立，这使得联合分布可以因式分解为独立分布的乘积：

$P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})={\prod }_{k}P\left({\mathbf{u}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{x}}_k\right){\prod }_{k,j}P\left({\mathbf{z}}_{k,j}\mid {\mathbf{x}}_k,{\mathbf{y}}_j\right)$

这说明我们可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：

$\begin{array}{l}{\mathbf{e}}_{u_k}={\mathbf{x}}_k-f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)\\ {\mathbf{e}}_{z_{k,j}}={\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\end{array}$

那么，最小化所有时刻估计值与真实读数之间马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和：

$\min J(\mathbf{x},\mathbf{y})=\min ({\sum }_k{\mathbf{e}}_{u_k}^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{e}}_{u_k}+{\sum }_k{\sum }_j{\mathbf{e}}_{z_{k,j}}^{\mathrm{T}}{\mathbf{Q}}_{k,j}^{-1}{\mathbf{e}}_{z_{k,j}})$

这样就得到了一个最小二乘问题，它的解等价于状态的最大似然估计。

如果使用李代数表示增量，则该问题是无约束的最小二乘问题，接下来需要讨论无约束非线性最小二乘问题的求解方法。

非线性最小二乘

先来考虑一个简单的最小二乘问题：

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

其中，自变量$\mathbf{x}\in {\mathbf{R}}^n$，$f$是任意标量非线性函数$f(\mathbf{x}):{\mathbf{R}}^n⟼\mathbf{R}$。

下面讨论如何求解这样一个优化问题。显然，如果$f$是个数学形式上很简单的函数，那么该问题可以用解析形式来求。令目标函数的导数为零，然后求解$\mathbf{x}$的最优值，就和求二元函数的极值一样：

$\frac{\mathrm{d}F}{\mathrm{d}x}=0$

如果$f$为简单的线性函数，那么这个问题就是简单的线性最小二乘问题，但是有些导函数可能形式复杂，使得该方程可能不容易求解。求解这个方程需要我们知道关于目标函数的全局性质，而通常这是不大可能的。对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤可列写如下：

1. 给定某个初始值${\mathbf{x}}_0$。
2. 对于第$k$次迭代，寻找一个增量$\Delta {\mathbf{x}}_k$，使得$\Vert f({\mathbf{x}}_k+\Delta {\mathbf{x}}_k){\Vert }_2^2$达到极小值。
3. 若$\Delta {\mathbf{x}}_k$足够小，则停止。
4. 否则，令${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，返回第2步。

这让求解导函数为零的问题变成了一个不断寻找下降增量$\Delta {\mathbf{x}}_k$的问题。

接下来我们考察如何寻找这个增量$\Delta {\mathbf{x}}_k$。

一阶和二阶梯度法

现在考虑第$k$次迭代，假设我们在${\mathbf{x}}_k$处，想要寻到增量$\Delta {\mathbf{x}}_k$，那么最直观的方式是将目标函数在${\mathbf{x}}_k$附近进行泰勒一阶展开：

$F\left({\mathbf{x}}_k+\Delta {\mathbf{x}}_k\right)\approx F\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^{\mathrm{T}}\Delta {\mathbf{x}}_k$

${(\Delta {\mathbf{x}}_k)}^{\ast }=\arg \min (F\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^{\mathrm{T}}\Delta {\mathbf{x}}_k)$

${(\Delta {\mathbf{x}}_k)}^{\ast }=-\mathbf{J}\left({\mathbf{x}}_k\right)$

最速下降法，它的直观意义非常简单，只要我们沿着反向梯度方向前进，步长为$\lambda$，在一阶（线性）的近似下，目标函数必定会下降。

将目标函数在${\mathbf{x}}_k$附近进行泰勒二阶展开：

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

记，$L(\Delta {\mathbf{x}}_k)=F\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^{\mathrm{T}}\Delta {\mathbf{x}}_k+\frac{1}{2}\Delta {\mathbf{x}}_k^{\mathrm{T}}\mathbf{H}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k$

${(\Delta {\mathbf{x}}_k)}^{\ast }=\arg \min (L(\Delta {\mathbf{x}}_k))$

$\frac{\mathrm{d}L(\Delta {\mathbf{x}}_k)}{\mathrm{d}\Delta {\mathbf{x}}_k}=\mathbf{J}\left({\mathbf{x}}_k\right)+\mathbf{H}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k=0$

求解这个线性方程，就得到了增量。该方法又称为牛顿法。

最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的$\mathbf{H}$矩阵，这在问题规模较大时非常困难，我们通常倾向于避免$\mathbf{H}$的计算。对于一般的问题，一些拟牛顿法可以得到较好的结果，而对于最小二乘问题，还有几类更加实用的方法：高斯牛顿法和列文伯格—马夸尔特方法。

高斯牛顿法

高斯牛顿法的思想是将$f(\mathbf{x})$进行一阶的泰勒展开，注意这里不是目标函数$F(\mathbf{x})$而是$f(\mathbf{x})$。

$f\left(\mathbf{x}+\Delta \mathbf{x}\right)\approx f\left(\mathbf{x}\right)+\mathbf{J}{\left(\mathbf{x}\right)}^{\mathrm{T}}\Delta \mathbf{x}$

$\Delta {\mathbf{x}}^{\ast }=\arg {\min }_{\Delta \mathbf{x}}\frac{1}{2}{\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\Delta \mathbf{x}\Vert }^2$

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

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

$\mathbf{J}(\mathbf{x})f(\mathbf{x})+\mathbf{J}(\mathbf{x}){\mathbf{J}}^{\mathrm{T}}(\mathbf{x})\Delta \mathbf{x}=0$

可以得到如下方程组：

$\underset{\mathbf{H}(\mathbf{x})}{\underbrace{\mathbf{J}(\mathbf{x}){\mathbf{J}}^{\mathrm{T}}}}(\mathbf{x})\Delta \mathbf{x}=\underset{\mathbf{g}(\mathbf{x})}{\underbrace{-\mathbf{J}(\mathbf{x})f(\mathbf{x})}}$

这个方程是关于变量$\Delta \mathbf{x}$的线性方程组，我们称它为增量方程，也可以称为高斯牛顿方程或者正规方程。我们把左边的系数定义为$\mathbf{H}$，右边定义为$\mathbf{g}$，那么上式变为：

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

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

1. 给定初始值${\mathbf{x}}_0$。

2. 对于第$k$次迭代，求出当前的雅可比矩阵$\mathbf{J}({\mathbf{x}}_k)$和误差$f({\mathbf{x}}_k)$。

3. 求解增量方程：$\mathbf{H}\Delta {\mathbf{x}}_k=\mathbf{g}$。

4. 若$\Delta {\mathbf{x}}_k$足够小，则停止。否则，令${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，返回第2步。

增量方程的求解在算法中至关重要，但在高斯牛顿法中，若矩阵$\mathbf{H}$仅为半正定或出现奇异、病态情况，增量的稳定性可能较差，导致算法不收敛。此外，过大的步长可能使局部近似失效，甚至无法保证迭代收敛。尽管存在这些问题，高斯牛顿法仍然是非线性优化中一种简单且有效的方法，许多算法可以视为其变种。这些算法基于高斯牛顿法的思想，并通过改进，例如一些线搜索方法加入了一个步长$\alpha$，在确定了$\Delta \mathbf{x}$后进一步找到$\alpha$使得$\parallel f(\mathbf{x})+\alpha \Delta \mathbf{x}{\parallel }^2$达到最小，而不是简单地令$\alpha =1$。

列文伯格—马夸尔特方法

高斯牛顿法只能在展开点附近具有良好的近似效果，因此引入了信赖区域来限制优化的范围，确保近似的有效性。这类方法称为信赖区域方法。信赖区域的范围根据近似模型与实际函数之间的差异来动态调整：若差异小则扩大信赖区域，若差异大则缩小区域。通过定义一个指标 $\rho$ 来量化近似的好坏程度：

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

$\rho$的分子是实际函数下降的值，分母是近似模型下降的值。如果$\rho$接近于1，则近似是好的。如果$\rho$太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果$\rho$比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。

于是，我们构建一个改良版的非线性优化框架，该框架会比高斯牛顿法有更好的效果：

1. 给定初始值${\mathbf{x}}_0$，以及初始优化半径$\mu$。

2. 对于第$k$次迭代，在高斯牛顿法的基础上加上信赖区域，求解：
   ${\min }_{\Delta {\mathbf{x}}_k}\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^{\mathrm{T}}\Delta {\mathbf{x}}_k\Vert }^2,$ s.t. ${\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2⩽\mu$

   其中$\mu$是信赖区域的半径，$\mathbf{D}$为系数矩阵，将在后文说明。

3. 计算$\rho$。

4. 若$\rho >\frac{3}{4}$，则设置$\mu =2\mu$

5. 若$\rho <\frac{1}{4}$，则设置$\mu =0.5\mu$

6. 如果$\rho$大于某阈值，则认为近似可行。令${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$。

7. 判断算法是否收敛。如不收敛则返回第2步，否则结束。

带不等式约束的优化问题，我们用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：

$\mathcal{L}\left(\Delta {\mathbf{x}}_k,\lambda \right)=\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^{\mathrm{T}}\Delta {\mathbf{x}}_k\Vert }^2+\frac{\lambda }{2}\left({\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2-\mu \right)$

这里$\lambda$为拉格朗日乘子。类似于高斯牛顿法中的做法，令该拉格朗日函数关于$\Delta \mathbf{x}$的导数为零，它的核心仍是计算增量的线性方程：

$\left(\mathbf{H}+\lambda {\mathbf{D}}^{\mathrm{T}}\mathbf{D}\right)\Delta {\mathbf{x}}_k=\mathbf{g}$

列文伯格—马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量$\Delta \mathbf{x}$。

实际问题中，我们通常选择高斯牛顿法或列文伯格—马夸尔特方法其中之一作为梯度下降策略。当问题性质较好时，用高斯牛顿。如果问题接近病态，则用列文伯格—马夸尔特方法。

小结

非线性优化算法，如高斯牛顿法和列文伯格—马夸尔特方法，需要合理的初始值，因复杂的目标函数易导致迭代陷入局部极小值。一个科学的初始值对优化至关重要，如在视觉SLAM中，常用ICP或PnP算法提供初始估计。此外，线性增量方程组的求解通常采用数值方法，如QR分解、Cholesky分解，而不是直接求逆，尤其在处理大规模问题时，这种方法更为高效。