09 后端 2

9.1 滑动窗口滤波和优化

9.1.1 实际环境下的 BA 结构

由于计算机算力的限制，我们必须控制 BA 的规模，一种简单的思路是仅保留离当前时刻最近的 $N$ 个关键帧，去掉时间上更早的关键帧。于是，BA 被固定在一个时间窗口内，离开这个窗口的即被抛弃，称为 滑动窗口法。

或者像 ORB-SLAM 2 那样，定义一种称为 共视图 的结构，即与当前相机存在共同观测的关键帧构成的图。在 BA 优化时，按照某些原则在共视图内取一些关键帧和路标进行优化。

9.1.2 滑动窗口法

（1）现在考虑一个滑动窗口，假设窗口内有 $N$ 个关键帧，他们的位姿表达为（李代数形式）：

$${\mathbf{x}}_1,{\mathbf{x}}_2,...,{\mathbf{x}}_N$$

假设这个滑动窗口中还有 $M$ 个路标点 ${\mathbf{y}}_1,{\mathbf{y}}_2,...,{\mathbf{y}}_M$，用上一讲中的 BA 方法来处理这个滑动窗口，包括建立图优化模型，构建海森矩阵，在边缘化所有路标点来加速求解。边缘化时，考虑关键帧的位姿：

$${\left[{\mathbf{x}}_1,\dots ,{\mathbf{x}}_N\right]}^{\mathrm{T}}\sim N\left({\left[{\mathbf{\mu }}_1,\dots ,{\mathbf{\mu }}_N\right]}^{\mathrm{T}},\mathbf{\Sigma }\right)\text{\hspace{1em}(9-1)}$$

其中 ${\mathbf{\mu }}_k$ 为第 $k$ 个关键帧的位姿均值，$\mathbf{\Sigma }$ 为所有关键帧的协方差矩阵。显然，均值部分就是 BA 迭代之后的结果，$\mathbf{\Sigma }$ 是对整个 BA 的 $\mathbf{H}$ 矩阵进行边缘化之后的结果。

（2）当窗口结构改变时：

① 先在窗口中新增一个关键帧，以及观测到的路标点；

② 把窗口中一个旧的关键帧删除，可能会删除他观测到的路标点。

• 新增一个关键帧和路标点

将新的关键帧 ${\mathbf{x}}_{N+1}$ 按照正常的 BA 流程处理即可。

• 删除一个旧的关键帧

删除旧的关键帧时，就比较麻烦。比如删除 ${\mathbf{x}}_1$ ，但 ${\mathbf{x}}_1$ 并不是孤立的，它会和其它帧观测到同样的路标，将 ${\mathbf{x}}_1$ 边缘化后将导致整个问题不再稀疏（破坏了路标部分的对角块结构）。

（3）滑动窗口法适合 VO 系统，而不适合大规模建图的系统。

9.2 位姿图

9.2.1 位姿图的意义

（1）随着时间的流逝，机器人的运动轨迹会越来越长，地图规模也会越来越大，BA 的计算效率就会下降。同时我们发现，经过若干次迭代后，收敛的特征点位置变化很小，发散的外点则已被剔除，因此在后续优化中没有必要再将收敛点考虑进来，而是只把他们当做位姿估计的约束。

（2）放开思路，我们完全可以构建一个只有轨迹的图优化，而位姿节点的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。一旦初始估计完成，就不再优化那些路标点的位置，而只关心相机位姿之间的联系。这样的方式，省去了大量特征点优化的计算，只保留了关键帧的轨迹，构建了所谓的 位姿图。

通过舍弃对路标点的优化，提高计算效率。

9.2.2 位姿图优化

位姿图中的节点表示相机位姿，用 ${\mathbf{T}}_1,{\mathbf{T}}_2,...,{\mathbf{T}}_n,$ 表示，边则是两个位姿节点之间相对运动的估计，这个估计可以通过特征点法或直接法得到。假设我们估计了 ${\mathbf{T}}_i$ 和 ${\mathbf{T}}_j$ 之间的相对运动 ${\mathbf{T}}_{ij}$，则有

$${\mathbf{T}}_i{\mathbf{T}}_{ij}={\mathbf{T}}_j$$

