VIO学习笔记（四）—— 基于滑动窗口算法的 VIO 系统:可观性和 一致性

学习资料是深蓝学院的《从零开始手写VIO》课程，对课程做一些记录，方便自己以后查询，如有错误还请斧正。由于习惯性心算公式，所以为了加深理解，文章公式采用手写的形式。

VIO学习笔记（一）—— 概述
VIO学习笔记（二）—— IMU 传感器
VIO学习笔记（三）—— 基于优化的 IMU 与视觉信息融合

本篇博客的主题

            
当维数变大时,求解计算量巨大。如何优雅的从多元高斯分布中丢弃变量?
$$p(x,y)\to p(x)orp(y)$$

从高斯分布到信息矩阵

SLAM 问题概率建模

考虑某个状态 $\xi$,以及一次与该变量相关的观测 $r_i$。由于噪声的存在,观测服从概率分布 $p(r_i|\xi )$。
多次观测时,各个测量值相互独立,则多个测量 $r=(r_1,...,r_n{)}^{\top }$ 构建的似然概率为:
$$p(r|\xi )=\prod _{i}p(r_i|\xi )$$
如果知道机器人状态的先验信息 p(ξ),如 GPS, 车轮码盘信息等,则根据 Bayes 法则,有后验概率:
$$p(\xi |r)=\frac{p(r|\xi )p(\xi )}{p(r)}$$
通过最大后验估计,获得系统状态的最优估计:
$${\xi }_{MAP}=arg\underset{\xi }{\max }p(\xi |r)$$

SLAM 问题求解

后验公式中分母跟状态量无关,舍弃。最大后验变成了:
$${\xi }_{MAP}=arg\underset{\xi }{\max }\prod _{i}p(r_i|\xi )p(\xi )$$
即
$${\xi }_{MAP}=arg\underset{\xi }{\min }[-\sum _i\log p(r_i|\xi )-\log p(\xi )]$$
如果假设观测值服从多元高斯分布:
$$p(r_i|\xi )=N({\mu }_i,{\Sigma }_i),p(\xi )=N({\mu }_{\xi },{\Sigma }_{\xi })$$
则有:
$${\xi }_{MAP}=arg\underset{\xi }{\min }\sum _i\Vert r_i-{\mu }_i{\Vert }_{{\Sigma }_i}^2+||\xi -{\mu }_{\xi }|{|}_{{\Sigma }_{\xi }}^2$$
这个最小二乘的求解可以使用Gauss-Newton的解法:
$$J^{\top }{\Sigma }^{-1}J\delta \xi =-J^{\top }{\Sigma }^{-1}r$$

高斯分布和协方差矩阵

多元高斯分布

零均值的多元高斯分布有如下概率形式:
$$p(x)=\frac{1}{Z}exp(-\frac{1}{2}{x}^{\top }{\Sigma }^{-1}x)$$
其中 Σ 是协方差矩阵,协方差矩阵的逆记作 Λ = Σ −1 .
比如变量 X 为三维的变量时,协方差矩阵为:
$$\Sigma =\left[\begin{array}{ccc}{\Sigma }_{11}& {\Sigma }_{12}& {\Sigma }_{13}\\ {\Sigma }_{21}& {\Sigma }_{22}& {\Sigma }_{23}\\ {\Sigma }_{31}& {\Sigma }_{32}& {\Sigma }_{33}\end{array}\right]$$
其中 ${\Sigma }_{ij}=E(x_i{x}_j)$ 为对应元素求期望。

toy example 1

                
设 x 2 为室外的温度,x 1 , x 3 分别为房间 1和房间 3 的室内温度:
$$\begin{gathered} x_2=v_2 \\ {x}_1=w_1{x}_2+v_1 \\ {x}_3=w_3{x}_2+v_3 \end{gathered}$$
其中,$v_i$ (从后面的公式来看，这个玩意很像($x-\mu$))相互独立,且各自服从协方差为${\sigma }_i^2$ 的高斯分布。

样例对应的状态量协方差矩阵

从上述关系,根据协方差公式的计算方式,我们可以写出 x 的协方差矩阵,先从对角元素开始计算:
    
同理有
              
对于协方差矩阵的非对角元素:
        
以此类推,可以得到整个协方差矩阵:
          

对应的协方差矩阵的逆呢?

通过计算联合高斯分布从而得到协方差矩阵的逆:

利用指数性质求出联合概率分布:

toy example 1对应的信息矩阵

由此得到协方差矩阵的逆,即信息矩阵:
        
