随着 VSLAM 系统不断往新环境探索,就会有新的相机姿态以及看到新的环境特征,最小二乘残差就会越来越多,信息矩阵越来越大,计算量将不断增加。为了保持优化变量的个数在一定范围内,需要使用滑动窗口算法动态增加或移除优化变量。

在学习滑动窗口算法之前,推荐读者先阅读,高斯分布、协方差矩阵以及舒尔补的应用以及VSLAM中的非线性优化,这两篇博客奠定了本文的基础,有助于对本文的理解。

引例

我们知道,SLAM中的优化问题,就是优化一个最小二乘的问题,这里我们首先介绍一个引例。

如下图所示,存在这么一个图模型,对于图模型大家肯定不是很陌生,了解过g2o的都应该很清楚,图中圆圈表示顶点,是待优化的变量,而顶点之间的边,表示的顶点之间构建的残差。

image-20220513195856537

引自:Matthew R Walter, Ryan M Eustice, and John J Leonard. “Exactly sparse extended information filters for feature-based SLAM”.

对于这样一个系统,我们首先构建出最小二乘,如下:

$$ \xi^*=\underset{\xi}{\arg\min}\frac{1}{2}\sum_i\parallel r_i \parallel^2_{\Sigma_i}\tag{1} $$

其中,$\xi=[\xi_1,\xi_2,\xi_3,\xi_4,\xi_5,\xi_6]^T$,$r=[r_{12},r_{13},r_{14},r_{15},r_{56}]$

根据文章VSLAM中的非线性优化中提到的,SLAM优化问题,最终运动观测方程优化的部分是马氏距离,残差项即为马氏距离:

$$ \parallel r_i \parallel^2_{\Sigma_i}=r^T\Sigma^{-1}r\tag{2} $$

其中 $\Sigma$ 表示残差的协方差矩阵, $\Sigma^{-1}$ 表示残差的信息矩阵,因此,此处相当于一种加权的最小二乘问题。

应用高斯牛顿法,可以求解上述最小二乘问题的正规方程

$$ J^T\Sigma^{-1}J\ \delta\xi = -J^T\Sigma^{-1}\ r\tag{3} $$

其中海塞矩阵 $H=J^T\Sigma^{-1}J$,矩阵 $g= -J^T\Sigma^{-1}\ r$,雅可比矩阵为:

$$ J=\frac{\partial r}{\partial \xi}=\begin{bmatrix} \frac{\partial r_{12} }{\partial \xi}\\\frac{\partial r_{13} }{\partial \xi}\\ \frac{\partial r_{14} }{\partial \xi}\\\frac{\partial r_{15} }{\partial \xi}\\ \frac{\partial r_{56} }{\partial \xi} \end{bmatrix}=\begin{bmatrix}J_1\\J_2\\J_3\\J_4\\J_5\\J_6 \end{bmatrix}\tag{4} $$

其中,$\xi=[\xi_1,\xi_2,\xi_3,\xi_4,\xi_5,\xi_6]^T$,$r=[r_{12},r_{13},r_{14},r_{15},r_{56}]$

矩阵乘法公式 $(3)$ 可以写成连加:

$$ \sum^{5}_{i=1}J^T_i\Sigma^{-1}_i J_i\ \delta\xi=-\sum^{5}_{i=1}J^T_i\Sigma^{-1}_i\ r\tag{5} $$

由于每个残差只和某几个状态量有关,因此,雅克比矩阵求导时,无关项的雅克比为 0。比如

$$ J_2=\frac{\partial r_{13} }{\partial \xi}= \begin{bmatrix} \frac{\partial r_{13} }{\partial \xi_1}&0& \frac{\partial r_{13} }{\partial \xi_3}&0&0&0 \end{bmatrix}\tag{6} $$

对应的新的信息矩阵:

