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

10 后端

10.1 滑动窗口滤波和优化

10.1.1 实际环境下的 BA 结构

带有相机位姿和空间点的图优化称为 BA，它能够有效地求解大规模的定位和建图问题。这在 SfM 问题中十分有用。但是在 SLAM 过程中，往往需要控制 BA 的规模，保持计算的实时性。在现实条件中，必须限制后端的计算时间。比如 BA 规模不能超过 1万个路标点，迭代不超过 20 次，用时不超过 0.5 秒等等。像 SfM 这样用一周时间重建一个城市地图的算法，在 SLAM 里不见得有效。
控制计算规模的做法有很多，比如从连续的视频中抽出一部分作为关键帧，仅构造关键帧与路标点之间的 BA，于是非关键帧只用于定位，对建图没有影贡献。但是即便如此，随着时间的推移，关键帧的数量会越来越多，地图规模将不断增长。像 BA 这样的批量优化方法，计算效率会不断下降。为了避免这种情况，需要一定的手段控制后端 BA 的规模。这些方法可以是理论上的，也可以是工程上的。
例如，最简单的控制 BA 规模的思路，是仅保留当前时刻最近的 N 个关键帧，去掉时间上更早的关键帧。于是， BA 将被固定在一个时间窗口内，离开这个窗口的则被丢掉。这种方法称为滑动窗口法。这种取 N 个关键帧的具体做方法可以有一些改变，例如不见得必须取时间上最近的，而按照某些原则，取时间上靠近，空间上又可以展开的关键帧，避免相机在停止不动时，BA 的结构会缩成一团。如果再考虑深一些，像 ORB-SLAM2 中，定义一种称为==“共视图” (Covisibility graph)==
的结构。所谓共视图，就是指那些“与现在的相机存在共同观测的关键帧构成的图”。

于是在 BA 优化时，按照某些原则在共视图内取一些关键帧和路标进行优化，仅优化与当前帧有 20 个以上共视路标的关键帧，其余部分固定不变。当共视图关系能够正确构造的时候，基于共视图的优化也会在更长的时间内保持最优。
滑动窗口也好，共视图也好，其实都是对实时计算的某种工程化的这种。不过在理论上也引入了一个新问题：刚才提到只用关键帧的信息，那么对于非关键帧的信息，那么对于非关键帧的数据该如何处理尼?接下来会仔细讨论。

10.1.2 滑动窗口法

考虑一个滑动窗口。假设这个窗口有 $N$ 个关键帧，它们的位姿表达为：
$$x_1,x_2,...,x_N$$
假设它们在向量空间可以用李代数表达，那么关于这几个关键帧，能谈论些什么尼?
显然，关心这几个关键帧的位置在哪里，以及它们的不确定度如何，这对应着它们在高斯分布假设下的均值协方差。
如果这几个关键帧还对应着一个局部地图，则可以顺带着问整个局部系统的均值和方差应该是多少。设这个滑动窗口中还有 M 个路标点: $y_1,y_2,...,y_M$，它们与 $N$ 个关键帧组成了局部地图。显然可以用上一章的 BA 方法处理这个滑动窗口，包括建立图优化模型，构建整体的 Hessian 矩阵，然后边缘化所有路标点来加速求解。
在边缘化时，考虑关键帧的位姿，即：
$$[x_1,x_2,...,x_N{]}^T\sim N([{\mu }_1,{\mu }_2,...,{\mu }_N],\Sigma )$$
其中 ${\mu }_k$ 为第 $k$ 个关键帧的位姿均值，$\Sigma$ 为所有关键帧的协方差矩阵。那么显然，均值部分就是指 BA 迭代之后的结果，而 $\Sigma$ 就是对整个 BA 的 $H$ 矩阵进行边缘化之后的结果，即对 $H$ 矩阵进行舒尔消元后的 $S$。
在滑动窗口中，当窗口结构发生改变，这些状态变量应该如何变化?这件事情可以分为两部分讨论：
1.需要在窗口中新增一个关键帧，以及它观测到的路标点。
2.需要把窗口中一个旧的关键帧删除，也可能删除它观测的路标点。
这时，滑动窗口法和传统的 BA 的区别就显现出来了。
在传统的 BA 求解中，这仅仅对应于两个不同结构的 BA，在求解上没有任何区别。但在滑动窗口的情况下，就需要讨论一些具体的细节问题了。
新增一个关键帧和路标点
考虑在上一个时刻，滑动窗口已经建立了 $N$ 个关键帧，也已知道它们服从某个高斯分布，其均值和方差如前所述。此时，新来了一个关键帧 $x_{N+1}$，那么整个问题中的变量变为 $N+1$ 个关键帧和更多路标点的集合。
删除一个旧的关键帧
当考虑删除旧关键帧时，一个理论问题将显现出来。例如，要删除旧关键帧 $x_1$，但是 $x_1$ 并不是孤立的，他会和其他帧观测到同样的路标。将 $x_1$ 边缘化之后将导致整个问题不再稀疏。