注意:信息矩阵中有两个元素为 0 ,它有什么具体含义呢?协方差逆矩阵中如果坐标为 (i, j) 的元素为 0,表示元素 i 和 j 关于其他变量条件独立,上面的例子中意味着变量 $x_1$ 和 $x_3$ 关于 $x_2$ 条件独立。

协方差元素 vs 信息矩阵元素

假设室内温度和室外温度正相关$(w_i>0)$:
  协方差中非对角元素 ${\Sigma }_{ij}>0$ 表示两变量是正相关。
  信息矩阵中非对角元素为负数,甚至为 0。${\Lambda }_{12}<0$ 表示在变量$x_3$ 发生的条件下,元素 $x_1$ 和 $x_2$ 正相关。

toy example 2

                
比如特征三角化,两个相机 pose 得到特征三维坐标:
$$x_2=w_1{x}_1+w_3{x}_3+v_2$$
同理,根据协方差矩阵的定义,可以得到协方差矩阵:
         

协方差矩阵中非对角元素为 0 表示变量之间没有相关性。这是否意味着信息矩阵中也会为 0 呢?

toy example 2 协方差矩阵的逆

按照例子 1 中的方式,求取协方差矩阵的逆:

将变量整成向量形式:

toy example 2 中的有趣现象

从上面推导出的信息矩阵来看:
  虽然 $x_1$ 和 $x_3$不相关,但是不说明他们的信息矩阵对应元素 ${\Lambda }_{13}$为 0。
  恰恰信息矩阵中${\Lambda }_{13}>0$, 表示的是在变量 $x2$ 发生的条件下,变量 $x_1$ , $x_3$ 成负相关。
  对应上面的例子即 $x_2$ 为常数,如果 $x_1$ 大,则 $x_3$ 小。

疑问:如果我们移除变量,信息矩阵或协方差矩阵如何变化呢?

toy example 1 中去除变量 $x_3$

                

协方差如何变化?

利用协方差的计算公式可知,$x_1$ , $x_2$ 计算协方差时跟 $x_3$ ,并无关系,所以
          
就能得到去除 $x_3$ 后的协方差矩阵:
               

信息矩阵如何变化?

同样,我们只需要把信息矩阵公式中$x_3$ 对应的部分 (蓝色) 去掉就可以:
          
从而得到:
                
是不是非常简单?但是问题在于:实际操作过程中并不会有这种颜色标记。

这时,需要引入 marginalization (边缘化)和 Schur’s complement (舒尔补)来解决这个问题.

舒尔补应用:边际概率, 条件概率

舒尔补的概念

给定任意的矩阵块 M,如下所示:
$$M=\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]$$

1. 如果,矩阵块 D 是可逆的,则 $A-B{D}^{-1}C$ 称之为 D 关于 M的舒尔补。
2. 如果,矩阵块 A 是可逆的,则 $D-C{A}^{-1}B$ 称之为 A 关于 M的舒尔补。

舒尔补的来由

将 M 矩阵变成上三角或者下三角形过程中,都会遇到舒尔补:
        
其中:${∆}_A=D-C{A}^{-1}B$。联合起来,将 M 变形成对角形:

反过来,我们又能从对角形恢复成矩阵 M:
  

使用舒尔补分解的好处

快速求解矩阵 M 的逆:

矩阵 M 可写成:
  
由此可得矩阵 M 的逆:

因为:
          

舒尔补应用于多元高斯分布

toy example

假设多元变量 x 服从高斯分布,且由两部分组成:$X=\left[\begin{array}{c}a\\ b\end{array}\right]$,变量之间构成的协方差矩阵为:
$$K=\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]$$
其中 $A=cov(a,a)$, $D=cov(b,b)$, $C=cov(a,b)$. 由此变量 x 的概率分布为:

利用舒尔补一节公式, 对高斯分布进行分解,得:

这意味着我们能从多元高斯分布 $P(a,b)$ 中分解得到边际概率 $p(a)$ 和条件概率 $p(b|a)$。

关于 $P(a)$, $P(b|a)$ 的协方差矩阵

P(a) 的启示

            

启示:边际概率的协方差就是从联合分布中取对应的矩阵块就行了。

P(b|a) 的启示

    

启示: $P(b|a)\sim N(C{A}^{-1}a,{∆}_A)$。协方差变为 a 对应的舒尔补,均值也变了。

疑问三连:信息矩阵也是这样的吗? 如果不是,P(a) 的信息矩阵如何算? P(b|a) 的呢?

关于 P(a), P(b|a) 的信息矩阵

为什么要讨论 P (a), P (b|a) 的信息矩阵?

因为基于优化的 SLAM 问题中,我们往往直接操作的是信息矩阵,而不是协方差矩阵。所以,有必要知道边际概率,条件概率的信息矩阵是何形式。

