SLAM【九】后端2

当机器人在更大范围的时间和空间运动时，由于算力不够及数据太多的原因，必须考虑一些解决方式：1. 滑动窗口：丢弃一些历史数据 2. 位姿图：舍弃对路标点的优化，只保留Pose之间的边。

滑动窗口滤波和优化

问题：需要控制BA的规模，保持计算的实时性，需要限制后端的计算时间。
控制计算规模的方法：
滑动窗口法：仅保留当前时刻最近的$N$个关键帧，去掉时间上更早的关键帧。即将BA固定在一个时间窗口内，离开这个窗口的关键帧将被丢弃。
共视图：按照某种原则，取时间上靠近，空间上又可以展开的关键帧，从而保证相机即使在 停止不动时，BA的结构也不至于缩成一团。与现在的相机存在共同观测的关键帧构成的图

滑动窗口法

现考虑一个滑动窗口，其中有$N$个关键帧和$M$个路标点,$N$个关键帧的位姿表示和$M$个路标点的世界坐标表示为：
$$x_1,...,x_N和y_1,...,y_M$$
根据这$N$个关键帧和$M$个路标可以组成一个局部地图，然后根据上一讲SLAM【八】后端1介绍的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)$$
其中${\mu }_k$为第$k$和关键帧的位姿均值，$\mathbf{\Sigma }$为所有关键帧的协方差矩阵，那么显然，均值部分就是指BA迭代后的结果，而$\mathbf{\Sigma }$就是对整个BA的$\mathbf{H}$矩阵进行边缘化后的$\mathbf{S}$矩阵。这里不懂 的可以看这里

在滑动窗口中，当窗口结构发生改变时，整个过程可以分成两部分：

1. 在窗口中新增一个关键帧，以及它观测到的路标点。
2. 把窗口中旧的关键帧删除，也可能删除它观测到的路标点。

新增一个关键帧和路标点
在这个过程中，并没有什么问题，按照正常的BA流程处理即可，对所有点进行边缘化时，得到这个N+1个关键帧的高斯分布参数。

删除一个旧的关键帧

遇到的问题：要删除旧关键帧$x_1$，但是$x_1$并不是孤立的，这一帧和其他帧观测到同样的路标。
解决：边缘化这一帧，但是因为它不是孤立的，所以会使整个问题不再稀疏。

在这里解释为什么是边缘化会删除一个不是孤立的帧
边缘化实际上在概率上的意义就是指条件概率。当我们边缘化某个关键帧，即“保持这个关键帧当前的估计值，求其他状态变量以这个关键帧为条件的条件概率”。
$$p\left(x_1,\dots x_4,y_1,\dots y_6\right)=p\left(x_2,\dots ,x_4,y_1\dots y_6\mid x_1\right)\underset{\text{舍去}}{\underbrace{p\left(x_1\right)}}$$
当某个关键帧被边缘化，它观测的路标就会产生一个“这些路标应该在哪里”的先验信息（其实是保持当前的估计值，在下一个优化过程中，不再优化这个变量了，这个变量其实是路标点的世界坐标系）。如果再边缘化这些路标点，那么它们将得到一个“观测它们的关键帧应该在哪里”（其实是确定了相机的姿态）

滑动窗口删除关键帧将破坏路标部分的对角结构（这里的删除就是边缘化一个关键帧）

边缘化$x_1$，Schur消元过程相当于通过矩阵行和列操作消去非对角线处几个非零矩阵块（这个非零矩阵块就是上一讲的$E和E^T$），显然这将导致右下角的路标点矩阵块不再是对角矩阵。称这个过程为填入(Fill-in)

当我们边缘化路标点时，Fill-in将出现在左上角的位姿块中。不过，因为BA不要求位姿块（左上角）为对角块，所以稀疏BA求解仍然可行，但是，当边缘化关键帧时，将破坏右下角路标点之间的对角块结构，这时BA就无法按照先前的稀疏方式迭代求解。

解决方案：对边缘化的过程进行改造，也可以保持滑动窗口BA的稀疏性。