也即

$${\mathbf{T}}_{ij}={\mathbf{T}}_i^{-1}{\mathbf{T}}_j\text{\hspace{1em}(9-2)}$$

写成李代数形式

$${\mathbf{\xi }}_{ij}={\mathbf{\xi }}_i^{-1}\circ {\mathbf{\xi }}_j=\ln {\left({\mathbf{T}}_i^{-1}{\mathbf{T}}_j\right)}^{\vee }\text{\hspace{1em}(9-3)}$$

将式（9-2）中的 $\Delta {\mathbf{T}}_{ij}$ 移至方程右侧，则

$${\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}{\mathbf{T}}_j=\mathbf{I}\text{\hspace{1em}(9-4)}$$

但实际上，并不可能完全相等。定义

$${\mathbf{e}}_{ij}=\ln ({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}{\mathbf{T}}_j{)}^{\vee }\text{\hspace{1em}(9-5)}$$

我们需要优化的是 ${\mathbf{T}}_i$ 和 ${\mathbf{T}}_j$，也即 ${\mathbf{\xi }}_i$ 和 ${\mathbf{\xi }}_j$，因此需要求这两个变量关于 ${\mathbf{e}}_{ij}$ 的导数。分别左乘一个左扰动：${\mathbf{\delta \xi }}_i$ 和 ${\mathbf{\delta \xi }}_j$

$${\hat{\mathbf{e}}}_{ij}=\ln ({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}\exp ((-{\mathbf{\delta \xi }}_i{)}^{\wedge }\exp (({\mathbf{\delta \xi }}_j{)}^{\wedge }{\mathbf{T}}_j{)}^{\vee }\text{\hspace{1em}(9-6)}$$

根据伴随矩阵的性质：

$$\exp \left((\mathrm{Ad}(\mathbf{T})\mathbf{\xi }{)}^{\wedge }\right)=\mathbf{T}\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{T}}^{-1}\text{\hspace{1em}(9-7)}$$

稍作改变（把 $\mathrm{Ad}(\mathbf{T})$ 移到右侧，$ \boldsymbol{T}^{-1}$ 移到左侧）

$$\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)\text{\hspace{1em}(9-9)}$$

那么，式（9-6）可写为（从右往左化简）

$$\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)\underline{\exp \left(\delta {\mathbf{\xi }}_j^{\wedge }\right){\mathbf{T}}_j}\right)}^{\vee }\\ & =\ln {\left({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}\underline{\exp \left({\left(-\mathbf{\delta }{\mathbf{\xi }}_i\right)}^{\wedge }\right){\mathbf{T}}_j}\exp \left({\left(\mathrm{Ad}\left({\mathbf{T}}_j^{-1}\right)\mathbf{\delta }{\mathbf{\xi }}_j\right)}^{\wedge }\right)\right)}^{\vee }\\ & =\ln {\left({\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)\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 \mathbf{\delta }{\mathbf{\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}\text{\hspace{1em}(9-10)}$$

其中，第四步将两个指数一阶泰勒展开，相乘后舍去二次项；第四步到第五步则使用了 BCH 近似。

按照李代数上的求导法则，我们得到了误差关于两个位姿的雅克比矩阵，即

$$\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)\text{\hspace{1em}(9-11)}$$

${\mathcal{J}}_r$ 的形式比较复杂，通常取它的近似，

$${\mathcal{J}}_r^{-1}\left(e_{ij}\right)\approx \mathbf{I}+\frac{1}{2}\left[\begin{array}{cc}{\varphi }_e^{\wedge }& {\rho }_e^{\wedge }\\ 0& {\varphi }_e^{\wedge }\end{array}\right]\text{\hspace{1em}(9-12)}$$

了解雅克比求导后，剩下的部分就是普通的图优化。 记所有的边（也就是位姿）为 $\mathcal{E}$，则总体目标函数为

$$\min \frac{1}{2}\sum _{i,j\in \mathcal{E}}{\mathbf{e}}_{ij}^{\mathrm{T}}{\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{e}}_{ij}\text{\hspace{1em}(9-13)}$$

然后再用高斯牛顿法或 L-M 法优化求解。