P(a), P(b|a) 的信息矩阵

假设我们已知信息矩阵:
               
另外,由舒尔分解公式可知,协方差矩阵各块和信息矩阵之间有:

由条件概率 P (b|a) 的协方差为 ${∆}_A$ 以及上式,易得其信息矩阵为:
                              
由边际概率 P (a) 的协方差为 A ,易得其信息矩阵为:
                       

回顾 toy example 1 中去除变量 x3 的操作

        
从联合分布 $P(x_1,x_2,x_3)$ 中 marg 掉变量 $x_3$ ,即 $P(x_1,x_2)$ 对应的信息矩阵可以用边际概率 P (a) 的协方差公式得到。
           

总结

边际概率对于协方差矩阵的操作是很容易的,但不好操作信息矩阵。条件概率恰好相反,对于信息矩阵容易操作,不好操作协方差矩阵。表格总结如下 :

滑动窗口算法

toy example 3

最小二乘的图表示

                          

圆圈:表示顶点,需要优化估计的变量.
边:表示顶点之间构建的残差。有一元边(只连一个顶点),二元边,多元边…

有如下最小二乘系统,对应的图模型如有图所示:
$$\xi =arg\underset{\xi }{\min }\frac{1}{2}\sum _i\Vert r_i{\Vert }_{{\Sigma }_i}^2$$
其中
$$\xi =\left[\begin{array}{c}{\xi }_1\\ {\xi }_2\\ \dots \\ {\xi }_6\end{array}\right],r=\left[\begin{array}{c}{r}_{12}\\ r_{13}\\ r_{14}\\ r_{15}\\ r_{56}\end{array}\right]$$

最小二乘问题信息矩阵的构成

上述最小二乘问题,对应的高斯牛顿求解为:
                
注意,这里的 $\Lambda ={\Sigma }_{new}^{-1}\ne {\Sigma }^{-1}$ 反应的是求解的方差,而 ${\Sigma }^{-1}$ 反应的是残差的方差。另外,公式中的雅克比矩阵为:

矩阵乘法公式可以写成连加:
              

信息矩阵的稀疏性

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

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

信息矩阵组装过程的可视化

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

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

为什么 SLAM 需要滑动窗口算法?

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

滑动窗口算法大致流程

1. 增加新的变量进入最小二乘系统优化
2. 如果变量数目达到了一定的维度,则移除老的变量。
3. SLAM 系统不断循环前面两步

疑问:怎么移除老的变量?直接丢弃这些变量吗?

利用边际概率移除老的变量

直接丢弃变量和对应的测量值,会损失信息。正确的做法是使用边际概率,将丢弃变量所携带的信息传递给剩余变量。
下图为 toy expamle 3 中使用边际概率移除变量 ${\xi }_1$ ,信息矩阵的变化过程:

toy example 4

如下图优化系统中,随着 $x_{t+1}$ 的进入,变量 $x_t$ 被移除 。
      
      

marginalization 会使得信息矩阵变稠密!原先条件独立的变量,可能变得相关。

滑动窗口中的 FEJ 算法

toy example 3 再移除变量 ${\xi }_1$ 后加入新变量
                
新的变量 ${\xi }_7$ 跟老的变量 ${\xi }_2$ 之间存在观测信息,能构建残差 $r_{27}$ 。图模型如上图所示。
新残差加上之前 marg 留下的信息,构建新的最小二乘系统,对应的信息矩阵的变化如下图所示:

注意:${\xi }_2$ 自身的信息矩阵由两部分组成,这会使得系统存在潜在风险。

系统回顾 toy example 3

                  

红色为被 marg 变量以及测量约束。
绿色为跟 marg 变量有关的保留变量。
蓝色为和 marg 变量无关联的变量。

1. 如上图所示,在 $t\in [0,k{]}_s$ 时刻, 系统中状态量为 ${\xi }_i$ , $i\in [1,6]$。第 $k^{\prime }$时刻,加入新的观测和状态量 ${\xi }_7$ .
2. 在第 k 时刻,最小二乘优化完以后,marg掉变量 ${\xi }_1$ 。被 marg 的状态量记为 $x_m$ , 剩余的变量 ${\xi }_i$ , $i\in [2,5]$ 记为 $x_r$ .
3. marg 发生以后,$x_m$ 所有的变量以及对应的测量将被丢弃。同时,这部分信息通过 marg操作传递给了保留变量 $x_r$ . marg 变量的信息跟 ${\xi }_6$ 不相关。
4. 第 $k^{\prime }$ 时刻,加入新的状态量 ${\xi }_7$ (记作 $x_n$ ) 以及对应的观测,开始新一轮最小二乘优化。

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