在上图这个例子中，假设 $x_1$ 看到了路标点 $y_1$ 到 $y_4$，于是在处理之前， BA 问题的 Hesian 矩阵应该像上图中左侧一样，在 $x_1$ 行的 $y_1$ 到 $y_4$ 列存在非零块，表示 $x_1$ 看到了它们。这时考虑边缘化 $x_1$ ，那么舒尔消元(Schur) 过程中相当于通过矩阵行和列操作消去非对角线处几个非零矩阵块，显然这将导致右下角的路标点矩阵块不再是非对角矩阵。这个过程称为边缘化中的填入(Fill in)。
在第九章中介绍了边缘化，当边缘化路标点时， Fill in 将出现左上角的位姿块中。不过，因为 BA 不要求位姿块为对角块，所以稀疏 BA 求解仍然可行。但是，当边缘化关键帧时，将破坏右下角路标点之间的对角块结构，这时 BA 就无法按照先前的稀疏方式迭代求解。这显然是个十分糟糕的问题。实际上，在早期的 EKF 滤波器后端中，人们确实保持着一个稠密的 Hessian 矩阵，这也使得 EKF 后端没法处理较大规模的滑动窗口。
不过，如果对边缘化的过程进行一些改造，也可以保持滑动窗口 BA 的稀疏性。例如，在边缘化某个旧的关键帧时，同时边缘化它观测到的路标点。这样，路标点的信息就会转换成剩下那些关键帧之间的共视信息，从而保持右下角部分的对角块结构。在某些 SLAM 框架中，边缘化策略会更复杂。例如在 OKVIS 中，会判断要边缘化的那个关键帧，它看到的路标点是否在最新的关键帧中仍可看到。如果不能就直接边缘化这个路标点；如果能，就丢弃被边缘化关键帧对这个路标点的观测，从而保持 BA 的稀疏性。
SWF中边缘化的直接解释
边缘化在概率上的意义就是指条件概率。所以，当边缘化某个关键帧，即"保持这个关键帧当前的估计值，求其他状态变量以这个关键帧为条件的条件概率"。所以，当某个关键帧被边缘化，它观测到的路标点就会产生一个“这些路标应该在哪里“的先验信息，从而影响其余部分的估计值。如果再边缘化这些路点值，那么它们的观测者将得到一个”观测它们的关键帧应该在哪里“的先验信息。
从数学上看，当边缘化某个关键帧，整个窗口中的状态变量的描述方式将从联合分布变为一个条件概率分布。以上面的例子来看，就是说：
$$p\left({\mathbf{x}}_1,\dots {\mathbf{x}}_4,{\mathbf{y}}_1,\dots {\mathbf{y}}_6\right)=p\left({\mathbf{x}}_2,\dots ,{\mathbf{x}}_4,{\mathbf{y}}_1,\dots {\mathbf{y}}_6\mid {\mathbf{x}}_1\right)\underset{\text{舍 去 }}{\underbrace{p\left({\mathbf{x}}_1\right)}}$$