$$ Λ_2=J^T_2\Sigma^{-1}_2J_2=\begin{bmatrix} (\frac{\partial r_{13} }{\partial \xi_1})^T\Sigma^{-1}_2\frac{\partial r_{13} }{\partial \xi_1}&0&(\frac{\partial r_{13} }{\partial \xi_1})^T\Sigma^{-1}_2\frac{\partial r_{13} }{\partial \xi_3}&0&0&0\\0&0&0&0&0&0\\ (\frac{\partial r_{13} }{\partial \xi_3})^T\Sigma^{-1}_2\frac{\partial r_{13} }{\partial \xi_1}&0&(\frac{\partial r_{13} }{\partial \xi_3})^T\Sigma^{-1}_2\frac{\partial r_{13} }{\partial \xi_3}&0&0&0\\ 0&0&0&0&0&0\\0&0&0&0&0&0 \end{bmatrix}\tag{7} $$

同理,可以计算 $Λ_1 , Λ_3 , Λ_4 , Λ_5 $,并且也是稀疏的。

将五个残差的信息矩阵加起来,得到样例最终的信息矩阵 $Λ$,可视化如下:

image-20220513203817364

关于稀疏矩阵。我们知道,可以用舒尔消元法或者称之为边缘化,来使之变得稠密,减少计算量。

基于边际概率的滑动窗口算法

什么是滑动窗口算法?

什么是滑动窗口算法

在引例的条件下,我们知道,一般情况下,SLAM问题构建的残差的信息矩阵是一个稀疏矩阵,在此基础上,我们讨论SLAM中的滑动窗口算法。

所谓的滑动窗口算法,并没有明确的定义,我做了一个动图,其展示的过程就是滑动窗口算法:

cell

**为什么使用滑动窗口算法?

那么,使用它有什么好处呢?在文章开篇的概述中已经提及,这里再做强调:

  • 随着 VSLAM 系统不断往新环境探索,就会有新的相机姿态以及看到新的环境特征,最小二乘残差就会越来越多,信息矩阵越来越大,计算量将不断增加。
  • 为了保持优化变量的个数在一定范围内,需要使用滑动窗口算法动态增加或移除优化变量

其实它的作用也是动图中展示的,我们优化的变量就是红色框中的部分,外部的变量不断增加进来,一些历史的变量做清除。

滑动窗口中的关键问题是什么?

但是我们需要考虑的关键就是,如何增加或者移除?直接移除吗?比如引例中的变量 $\xi_1$如果直接把顶点连同边一起移除,那么所有的其他变量之间的边可就都没了,边就是误差项,误差都没了,如何优化?这是下面我们需要介绍的重要内容。

滑动窗口算法大致流程

① 增加新的变量进入最小二乘系统优化

② 如果变量数目达到了一定的维度,则移除老的变量。

③ SLAM 系统不断循环前面两步

基于边际概率的滑动窗口算法

关于这里,有必要回顾文章高斯分布、协方差矩阵以及舒尔补的应用中的案例,会对此有更加深刻的理解。

直接丢弃变量和对应的测量值,会损失信息。因此,我们需要选择一种“优雅的”方式丢掉历史信息,正确的做法是使用边际概率,将丢弃变量所携带的信息传递给剩余变量。意思就是,历史变量虽然被移除了,但是它的“影响力”还在。

还是用引例,我们将引例中的历史变量 $\xi_1$ 丢掉,应该如何操作呢?

引自:Matthew R Walter, Ryan M Eustice, and John J Leonard. “Exactly sparse extended information filters for feature-based SLAM”.

image-20220513210526896

很明显,去除历史变量 $\xi_1$ 之后,其他出现了互相之间的关联,这就是说历史变量 $\xi_1$ 虽然被去除了,但是它把自己的信息传递给其他变量了,虽然自己凉了,但是完成了火炬的传递。

结论:marginalization 会使得信息矩阵变稠密!原先条件独立的变量,可能变得相关。 另外,marg的这一特征也给我们启示,就是在SLAM运动观测模型中,marg特征点的时候,我们要marg那些不被其他帧观测到的特征点。因为他们不会显著的使得 H 变得稠密。对于那些被其他帧观测到的特征点,要么就别设置为marg,要么就宁愿丢弃,这就是okvis和dso中用到的一些策略。


那么,如果在移除变量 $\xi_1$ 的同时,加入新的变量 $\xi_7$,整个信息矩阵又如何变化呢?

