SLAM后端：滑动窗口滤波

1万 · 2023-3-20 16:26

实际环境下BA结构

随着路标点和帧（每帧都有一个相机位姿）的增多，后端优化（BA）的问题规模越来越大，如果不对BA规模进行控制，算力逐渐吃紧，无法保证SLAM实时性。

控制计算规模的做法很多，如选取关键帧，非关键帧仅用于定位（估计帧间位姿变换），关键帧用于定位和建图。但是这样只能使BA规模增长缓慢一些，无法真正做到控制BA规模在一定范围内。

还有一种简单思路是只保留离当前时刻最近的N个关键帧，将更早的关键帧丢弃，称为“滑动窗口法”。取这N个关键帧的策略不应该只看时间，如果只看时间的话，在相机静止不动一段时间后，BA的结构就缩成一团了，因此既要考虑时间上的靠近，也要考虑空间上可以展开，在ORB-SLAM2中，还要考虑“共视”，共视图是指与当前相机关键帧存在共同观测的关键帧构成的图，如仅优化与当前帧有20个以上的共视路标点的关键帧，其余固定不变，不做优化。

下面讨论：丢弃具体是指如何丢弃？被丢弃的帧是否会对保留帧的BA产生影响？

1万 · 2023-3-20 16:29

本帖最后由 keer_zu 于 2023-3-20 16:31 编辑

1.2 滑动窗口法
假设滑动窗口内的N个关键帧的位姿为：

在ch9中我们讨论过，因为含有噪声，我们将关键帧位姿看作是一种高斯分布，均值就是最有可能的位姿值，方差就是它的不确定度：

1万 · 2023-3-20 17:11

滑动窗口发生改变时，这些状态变量应该如何变化？分为两部分讨论：

新增一个关键帧，以及它观测到的路标点
删除一个旧的关键帧，可能删除它观测到的路标点。

如果是传统的BA，这是两个结构不同、互相独立的BA问题，但是在滑动窗口中，这是一个问题，需要讨论它具体的细节。

新增一个关键帧和路标点是平凡的，不必讨论。

1万 · 2023-3-20 17:23

删除一个旧的关键帧

如果我们只删除一个关键帧（位姿）而不删除它对路标点的观测，这将会破坏H矩阵的稀疏结构，导致无法边缘化加速求解。

括号内是我的理解：（注意到对于关键帧x1看到的路标点变量y1,y2,y3,y4，删除了某个关键帧之后，明明没有关键帧观测到它们，但是BA中仍然保留了这些对路标点的观测，这就产生了路标点的“先验信息”，破坏了原来的H的稀疏结构）

（我认为边缘化就是将某个未知量通过矩阵恒等变换的方式，令其暂时不参与计算，将其余的未知量求出后，被边缘化的量可以自然得到，因此将它暂时理解为删除也未尝不可）

因此，边缘化某个旧关键帧的同时，同时边缘化它观测到的路标点（假装所有关键帧都没看到或提取到这个关键点），可以保持H矩阵的稀疏结构。

在某些SLAM框架中，会采用更复杂的策略，在OKVIS中，需要判断最新的关键帧中是否仍能看到这些旧的关键帧观测路标点，如果能，就边缘化旧关键帧对这个路标点观测（假装这个帧从未被当做关键帧）；如果不能，就直接边缘化这个路标点（假装从未观察到或提取到这个路标点）。（我觉得OKVIS这样做是认为：如果新关键帧没有观测到这个路标点，很有可能在之后的一段时间内也观测不到，因此直接抛弃这个路标点，否则仍保留它。）

1万 · 2023-3-20 17:25

SWF(Sliding Window Filter)中边缘化的直观解释

边缘化在概率中的意义是条件概率，边缘化某个关键帧的含义就是：保持这个关键帧当前估计值，求其他状态变量以这个关键帧为条件的条件概率。因此当某个关键帧被边缘化，它所观测的路标点就会产生一个“这些路标应该在哪里”的先验信息，从而影响其余部分的估计值，如果再边缘化这些路标点，那么它们的观测者将得到一个“观测它们的关键帧应该在哪里”的先验信息。（这段话与我上一段理解是同一个意思，但是可能比较难懂。）

从数学上来看，边缘化某个关键帧就将整个窗口的状态变量由联合分布变成条件概率分布：

其中p(x1)舍去了，在工程中不再使用它。（等价于令p(x1)=1，因此我觉得也可以说把它删去了）

因为总是要把路标点边缘化（丢弃），所以滑动窗口比较适合VO系统（核心在于定位轨迹），而不适合大规模建图的系统（核心在于建图）。

1万 · 2023-3-20 17:27

位姿图的优化

位姿图的结构：

节点：相机位姿。

边：两个位姿节点之间的相对运动估计。（特征点法，直接法，GPS，IMU积分）

雅克比矩阵推导：（理清思路：我们求的是误差关于优化变量的导数）

左扰动模型：

交换法则（ch4习题）：

于是有

其中第二行到第三行是exp(X)exp(Y)在0向量处一阶Taylor展开。

显然扰动部分为小量，所以需要用右乘BCH近似： <span class="MathJax_SVG" id="MathJax-Element-10-Frame" tabindex="0" data-mathml="exp⁡(ξ∧)exp⁡(Δξ∧)≈exp⁡((Jr−1Δξ+ξ)∧)" role="presentation" style="display: inline-block; line-height: normal; font-size: 16px; word-spacing: normal; overflow-wrap: normal; white-space: nowrap; float: none; direction: ltr; max-width: none; max-height: none; min-width: 0px; min-height: 0px; border: 0px; position: relative;">exp⁡(�∧)exp⁡(Δ�∧)≈exp⁡((��−1Δ�+�)∧)

比较系数，就得到了关于Ti和Tj的导数：