然后舍去被边缘化部分的信息。在变量被边缘化之后，在工程中就不应该再使用它。所以滑动窗口法比较适合 VO 系统，而不适合大规模建图的系统。
由于现在 g2o 和 Ceres 还未直接支持滑动窗口法中的边缘化操作，略去本节对应的实验部分。

10.2 位姿图

10.2.1 位姿图的意义

特征点在优化问题中占据了绝大部分。实际上，经过若干次观测之后，收敛的特征点位置变化很小，发散的外点则已被剔除。对收敛点再进行优化，似乎是有些费力不讨好的。因此，更倾向于在优化几次之后就把特征点固定住，只把它们看做位姿估计的约束，而不再实际地优化它们的位置估计。
沿着这个思路继续思考，会想到：是否能够完全不管路标而只管轨迹尼?完全可以构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。不同的是，一旦初始估计完成，就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系。通过这种方式，省去了大量的特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图(Pose Graph)。位姿图示意图如下图所示，当不再优化 BA 中的路标点，仅把它们看成对姿态节点的约束时，就得到了一个计算规模小很多的位姿图。

在 BA 中特征数量远大于位姿节点。一个关键帧往往关联了数百个关键点，而实时 BA 的最大计算规模，即利用稀疏性，在当前的主流 CPU 上一般也就是几万个点左右。这就限制了 SLAM 应用场景。所以，当机器人在更大范围的时间和空间中运动时，必须考虑一些解决方式：要么像滑动窗口法那样，丢弃一些历史数据；要么像位子图的做法一样，舍弃对路标点的优化，只保留 Pose 之间的边。此外，如果有额外测量 Pose 的传感器，那么位姿图也是一种常见的融合 Pose 测量的方法。

10.2.2 位姿图的优化