如下图所示,在 $t \in [0, k]$s 时刻, 系统中状态量为 $\xi_i , i \in [1, 6]$。

在第 $k $ 时刻,最小二乘优化完以后,marg 掉变量 $\xi_1$ 。被 marg 的状态量记为 $x_m$ , 剩余的变量 $\xi_i , i \in [2, 5]$ 记为 $x_r $,marg 发生以后, $x_m$ 所有的变量以及对应的测量将被丢弃。同时,这部分信息通过 marg 操作传递给了保留变量 $x_r$ 。

第 $k^′$ 时刻,加入新的观测和状态量 $\xi_7$,新的变量 $\xi_7$ 跟老的变量 $\xi_2$ 之间存在观测信息,能构建残差 $r_{27}$ 。然后开始新一轮最小二乘优化。

image-20220513213728826

新残差加上之前 marg 留下的信息,构建新的最小二乘系统,对应的信息矩阵的变化如下图所示:

image-20220513212137697

注意: $\xi_2$ 自身的信息矩阵由两部分组成,一部分是原来的$Λ^{\prime}$ 中的老信息,另一部分是新生成的 $Λ_6$中的新信息,这会使得系统存在潜在风险

新老信息融合的问题在于旧的求解雅克比矩阵的变量线性化点和和新的求解雅克比矩阵的变量线性化点不同,可能会导致信息矩阵的零空间发生变化,使得不客观的变量变得可观,从而引入错误信息。对于零空间的介绍在后文中。


marg 留下的信息到底是啥?

marg 发生后,留下的到底是什么信息?

marg 前,变量 $x_m$ (指将要被 marg 的变量,$\xi_i,i=1$)以及对应测量 $\mathcal{S}_m$ (指的是与 $\xi_1$ 构成的边 $r_{1j},j\in[2,5]$)构建的最小二乘信息矩阵为(使用高斯牛顿):

$$ \begin{align*} b_m(k)&= \begin{bmatrix}b_{mm}(k)\\b_{mr}(k)\end{bmatrix} =-\sum_{(i,j)\in\mathcal{S}_m}J^T_{ij}(k)\Sigma^{-1}_{ij}r_{ij}\\ Λ_m(k)&= \begin{bmatrix}Λ_{mm}(k)&Λ_{mr}(k)\\Λ_{rm}(k)&Λ_{rr}(k)\end{bmatrix} =\sum_{(i,j)\in\mathcal{S}_m}J^T_{ij}(k)\Sigma^{-1}_{ij}J_{ij}(k) \end{align*}\tag{8} $$

然后marg掉 $\xi_1$ ,即丢掉变418133364量 $x_m$ , 而 $x_m$ 的测量信息,传递给了变量 $x_r$ (指的是 marg 剩余后的变量 $\xi_i,i\in[2,5]$):

$$ \begin{align*} b_p(k)&=b_{mr}(k)-Λ_{rm}(k)Λ^{-1}_{mm}(k)b_{mm}(k)\\ Λ_p(k)&=Λ_{rr}(k)-Λ_{rm}(k)Λ^{-1}_{mm}(k)Λ_{mm}(k) \end{align*}\tag{9} $$

下标 p 表示 prior. 即这些信息将构建一个关于 $x_r$ 的先验信息。

由:

$$ \begin{align*} Λ_p(k)\delta x&=b_p(k)\\ J^T_{p}(k)\Sigma^{-1}J_{p}(k)\ \delta x&= -J^T_{p}(k)\Sigma^{-1}r_p(k) \end{align*}\tag{10} $$

因此,我们可以从 $b_p(k),Λ_p(k)$反解出一个残差 $r_p(k)$ 和对应的雅可比矩阵 $J_{p}(k)$ ,需要注意的是,随着变量 $x_r(k)$ 的后续不断优化变化,残差 $r_p(k)$ 或者 $b_p(k)$ 也跟着变换,但是雅可比 $J_{p}(k)$ 则固定不变了。

