滑动窗口算法
图优化基础
在十四讲中,我们已经接触了图优化以及如何使用 g2o 进行后端优化。
- 圆圈 Vertex:表示顶点,需要优化估计的变量。
- 边 Edge:表示顶点之间构建的残差。有一元边 BaseUnaryEdge,二元边 BaseBinaryEdge,多元边 BaseMultiEdge。
Example 1

图模型:

其中:

这个问题很简单,五条边六个变量,最小化残差 优化
。 Gauss-Newton 增量方程:

这里的 反映的是求解的方差(解
的方差),而
反映的是残差的方差(
里测量
的噪声方差)。

因为优化的变量是一致的,可以将五个增量方程写成连加:

在《十四讲》中,对 Sparse Matrix 有这样的描述:


我们提到的 Example 也是这样的: ,对应的信息矩阵 H 就是:

也是稀疏的。
利用边际概率移除老的变量[1]
使用边际概率移除变量 ,信息矩阵的变化过程:


丢弃变量所携带的信息使用边际概率传递给剩余变量。

想要 marg 掉的变量在矩阵的左上角,用 https://zhuanlan.zhihu.com/p/166663184 提到的用联合概率分布的信息矩阵求算边缘概率的信息矩阵,可以得出右边矩形框内的结果,相当于将变量 固定住。原先
是在
发生的前提下,相互独立,但与
间都存在联系,丢弃
后,使得本和它相关的变量之间建立了新的联系,称作 fill-in。
Example 2


可以将 和
在信息矩阵中调换位置,后使用边缘概率的求算公式,就能够得到答案。 这是我推导手画的示意图:(不想自己画的就凑合着看吧)

滑动窗口中的 FEJ 算法
Example 3
若在 Example 1 的基础上构建一个新的变量 :

添加前图模型和信息矩阵是如下这样↓

加上 后↓

这种情况下, 自身的信息矩阵由两部分组成,这使得系统存在潜在风险。

图中系统在 时刻,系统中状态量为
。在
时刻,加入了新的观测和状态量
。
时刻,最小二乘优化结束以后,marg 掉变量
。被 marg 的状态量记为
,剩余的变量
记为
。marg 会使
所有的变量和对应的测量信息丢弃并通过 marg 操作将部分信息传递给了保留变量
,加入
后,进行新的一轮优化。 marg 前,变量
以及对应测量
构建的最小二乘信息矩阵为:


和
的形式一个是
一个是
,原因是
是两个雅可比相乘得到的是
与信息矩阵拼凑而成的方块矩阵,
是雅可比和残差
相乘那就和雅可比
两项拼凑而成的很像长条的矩阵。 marg 后,变量
的测量信息传递给了变量
:

这些信息将构建一个先验信息。从上式可以反解出一个残差 和对应的雅可比矩阵
。随着变量
的后续不断优化变化,残差
或者
也将跟着变化,但雅可比
则固定不变了。 先验残差的变换用一阶泰勒展开近似:

对
的导数就是
,因为残差
对更新量的导数就是雅可比。 在
时刻,新残差
和先验信息
以及残差
构建新的最小二乘问题:

其中 用来将矩阵的维度进行扩张,
是 prior 先验信息,由于上面提供它的维数问题,这里只做了一个维度扩张,对于(7)式子来说就是对 行 做扩张。对于
来说,它的维度需要同时对 行 和 列 都做扩张。
用来表示除被 marg 掉的测量以外的其他测量。
由于 marg 的变量及其对应的测量已被丢弃,先验信息
关于
的雅可比在后续求解中被固定在丢弃前的样子无法更新。
中不分辨率与其他残差有联系,
。这些残差如
对
的雅可比会随着
的迭代更新不断在新的线性化点出计算。 这就导致了信息矩阵出现了两部分:一部分是已经 固定 雅可比的 丢弃 的变量和测量通过边际概率传递的;一部分是新加入变量的残差,是 变化 的。这导致信息矩阵的零空间发生变化,从而在求解时引入错误信息。
two way marginalization[2]
- 当滑动窗口中第二新的图像帧为关键帧,则 marg 最老的帧,以及上面的路标点。
- 当第二新的图像帧不是关键帧,则丢弃这一帧的视觉测量信息,IMU 预积分不能丢弃需要传递给下一帧。