位姿图优化中的节点和边都是什么意思尼?这里的节点是相机位姿，以 $T_1$，$T_2$，…，$T_n$ 来表达。而边，则是两个位姿节点之间相对运动的估计，该运动估计可以来自于特征点法或者直接法，也可以来自 GPS 或者 IMU 积分。无论通过哪种手段，假设估计了 $T_i$ 和 $T_j$ 之间的一个运动 $\Delta T_{ij}$。该运动可以由若干种表达方式，取比较自然的一种：
$$\Delta {\xi }_{ij}={\xi }_i^{-1}\circ {\xi }_j=ln(T_i^{-1}{T}_j{)}^{\vee }$$
或者按照李群的写法：
$$T_{ij}=T_i^{-1}{T}_j$$
按照图优化的思路，实际中该等式不会精确的成立，因此设立最小二乘误差，然后和以往一样，讨论误差关于优化变量的导数。这里，把上式的 $T_{ij}$ 移到等式右侧，构建误差 $e_{ij}$:
$$e_{ij}=\Delta {\xi }_{ij}ln(T_{ij}^{-1}{T}_i^{-1}{T}_j{)}^{\vee }$$
注意优化变量有两个：${\xi }_i$ 和 ${\xi }_j$。因此，求 $e_{ij}$ 关于这两个变量的导数。按照李代数的求导方式，给 ${\xi }_i$ 和 ${\xi }_j$ 各一个扰动：$\delta {\xi }_i$ 和 $\delta {\xi }_j$。于是，误差变为：
$${\hat{\mathbf{e}}}_{ij}=\ln {\left({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}\exp \left({\left(-\mathbf{\delta }{\mathbf{\xi }}_i\right)}^{\wedge }\right)\exp \left(\delta {\mathbf{\xi }}_j^{\wedge }\right){\mathbf{T}}_j\right)}^{\vee }$$
该式中两个扰动被夹在中间。为了利用 BCH 近似，希望将扰动项移至式子左侧或者右侧。
根据第四章课后习题 式子4.55(交换法则)：
$$\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{T}=\mathbf{T}\exp \left({\left(\mathrm{Ad}\left({\mathbf{T}}^{-1}\right)\mathbf{\xi }\right)}^{\wedge }\right)$$
该式表明，通过引入一个伴随项，能够”交换“扰动项左右侧的 $T$。利用它，可以将扰动挪到最右，导出右乘形式的雅克比矩阵：
$$\begin{array}{rl}{\hat{\mathbf{e}}}_{ij}& =\ln {\left({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}\exp \left({\left(-\mathbf{\delta }{\mathbf{\xi }}_i\right)}^{\wedge }\right)\exp \left(\delta {\mathbf{\xi }}_j^{\wedge }\right){\mathbf{T}}_j\right)}^{\vee }\\ & =\ln ({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}{\mathbf{T}}_j\exp \left({\left(-\mathrm{Ad}\left({\mathbf{T}}_j^{-1}\right)\mathbf{\delta }{\mathbf{\xi }}_i\right)}^{\wedge }\right)\exp {\left({\left(\mathrm{Ad}\left({\mathbf{T}}_j^{-1}\right)\mathbf{\delta }{\mathbf{\xi }}_j\right)}^{\wedge }\right)}^{\vee }\\ & \approx \ln {\left({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}{\mathbf{T}}_j\left[\mathbf{I}-{\left(\mathrm{Ad}\left({\mathbf{T}}_j^{-1}\right)\mathbf{\delta }{\mathbf{\xi }}_i\right)}^{\wedge }+{\left(\mathrm{Ad}\left({\mathbf{T}}_j^{-1}\right)\mathbf{\delta }{\mathbf{\xi }}_j\right)}^{\wedge }\right]\right)}^{\vee }\\ & \approx {\mathbf{e}}_{ij}+\frac{\partial {\mathbf{e}}_{ij}}{\partial \delta {\xi }_i}\mathbf{\delta }{\mathbf{\xi }}_i+\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathbf{\delta }{\mathbf{\xi }}_j}\mathbf{\delta }{\mathbf{\xi }}_j\end{array}$$
因此，按照李代数上的求导法则，求出了误差关于两个位姿的雅克比矩阵。关于 $T_i$ 的和关于 $T_j$ 的：
$$\begin{array}{rl}\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathbf{\delta }{\mathbf{\xi }}_i}& =-{\mathcal{J}}_r^{-1}\left({\mathbf{e}}_{ij}\right)\mathrm{Ad}\left({\mathbf{T}}_j^{-1}\right)\\ \frac{\partial {\mathbf{e}}_{ij}}{\partial \mathbf{\delta }{\mathbf{\xi }}_j}& ={\mathcal{J}}_r^{-1}\left({\mathbf{e}}_{ij}\right)\mathrm{Ad}\left({\mathbf{T}}_j^{-1}\right)\end{array}$$
由于 se(3) 上的雅克比矩阵 ${\mathcal{J}}_r$ 形式过于复杂，通常取它们的近似。如果误差接近零，就可以设它们近似为 $I$ 或者：
$${\mathcal{J}}_r^{-1}\left({\mathbf{e}}_{ij}\right)\approx \mathbf{I}+\frac{1}{2}\left[\begin{array}{cc}{\varphi }_e^{\wedge }& {\mathbf{\rho }}_e^{\wedge }\\ 0& {\varphi }_e^{\wedge }\end{array}\right]$$
理论上，即使在优化之后，由于每条边给定的观测数据并不一致，误差也不见得近似于零，所以简单地把这里的 ${\mathcal{J}}_r$ 设置为 $I$ 会有一定的损失。
了解雅克比求导后，剩下的部分就和普通的图优化一样了。简而言之，所有的位姿顶点和位姿—位姿边构成了一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束。记 $\epsilon$ 为所有边的集合，那么总体目标函数为：
$$\underset{\mathbf{\xi }}{\min }\frac{1}{2}\sum _{i,j\in \mathcal{E}}{\mathbf{e}}_{ij}^T{\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{e}}_{ij}$$
依然可以用高斯牛顿法、列文伯格－马夸尔特方法来求解此问题，除了用李代数表示优化位姿，别的都是相似的。根据以前的经验，可以用 Ceres 或者 g2o 进行求解，不必讨论详细过程。