1. 在边缘化某个旧的关键帧时，同时边缘化它观测到的路标点。这样，路标点的信息就会转换成剩下那些关键帧之间的共视信息，从而保持右下角部分的对角块结构。
2. 判断要边缘化的那个关键帧，它看到的路标点是否在最新的关键帧中仍能看到。如果不能，就直接边缘化这个路标点，如果能，就丢弃被边缘化关键帧对这个路边点的观测，从而保持BA的稀疏性。

位姿图

问题：经过若干次观测之后，收敛的特征点再进行观测优化，变化也不会太大，发散的外点也早就被剔除了，这时优化特征点的性价比已经不高了。
解决：优化几次后，把特征点固定住，只把它们看作位姿估计的约束，而不再实际的优化它们的位置估计了。

构建一个只有轨迹的凸优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。一旦初始估计完成，我们就不再优化那些路标点的位置，指保留了关键帧的轨迹，从而构建了所谓的位姿图。

位姿图的优化

节点表示相机位姿，以 $T_1,...,T_n$ 来表达。而边，则是两个位姿节点之间相对运动的估计，该估计可能来自于特征点法或直接法，也可以来着GPS或 IMU 积分。假设估计了$T_i$和$T_j$之间的一个运动$\Delta T_{ij}$。

$$\Delta {\mathbf{\xi }}_{ij}={\mathbf{\xi }}_i^{-1}\circ {\mathbf{\xi }}_j=\ln {\left(\exp \left({\left(-{\mathbf{\xi }}_i\right)}^{\wedge }\right)\exp \left({\mathbf{\xi }}_j^{\wedge }\right)\right)}^{\vee }$$

或者：

$$\Delta T_{ij}=T_i^{-1}{T}_j$$

按照图优化的思路，实际中上式不会精确成立，因此设立最小二乘误差，然后讨论误差关于优化变量的导数。把上式中的$\Delta T_{ij}$除到右边。定义误差$e_{ij}$：

$$\begin{array}{rl}{\mathbf{e}}_{ij}& =\ln {\left(\Delta {\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}{\mathbf{T}}_j\right)}^{\vee }\\ & =\ln {\left(\exp \left({\left(-\Delta {\mathbf{\xi }}_{ij}\right)}^{\wedge }\right)\exp \left({\left(-{\mathbf{\xi }}_i\right)}^{\wedge }\right)\exp \left({\mathbf{\xi }}_j^{\wedge }\right)\right)}^{\vee }\end{array}$$

对两个优化变量${\xi }_i$和${\xi }_j$求导。按照李代数的求导方式，给${\xi }_i$和${\xi }_j$各一个左扰动：$\delta {\xi }_i$和$\delta {\xi }_j$，则误差变为：

$${\hat{\mathbf{e}}}_{ij}=\ln {\left(\Delta {\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近似，希望把扰动项移至式子左侧或右侧，根据下面的式子，通过引入一个伴随项，能够“交换”扰动项左右侧的$T$，就可以将扰动挪到最右或者最左，
$$\exp \left((\mathrm{Ad}(\mathbf{T})\mathbf{\xi }{)}^{\wedge }\right)=\mathbf{T}\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{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)$$
导出最右或者最左的雅可比矩阵，这里导数的为最右。
$$\begin{array}{rl}{\hat{\mathbf{e}}}_{ij}& =\ln {\left(\Delta {\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 (\Delta {\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(\Delta {\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 }{\xi }_i}\mathbf{\delta }{\mathbf{\xi }}_i+\frac{\partial {\mathbf{e}}_{ij}}{\partial \mathbf{\delta }{\xi }_j}\mathbf{\delta }{\mathbf{\xi }}_j\end{array}$$
注意：约等于号这里使用的是泰勒展开，略去小项。
按照李代数的求导法则，求出了误差关于两个位姿的雅可比矩阵。
关于$T_i$
$$\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)$$

关于$T_j$
$$\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)$$

由于$se(3)$上的左右雅可比${\mathcal{J}}_r$形式过于复杂，我们通常取他们的近似。如果误差接近零，我们就可以设它们近似为$I$或者：

$${\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]$$

优化问题：优化变量为各个顶点的位姿，边来自于位姿观测约束。总体目标函数为：

$$\underset{\mathbf{\xi }}{\min }\frac{1}{2}\sum _{i,j\in \mathcal{E}}{\mathbf{e}}_{ij}^T{\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{e}}_{ij}$$

然后进行求解。

参考

1.视觉SLAM十四讲