在 $k^′$ 时刻,新残差 $r_{27}$ 和先验信息 $b_p(k),Λ_p(k)$ 以及残差 $r_{56}$ 构建新的最小二乘问题 :

$$ \begin{align*} b_m(k^′)&=\Pi^Tb_p(k) -\sum_{(i,j)\in\mathcal{S}_a(k^′)}J^T_{ij}(k^′)\Sigma^{-1}_{ij}r_{ij}(k^′)\\ Λ_m(k^′)&=\Pi^TΛ_p(k) \Pi +\sum_{(i,j)\in\mathcal{S}_a(k^′)}J^T_{ij}(k^′)\Sigma^{-1}_{ij}J_{ij}(k^′) \end{align*}\tag{11} $$

其中,$\Pi=[I_{dim\ x_r}\ \ 0]$用来将矩阵的维度进行扩张。 $\mathcal{S}_a$ 用来表示除被marg 掉的测量以外的其他测量,包括原本的与marg 变量无关的 $r_{56}$,以及新加入的变量 $r_{27}$。

出现的问题

由于 $\xi_2$ 的信息由两部分组成:

  • 一部分是marg 前的先验信息 $Λ_p(k)$ ,在marg 后,由于被 marg 的变量以及对应的测量已被丢弃,因此之前的雅可比不会被更新了,因此公式 $(11)$ 的等号右侧第一部分的线性化点是不变的
  • 另一部分信息就是新的边 $r_{27}$ ,其雅克比会随着 $\xi_2$ 的迭代更新而不断在最新的线性化点处计算,因此公式 $(11)$ 的等号右侧第二部分的线性化点是不断变化的
  • 旧的求解雅克比矩阵的变量线性化点和和新的求解雅克比矩阵的变量线性化点不同,它们在相加的时候,可能会导致信息矩阵的零空间发生变化,使得不客观的变量变得可观,从而引入错误信息

信息矩阵的零空间变化

滑动窗口算法优化的时候,信息矩阵如公式 $(11)$ 变成了两部分,且这两部分计算雅克比时的线性化点不同。这可能会导致信息矩阵的零空间发生变化,从而在求解时引入错误信息。

这里引用贺博的博客:SLAM中的marginalization 和 Schur complement

image-20220514165208897

在刘毅(稀疏毅),王京,晓佳等人讨论下,对这张图作出了如下解释:

四张能量图中,第一张是说明能量函数 $E$ 由两个同样的非线性函数 $E_1$和 $E_2$组成,我们令函数 $E=0$,这时方程的解为 $x y = 1$ ,对应图中深蓝色的一条曲线。

第二张能量函数图中的 $E_1^′$对应函数 $E_1$ 在点 $(0.5,1.4)$ 处的二阶泰勒展开;

第三张能量函数图中的 $E_2^′$ 对应函数在 $E_2$ 点 $(1.2,0.5)$ 处的二阶泰勒展开。注意这两个近似的能量函数 $E_1^′$ 和 $E_2^′$是在不同的线性点附近对原函数展开得到的。

最后一张图就是把这个近似得到的能量函数合并起来,对整个系统 $E$ 的二阶近似。

从第四个能量函数图中,我们发现一个大问题,能量函数为 $0$ 的解由以前的一条曲线变成了一个点,不确定性的东西变得确定了,专业的术语叫不可观的状态变量变得可观了,说明我们人为的引入了错误的信息。回到marg过程,上面这个例子告诉我们,marg 时,被 marg 的那些变量的雅克比已经不更新了,而此时留在滑动窗口里的其他变量的雅克比要用和 marg 时一样的线性点,就是 $FEJ,(first\ estimate\ jocabian)$ 算法,不要用新的线性点了。


FEJ 算法:$FEJ,(first\ estimate\ jocabian)$ ,不同残差对同一个状态求雅克比时,线性化点必须一致。这样就能避免零空间退化而使得不可观变量变得可观。 比如: 引例中计算 $r_{27}$ 对 $\xi_2$ 的雅克比时, $\xi_2$ 的线性话点必须和 $r_{12}$对其求导时一致。

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/15.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 08 月 03 日
赏杯咖啡