信息矩阵的零空间变化
求解单目视觉的 BA 问题,信息矩阵 不满秩,缺少尺度维度,对应的零空间为
,高斯牛顿求解时有:

是零空间的解,可以看作
的另一种表达形式(在
下的表达形式)。这样是解非齐次方程的通解(即非齐次方程的特解+齐次方程的通解)齐次通解就是这里的
零空间。
在零空间下变化,并不会改变残差。这意味着可以有多个满足最小化损失函数的解
。

never combine linearizations around different linearization points, especially in the presence of non-linear nullspaces! It will render unobservable demensions observable, and corrupt the system.
多个解的问题,变成了一个确定解。不可观的变量变成了可观的。 可观性[3]:对于测量系统 ,其中
为测量值,
为系统状态量,
为测量噪声向量。
是个非线性函数,将状态量映射成测量。对于理想数据,如果以下条件成立,则系统状态量
可观:

矩阵不满秩
信息矩阵
存在零空间
多个最小化损失函数的解
状态量变化的条件下,测量和损失函数可能不改变。
- 单目 SLAM 系统 7 自由度不客观:6 自由度姿态(se3)+ 尺度(相机平移距离变化不会影响观测到的观测点变化)

- 单目 + IMU 系统是 4 自由度不可观:yaw 角(我常称它扭头角)+ 3 自由度位置不可观。roll 和 pitch 由于重力的存在可观,尺度因子由于加速度计的存在而可观。
投影误差得到的H,从相机视角里,只能得到相对的关系,相机在真实世界坐标系的位置是不知道的,除非添加新的信息;a维接近0,表示零空间有a个自由变量,无穷多解满足最小化重投影误差。 为了解决不可观的问题,使用 FEJ(First Estimated Jacobian)算法,不同残差对同一个状态求雅可比时,线性化点必须一致。[4]
solver 求解 trick
- 不满秩的信息矩阵
(如单目系统有七维零空间)(满秩矩阵的线性齐次方程有唯一零解,列满秩的最小二乘问题有唯一解)使用 LM 算法,加阻尼因子使得系统满秩,可求解,但结果可能向零空间变化。
- 添加先验约束,增加系统的可观性。比如 g2o tutorial 中对第一个 Pose 的信息矩阵加上单位阵
,有在整体加上
,得出
第一个Pose fix不变。
- orbslam,svo等求 mono BA 问题时,fix 一个相机 pose 和一个特征点,或者 fix 两个相机 pose,为了限定优化值不乱飘。添加超强先验,使得对应的信息矩阵巨大,就能使
;设定对应雅可比矩阵为 0,意味着信息矩阵
和
等于 0,求解
时,只有
。
推荐阅读:
- Grisetti, G. , et al. “A Tutorial on Graph-Based SLAM.”IEEE Intelligent Transportation Systems Magazine2.4(2011):31-43.
2. Kummerle, Rainer , et al. “G2o: A general framework for graph optimization.”IEEE International Conference on Robotics & AutomationIEEE, 2011. 3. LOURAKIS,M. I. A. “SBA: A software package for generic sparse bundle adjustment.”Acm Trans.math.softw36.1(2009):2.
参考
- ^Walter M R , Eustice R M , Leonard J J . Exactly Sparse Extended Information Filters for Feature-based SLAM[J]. Int.j.robotics Res, 2007, 26(4):335-359. https://deepblue.lib.umich.edu/bitstream/handle/2027.42/86031/mwalter-22.pdf?sequence=1
- ^Tong Q , Peiliang L , Shaojie S . VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[J]. IEEE Transactions on Robotics, 2017, PP(99):1-17. https://arxiv.org/pdf/1708.03852.pdf
- ^Jauffret C . Observability and fisher information matrix in nonlinear regression[J]. Aerospace & Electronic Systems IEEE Transactions on, 2007, 43(2):756-759. https://hal-univ-tln.archives-ouvertes.fr/hal-01820468/document
- ^Dong-Si T C , Mourikis A I . Consistency analysis for sliding-window visual odometry[J]. Proceedings – IEEE International Conference on Robotics and Automation, 2012:5202-5209. https://intra.ece.ucr.edu/~mourikis/papers/DongSi2012-ICRA.pdf