marg 前,变量 $x_m$ 以及对应测量 $S_m$ 构建的最小二乘信息矩阵为(上文“最小二乘问题信息矩阵的构成”有提到):
    
marg 后,变量 $x_m$ 的测量信息传递给了变量 $x_r$ (舒尔补):
        
下标 p 表示 prior. 即这些信息将构建一个关于 $x_r$ 的先验信息。

先验：
我们可以从 $b_p(k)$, ${\Lambda }_p(k)$ 中反解出一个残差 $r_p(k)$ 和对应的雅克比矩阵 $J_p(k)$. 需要注意的是,随着变量 $x_r(k)$ 的后续不断优化变化,残差$r_p(k)$ 或者 $b_p(k)$也将跟着变化,但雅克比 $J_p(k)$ 则固定不变了。

新测量信息和旧测量信息构建新的系统

在 $k^{\prime }$ 时刻,新残差 $r_{27}$ 和先验信息 $b_p(k)$, ${\Lambda }_p(k)$ 以及残差 $r_{56}$ 构建新的最小二乘问题:
    
其中 $\Pi =[I_{dim{x}_r}0]$ 用来将矩阵的维度进行扩张。$S_a$ 用来表示除被marg 掉的测量以外的其他测量,如 $r_{56}$ , $r_{27}$ 。

出现的问题:
由于被 marg 的变量以及对应的测量已被丢弃,先验信息 ${\Lambda }_p(k)$中关于 $x_r$ 的雅克比在后续求解中没法更新。
但 $x_r$ 中的部分变量还跟其他残差有联系, 如 ${\xi }_2$ , ${\xi }_5$ 。这些残差如$r_{27}$ 对 ${\xi }_2$ 的雅克比会随着 ${\xi }_2$的迭代更新而不断在最新的线性化点处计算。

信息矩阵的零空间变化

滑动窗口算法导致的问题

滑动窗口算法优化的时候,信息矩阵变成了两部分,且这两部分计算雅克比时的线性化点不同。这可能会导致信息矩阵的零空间发生变化,从而在求解时引入错误信息。
比如: 求解单目 SLAM 进行 Bundle Adjustment 优化时,问题对应的信息矩阵 Λ 不满秩,对应的零空间为 N, 用高斯牛顿求解时有
                
咦,增量 δx 在零空间维度下变化,并不会改变我们的残差。这意味着系统可以有多个满足最小化损失函数的解 x。

toy example

多个解的问题,变成了一个确定解。不可观的变量,变成了可观的。

可观性的一种定义

对于测量系统 $z=h(\theta )+\epsilon$, 其中 $z\in R^n$ 为测量值,$\theta \in R^d$ 为系统状态量,ε 为测量噪声向量。$h(\cdot )$ 是个非线性函数,将状态量映射成测量。对于理想数据,如果以下条件成立,则系统状态量 θ 可观:
        
对于 SLAM 系统而言(如单目 VO),当我们改变状态量时,测量不变意味着损失函数不会改变,更意味着求解最小二乘时对应的信息矩阵 Λ 存在着零空间。
单目 SLAM 系统 7 自由度不可观:6 自由度姿态 + 尺度。
      
单目 + IMU 系统是 4 自由度不可观:yaw 角 + 3 自由度位置不可观。roll 和 pitch 由于重力的存在而可观,尺度因子由于加速度计的存在而可观。

滑动窗口中的问题

滑动窗口算法中,对于同一个变量,不同残差对其计算雅克比矩阵时线性化点可能不一致,导致信息矩阵可以分成两部分,相当于在信息矩阵中多加了一些信息,使得其零空间出现了变化。

解决办法:First Estimated Jacobian

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

相关资料：

1. David Mackay. “The humble Gaussian distribution”. In: (2006).
2. Huang. Conditional and marginal distributions of a multivariate Gaussian. https://gbhqed.wordpress.com/2010/02/21/conditional-and-marginal-distributions-of-a-multivariate-gaussian/
3. Matthew R Walter, Ryan M Eustice, and John J Leonard. “Exactly sparse extended information filters for feature-basedSLAM”. In: The International Journal of Robotics Research 26.4 (2007), pp. 335–359.
4. Claude Jauffret. “Observability and Fisher information matrix in nonlinear regression”.In: IEEE Transactions on .Aerospace and Electronic Systems 43.2 (2007), pp. 756–759.
5. Tue-Cuong Dong-Si and Anastasios I Mourikis. “Consistency analysis for sliding-window visual odometry”. In: 2012 IEEE International Conference on Robotics and Automation. IEEE. 2012, pp. 5202–5209.