视觉SLAM十四讲笔记-9-1

视觉SLAM十四讲笔记-9-1

后端

主要目标
1.理解后端的概念
2.理解以 EKF 为代表的滤波器后端的工作原理
3.理解非线性优化的后端，明白稀疏性是如何利用的
4.使用 g2o 和 Ceres 实际操作后端优化
从前面章节的学习中可以看到，前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，这个地图在长时间内是不准确的。所以，在视觉里程计的基础上，还希望构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。考虑到精度与性能的平衡，实际中存在许多不同的做法。

9.1 概述

9.1.1 状态估计的概率解释

在第 2 章中提到，视觉里程计只有短暂的记忆，但是实际应用中还是希望整个运动轨迹在较长时间内都能保持最优的状态。在后端优化中，通常考虑一段更长时间内(或所有时间内)的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会使用未来的信息来更新，这种处理方式称为"批量的"(Batch)。否则，如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定，则称为"渐进的"(Incremental)。
SLAM 过程可以由运动方程和观测方程来描述。假设在 $t=0$ 到 $t=N$ 的时间内，有位姿 $x_0$ 到 $x_N$，并且有路标 $y_1,y_2,...,y_m$。按照之前的写法，运动和观测方程为：
$$\{\begin{array}{c}{x}_k=f\left(x_{k-1},u_k\right)+w_k\\ z_{k,j}=h\left(y_j,x_k\right)+v_{k,j}\end{array}k=1,\cdots ,N,j=1,\cdots ,M$$
注意以下几点：
1.观测方程中，只有当 $x_k$ 看到了 $y_j$ 时，才会产生观测数据，否则就没有。事实上，在一个位置通常只能看到一小部分路标。而且，由于视觉 SLAM 特征点数量众多，所以实际中观测方程的数量会远远大于运动方程。
2.有时候有可能没有测量运动的装置，也可能没有运动方程。在这个情况下，有若干种处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种方式都是可行的。在没有相机运动的情况下，整个优化问题就只能由许多个观测方程组成。不同的是, SLAM 中的图像有时间上的先后顺序，而 SfM 中允许使用完全无关的图像。
每一个运动方程和观测方程都会受到噪声影响，所以要把这里的位姿 $x$ 和路标 $y$ 看成服从某种概率分布的随机变量，而不是单独的一个数。因此，关心的问题变成：当拥有某些运动数据 $u$ 和观测数据 $z$ 时，如何确定状态量 $x,y$ 的分布? 进而，如果得到了新时刻的数据，它们的分布又将发生怎样的变化?在比较常见且合理的情况下，假设状态量和噪声项服从高斯分布—这意味着在程序中只需要存储它们的均值和协方差矩阵即可。均值可以看作对变量最优值的估计，而协方差矩阵则度量了它的不确定性。那么，问题就转变为：当存在一些运动数据和观测数据时，如何估计状态量的高斯分布?
当机器人蒙着眼睛在一个未知的地方走路时，尽管知道自己每一步走多远，但是随着时间的流逝，越来越不确定自己的位置，这说明当输入数据受到噪声影响时，误差是逐渐累积的。对位置方差的估计将越来越大。但是，当睁开眼睛，由于能够不断地观测到外部场景，就会使得位置估计的不确定性变小。如果用椭圆或者椭球直观地表达协方差矩阵，那么这个过程就像是在手机地图软件中走路。以下图为例(左侧为只有运动方程时，由于下一时刻的位姿是在上一时刻的基础上添加了噪声，所以不确定性会越来越大；右侧为存在路标时，不确定性会明显减少。)，当没有观测数据的时候，这个圆会随着运动越来越大；而如果有正确的观测数据，圆就会缩小至一定的大小，保持稳定。

上面的过程以比喻的形式解释了状态估计中的问题，下面以定量的形式来看待它。在第 6 章中，介绍了最大似然估计，提到批量状态估计问题可以转化为最大似然估计问题，并使用最小二乘法进行求解。在本节中，将探讨如何将该结论应用于渐进式问题，得到一些经典的结论。同时，在视觉 SLAM 中，最小二乘法又有何特殊的结构。
首先，由于位姿和路标点都是待估计的变量，改变记号，令 $x_k$ 为 $k$ 时刻的所有未知量，它包含了当前时刻的相机位姿与 $m$ 个路标点。在这种记号的意义下，写成：
x k = def ⁡ { x k , y 1 , ⋯   , y m } x_{k} \stackrel{\operatorname{def}}{=}\left\{x_{k}, y_{1}, \cdots, y_{m}\right\} xk​=def{xk​,y1​,⋯,ym​}
同时，把 $k$ 时刻的所有观测记作 $z_k$。于是，运动方程和观测方程的形式可以写得更简洁。
$$\{\begin{array}{c}{x}_k=f\left(x_{k-1},u_k\right)+w_k\\ z_k=h\left(x_k\right)+v_k\end{array}k=1,\cdots ,N$$
现在考虑第 $k$ 时刻的情况。希望用过去 0 到 $k$ 时刻的数据中估计现在的状态分布：
$$P(x_k|x_0,u_{1:k},z_{1:k})$$
下标 $0:k$ 表示从 0 时刻到 $k$ 时刻的所有数据。$z_k$ 表示所有在 $k$ 时刻的观测数据。同时， $x_k$ 实际上和 $x_{k-1}$ 、$x_{k-2}$ 这些量都有关，但是此式没有显式地写出来。
下面来看如何对状态进行估计。按照贝叶斯法则，将 $z_k$ 与 $x_k$ 交换位置，有：
$$\underset{\text{posterior }}{\underbrace{P\left(x_k\mid x_0,u_{1:k},z_{1:k}\right)}}=P\left(x_k\mid x_0,u_{1:k},z_{1:k-1},z_k\right)=\frac{P\left(z_k\mid x_k\right)P\left(x_k\mid x_0,u_{1:k},z_{1:k-1}\right)}{P\left(z_k\right)}\propto \underset{\text{likelihood }}{\underbrace{P\left(z_k\mid x_k\right)P\left(x_k\mid x_0,u_{1:k},z_{1:k-1}\right)}}\text{ prior }$$
第一项称为似然，第二项称为先验。似然由观测方程给定，而先验部分，$x_k$ 是基于过去所有的状态估计得来的。至少，它会受 $x_{k-1}$ 影响，于是以 $x_{k-1}$ 时刻为条件概率展开：
$$P\left(x_k\mid x_0,u_{1:k},z_{1:k-1}\right)=\int P\left(x_k\mid x_{k-1},x_0,u_{1:k},z_{1:k-1}\right)P\left(x_{k-1}\mid x_0,u_{1:k},z_{1:k-1}\right)\mathrm{d}{x}_{k-1}$$
如果考虑很久之前的状态，也可以继续对此式进行展开，但只关心 $k$ 时刻和 $k-1$ 时刻的情况。至此，给出了贝叶斯估计。对这一步的后续处理，方法上产生了一些分歧。大体上讲，存在若干种选择：一种方法是假设马尔科夫性，简单的一阶马氏性认为，$k$ 时刻状态只与 $k-1$ 时刻状态有关，而与之前的无关。如果做出这样的假设，就会得到以扩展卡尔曼滤波(EKF) 为代表的滤波器方法。在滤波方法中，会从某时刻的状态估计，推导到下一个时刻。另一种方法是依然考虑 $k$ 时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。非线性优化的基本知识已经在前文介绍过。目前，视觉 SLAM 的主流为非线性优化方法。

9.1.2 线性系统和KF

首先来看滤波器模型，假设了马尔科夫性，从数学角度会发生哪些变化尼？首先当前时刻状态只和上一时刻有关，上面式子中右侧第一部分可以进行简化：
$$P\left(x_k\mid x_{k-1},x_0,u_{1:k},z_{1:k-1}\right)=P\left(x_k\mid x_{k-1},u_k\right)$$由于 $k$ 时刻状态只与 $k-1$ 之前的状态无关，所以就简化为只与 $x_{k-1}$ 和 $u_k$ 相关的形式，与 $k$ 时刻的运动方程对应。
$$P\left(x_{k-1}\mid x_0,u_{1:k},z_{1:k-1}\right)=P\left(x_{k-1}\mid x_0.u_{1:k-1}.z_{1:k-1}\right)$$考虑到 $k-1$ 时刻的输入量 $u_k$ 与 $k-1$ 时刻的状态无关，所以把 $u_k$ 拿掉。可以看出，这项实际上是 $k-1$ 时刻的状态分布。于是，这一系列方程说明，实际在做的是“如何把 $k-1$ 时刻的状态分布推导至 $k$ 时刻”。在程序运行期间，只要维护一个状态量，对它不断进行迭代和更新即可。如果假设状态量服从高斯分布，那么只需要考虑维护状态量的均值和协方差即可。
从形式最为简单的线性高斯系统开始，最后得到卡尔曼滤波器。线性高斯系统是指，运动方程和观测方程可以由线性方程来描述：
$$\{\begin{array}{c}{x}_k=A_k{x}_{k-1}+u_k+w_k\\ z_k=C_k{x}_k+v_k\end{array}k=1,\cdots ,N$$并假设所有的状态和噪声都满足高斯分布。记这里的噪声服从零均值高斯分布：
$$w_k\sim N(0,R)，v_k\sim N(0,Q)$$为了简洁，省略了 $R$ 和 $Q$ 的下标。利用马尔科夫性，假设知道了 $k-1$ 时刻的后验(在 $k-1$ 时刻看来)状态估计 ${\hat{x}}_{k-1}$ 及其协方差 ${\hat{P}}_{k-1}$，现在要根据 $k$ 时刻的输入和观测数据，确定 $x_k$ 的后验分布。(为区分推导中的先验和后验，在记号上做一点区别：以上 ${\hat{x}}_{k-1}$ 表示后验，${\breve{x}}_{k-1}$ 表示先验分布)。
卡尔曼滤波的第一步，通过运动方程确定 $x_k$ 的先验分布。这一步是线性的，而高斯分布的线性变换仍然是高斯分布。所以，显然有：$$P\left(x_k\mid x_0,u_{1:k},z_{1:k-1}\right)=N\left(A_k{\hat{x}}_{k-1}+u_k,A_k{\hat{P}}_{k-1}{A}_k^T+R\right)$$这一步称为预测。它显示了如何从上一个时刻的状态，根据输入信息(有噪声)推断当前时刻的状态分布。它的分布也就是先验，记：$${\check{x}}_k=A_k{\hat{x}}_{k-1}+u_k,{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R$$一方面，显然这一步状态的不确定性要变大，因为系统中添加了噪声。另一方面，由观测方程，可以计算在某个状态下应该产生怎样的观测数据。
$$P\left(z_k\mid x_k\right)=N\left(C_k{x}_k,Q\right)$$
为了得到后验概率，根据本章最开始由贝叶斯法则推导公式：

/
希望用过去 0 到 $k$ 时刻的数据中估计现在的状态分布：
$$P(x_k|x_0,u_{1:k},z_{1:k})$$
下标 $0:k$ 表示从 0 时刻到 $k$ 时刻的所有数据。$z_k$ 表示所有在 $k$ 时刻的观测数据。同时， $x_k$ 实际上和 $x_{k-1}$ 、$x_{k-2}$ 这些量都有关，但是此式没有显式地写出来。
/

得：要计算上述似然概率和先验概率的乘积。
首先，先把结果设为 $x_k\sim N({\hat{x}}_k,{\hat{P}}_k)$(因为状态 $x_k$ 一定也是服从高斯分布的)，那么：
$$N\left({\hat{x}}_k,{\hat{P}}_k\right)=\eta N\left(C_k{x}_k,Q\right)\cdot N\left({\check{x}}_k,{\check{P}}_k\right)$$上式中等式两侧都是高斯分布，因此只需要比较指数部分，无需理会高斯分布前面的因子部分。指数分布很像是一个二次型的配方，来推导一下。首先，把指数部分展开，有：$${\left(x_k-{\hat{x}}_k\right)}^T{\hat{P}}_k^{-1}\left(x_k-{\hat{x}}_k\right)={\left(z_k-C_k{x}_k\right)}^T{Q}^{-1}\left(z_k-C_k{x}_k\right)+\left(x_k-{\check{x}}_k\right){\check{P}}_k^{-1}\left(x_k-{\check{x}}_k\right)$$为了求左侧的 ${\hat{x}}_k$ 和 ${\hat{P}}_k$，把两边展开，左边展开为：
$$\begin{array}{c}{\left(x_k-{\hat{x}}_k\right)}^T{\hat{P}}_k^{-1}\left(x_k-{\hat{x}}_k\right)\\ =\left(x_k^T{\hat{P}}_k^{-1}-{\hat{x}}_k^T{\hat{P}}_k^{-1}\right)\left(x_k-{\hat{x}}_k\right)\\ =x_k^T{\hat{P}}_k^{-1}{x}_k-{\hat{x}}_k^T{\hat{P}}_k^{-1}{x}_k-x_k^T{\hat{P}}_k^{-1}{\hat{x}}_k+{\hat{x}}_k^T{\hat{P}}_k^{-1}{\hat{x}}_k\\ =\underset{\text{二 次 项 }}{\underbrace{x_k^T{\hat{P}}_k^{-1}{x}_k}}+(\underset{\text{一 次 项 }}{\underbrace{-2{\hat{x}}_k^T{\hat{P}}_k^{-1}{x}_k}})+\underset{\text{常 数 项 }}{\underbrace{{\hat{x}}_k^T{\hat{P}}_k^{-1}{\hat{x}}_k}}\end{array}$$
右边展开为：
$$\begin{array}{c}{\left(z_k-C_k{x}_k\right)}^T{Q}^{-1}\left(z_k-C_k{x}_k\right)+\left(x_k-{\check{x}}_k\right){\check{P}}_k^{-1}\left(x_k-{\check{x}}_k\right)\\ =\left(z_k^T{Q}^{-1}-x_k^T{C}_k^T{Q}^{-1}\right)\left(z_k-C_k{x}_k\right)+\left(x_k^T{\check{P}}_k^{-1}-{\check{x}}_k^T{\check{P}}_k^{-1}\right)\left(x_k-{\check{x}}_k\right)\\ =\left(z_k^T{Q}^{-1}{z}_k-x_k^T{C}_k^T{Q}^{-1}{z}_k-z_k^T{Q}^{-1}{C}_k{x}_k+x_k^T{C}_k^T{Q}^{-1}{C}_k{x}_k\right)+\left(x_k^T{\check{P}}_k^{-1}{x}_k-{\check{x}}_k^T{\check{P}}_k^{-1}{x}_k-x_k^T{\check{P}}_k^{-1}{\check{x}}_k+{\check{x}}_k^T{\check{P}}_k^{-1}{\check{x}}_k\right)\\ =\left(z_k^T{Q}^{-1}{z}_k-2z_k^T{Q}^{-1}{C}_k{x}_k+x_k^T{C}_k^T{Q}^{-1}{C}_k{x}_k\right)+\left(x_k^T{\check{P}}_k^{-1}{x}_k-2{\check{x}}_k^T{\check{P}}_k^{-1}{x}_k+{\check{x}}_k^T{\check{P}}_k^{-1}{\check{x}}_k\right)\\ =\left(x_k^T{C}_k^T{Q}^{-1}{C}_k{x}_k+x_k^T{\check{P}}_k^{-1}{x}_k\right)+\left(-2z_k^T{Q}^{-1}{C}_k{x}_k-2{\check{x}}_k^T{\check{P}}_k^{-1}{x}_k\right)+\left(z_k^T{Q}^{-1}{z}_k+{\check{x}}_k^T{\check{P}}_k^{-1}{\check{x}}_k\right)\\ =\underset{\text{二 次 项 }}{\underbrace{x_k^T\left(C_k^T{Q}^{-1}{C}_k+{\check{P}}_k^{-1}\right)x_k}}+\underset{\text{一 次 项 }}{\underbrace{\left(-2z_k^T{Q}^{-1}{C}_k{x}_k-2{\check{x}}_k^T{\check{P}}_k^{-1}{x}_k\right)}}+\underset{\text{常 数 项 }}{\underbrace{\left(z_k^T{Q}^{-1}{z}_k+{\check{x}}_k^T{\check{P}}_k^{-1}{\check{x}}_k\right)}}\end{array}$$
并比较 $x_k$ 的二次和一次系数。对于二次系数，有：
$$x_k^T{\hat{P}}_k^{-1}{x}_k=x_k^T\left(C_k^T{Q}^{-1}{C}_k+{\check{P}}_k^{-1}\right)x_k$$
因此，可得：
$${\hat{P}}_k^{-1}=C_k^T{Q}^{-1}{C}_k+{\check{P}}_k^{-1}$$
该式子给出了协方差的计算过程。
为了方便后面列写式子，定义一个中间变量：
$$K={\hat{P}}_k{C}_k^T{Q}^{-1}$$
根据此定义，在 $x_k^T{\hat{P}}_k^{-1}{x}_k=x_k^T\left(C_k^T{Q}^{-1}{C}_k+{\check{P}}_k^{-1}\right)x_k$ 左右各乘 $ \hat{P}_{k}$，有：
$$I={\hat{P}}_k{C}_k^T{Q}^{-1}{C}_k+{\hat{P}}_k{\check{P}}_k^{-1}$$
于是有：
$${\hat{P}}_k=(I-K{C}_k){\check{P}}_k$$
然后比较一次项的系数，有：
$$-2{\hat{x}}_k^T{\hat{P}}_k^{-1}{x}_k=-2z_k^T{Q}^{-1}{C}_k{x}_k-2{\check{x}}_k^T{\check{P}}_k^{-1}{x}_k$$
整理(取系数并转置)得：
$$\begin{array}{rl}{\hat{x}}_k^T{\hat{P}}_k^{-1}{x}_k=& \left(z_k^T{Q}^{-1}{C}_k+{\check{x}}_k^T{\check{P}}_k^{-1}\right)x_k\\ {\hat{x}}_k^T{\hat{P}}_k^{-1}& =z_k^T{Q}^{-1}{C}_k+{\check{x}}_k^T{\check{P}}_k^{-1}\\ {\left({\hat{P}}_k^{-1}{\hat{x}}_k\right)}^T& ={\left(C_k^T{Q}^{-1}{z}_k+{\check{P}}_k{\check{x}}_k\right)}^T\end{array}$$
整理后可得：
$${\hat{P}}_k^{-1}{\hat{x}}_k=C_k^T{Q}^{-1}{z}_k+{\check{P}}_k{\check{x}}_k$$
再对两侧左乘一个 ${\hat{P}}_k$，可得：
$${\hat{x}}_k={\hat{P}}_k{C}_k^T{Q}^{-1}{z}_k+{\hat{P}}_k{\check{P}}_k{\check{x}}_k$$
再将上面定义的中间变量：$K={\hat{P}}_k{C}_k^T{Q}^{-1}$，根据上面推导得：
$${\hat{x}}_k=K{z}_k+\left(I-K{C}_k\right){\check{x}}_k=K\left(z_k-C_k{\check{x}}_k\right)+{\check{x}}_k$$
于是就得到了后验均值的表达。
总而言之，上面的两个步骤可以归纳为"预测"和"更新"两个步骤：
1.预测：
$${\check{x}}_k=A_k{\hat{x}}_{k-1}+u_k,{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R$$
2.更新：
先计算 $K$，它称为卡尔曼增益。
$$K={\check{P}}_k{C}_k^T{\left(C_k{\check{P}}_k{C}_k^T+Q_k\right)}^{-1}$$
然后计算后验概率的分布：
$$\begin{array}{rl}& {\hat{x}}_k=K\left(z_k-C_k{\check{x}}_k\right)+{\check{x}}_k\\ & {\hat{P}}_k=\left(I-K{C}_k\right){\check{P}}_k\end{array}$$
至此，推到了经典的卡尔曼滤波器的整个过程。事实上，卡尔曼滤波器有若干种推导方式，而我们使用的是概率角度出发的最大后验概率估计的方式。可以看到，在线性高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计。而且，由于高斯分布经过线性变换后仍服从高斯分布，所以整个过程中没有进行任何的近似。可以说，卡尔曼滤波器构成了线性系统的最优无偏估计。

9.1.3 非线性系统和EKF

SLAM 中的运动方程和观测方程通常是非线性函数，尤其是视觉 SLAM 中的相机模型，需要使用相机内参模型及李代数表示的位姿，更不可能是一个线性系统。一个高斯分布，经过非线性变换后，往往不再是高斯分布，所以在非线性系统中，必须取一定的近似，将一个非高斯分布近似为高斯分布。
希望将卡尔曼滤波的结果拓展到非线性系统中，称为扩展卡尔曼滤波器。通常的做法是，在某个点附近考虑运动方程和观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。令 $k-1$ 时刻的均值与协方差矩阵为 ${\hat{x}}_{k-1}$，${\hat{P}}_{k-1}$。在 $k$ 时刻，把运动方程和观测方程在 ${\hat{x}}_{k-1}$，${\hat{P}}_{k-1}$ 处进行线性化(相当于一阶泰勒展开)，有：
$$x_k\approx f({\hat{x}}_{k-1},u_k)+\frac{\partial f}{\partial x_{k-1}}{|}_{{\hat{x}}_{k-1}}(x_{k-1}-{\hat{x}}_{k-1})+w_k$$
记这里的偏导数为：
$$F=\frac{\partial f}{\partial x_{k-1}}{|}_{{\hat{x}}_{k-1}}$$
同样，对于观测方程，亦有：
$$z_k\approx h(\check{x_k})+\frac{\partial h}{\partial x_k}{|}_{\check{x_k}}(x_k-\check{x_k})+n_k$$
记这里的偏导数为：
$$H=\frac{\partial h}{\partial x_k}{|}_{\check{x_k}}$$
那么，在预测步骤中，根据运动方程有：
$$P(x_k|x_0,u_{1:k},z_{0:k-1})=N(f({\hat{x}}_{k-1},u_k),F{\hat{P}}_{k-1}{F}^T+R_k)$$
上式推导和卡尔曼滤波是十分相似的。为方便表述，记这里的先验和协方差的均值为：
$$\check{x_k}=f({\hat{x}}_{k-1},u_k)，{\check{P}}_k=F{\hat{P}}_{k-1}{F}^T+R_k$$
然后，考虑在观测中有：
$$P(z_k|x_k)=N(h(\check{x_k})+H(x_k-\check{x_k}),Q_k)$$
最后，根据最开始的卡尔曼滤波展开式，可以推导出 $x_k$ 的后验概率形式。这里省略中间的推导过程，只介绍其结果。
首先，定义一个卡尔曼增益 $K_k$：
$$K_k={\check{P}}_k{H}^T(H{\check{P}}_k{H}^T+Q_k{)}^{-1}$$
在卡尔曼增益的基础上，后验概率的形式为：
$${\hat{x}}_k={\check{x}}_k+K_k\left(z_k-h\left({\check{x}}_k\right)\right),{\hat{P}}_k=\left(I-K_{k}H\right){\check{P}}_k$$
总而言之，上面的步骤可以归纳为“预测”和“更新”两个步骤：
///
1.预测：
$$\check{x_k}=f({\hat{x}}_{k-1},u_k)，{\check{P}}_k=F{\hat{P}}_{k-1}{F}^T+R_k$$
2.更新：
先计算 $K$:
$$K_k={\check{P}}_k{H}^T(H{\check{P}}_k{H}^T+Q_k{)}^{-1}$$
再计算后验概率的分布：
$${\hat{x}}_k={\check{x}}_k+K_k\left(z_k-h\left({\check{x}}_k\right)\right),{\hat{P}}_k=\left(I-K_{k}H\right){\check{P}}_k$$
///

9.1.4 EKF的讨论

EKF 以形式简洁、应用广泛著称。当想要在某段时间内估计某个不确定量时，首先想到的是EKF。在早期的SLAM中，EKF占据了很长一段时间的主导地位，研究者们讨论了各种各样的滤波器在SLAM中的应用，比如信息滤波器（Information Filter）、迭代卡尔曼滤波器（Iterated KF）、无迹卡尔曼滤波器（Unscented KF）和粒子滤波器（PF）、滑动窗口滤波器（Sliding Window Filter，SWF）等，或者用分治法等思路改进EKF的效率。时至今日，尽管认识到非线性优化比滤波器占有明显的优势，但是在计算资源受限，或待估计量比较简单的场合，EKF仍不失为一种有效的方式。
EKF有哪些局限性呢？
1.滤波器方法在一定程度上假设了马尔科夫性，也就是 $k$ 时刻的状态只与 $k-1$ 时刻相关，而与 $k-1$ 之前的状态和观测都无关。这有点像是在视觉里程计中只考虑相邻两帧的关系。如果当前帧确实与很久之前的数据相关，那么滤波器将难以处理。
而非线性优化更倾向于使用所有的历史数据。它不光考虑临近时刻的特征点与轨迹关系，更会把很久之前的状态也考虑进来，称为全体时间上的 SLAM(Full-SLAM)。在这种意义下，非线性优化方法使用了更多信息，当然也需要更多的计算。
2.EKF滤波器仅在 ${\hat{x}}_{k-1}$ 处做了一次线性化，就直接根据这次线性化的结果，把后验概率算出来。这相当于在说，认为该点处的线性化近似在后验概率处仍然是有效的。而实际上，当离工作点较远时，一节泰勒展开并不一定能够近似整个函数，这取决于运动模型和观测模型的非线性情况。如果它们有很强的非线性，那么线性近似就只在很小范围内成立，不能认为在很远的地方仍能用线性来近似。这就是 EKF 的非线性误差。
在优化问题中，尽管也做一阶(最速下降)或二阶(高斯牛顿法或者列文伯格-马夸尔特方法)的近似，但每迭代一次，状态估计发生改变之后，会重新对新的估计点做泰勒展开，而不像 EKF 那样只在固定点上做一次泰勒展开。这就使得优化的方法范围更广，在状态变化较大时也能适用。所以大体来讲，可以粗略认为 EKF 仅是优化中的一次迭代。
3.从程序实现来说，EKF 需要存储状态量的均值和方差，并对它们进行维护和更新。如果把路标也加入状态变量，由于视觉 SLAM 中路标数量很多，则这个存储量是相当可观的。因此，普遍认为 EKF SLAM 不适用于大型场景。
4.EKF 等滤波器方法没有异常检测机制，导致系统在存在异常值的时候很容易发散。而在视觉 SLAM 中，异常值确是常见的：无论特征匹配还是光流法，都容易追踪或匹配到错误的点。没有异常值检测机制会让系统在使用中非常不稳定。
由于 EKF 存在这些明显的缺点，通常认为，在同等计算量的情况下，非线性优化能取得更好的效果。这里的更好是指精度和鲁棒性同时达到更好的意思。
后续将主要讨论以非线性优化为主的后端。