滑动窗口算法

图优化基础
在十四讲中,我们已经接触了图优化以及如何使用 g2o 进行后端优化。
  • 圆圈 Vertex:表示顶点,需要优化估计的变量。
  • 边 Edge:表示顶点之间构建的残差。有一元边 BaseUnaryEdge,二元边 BaseBinaryEdge,多元边 BaseMultiEdge。
 
Example 1
图模型:
(1)
其中:
(2)
这个问题很简单,五条边六个变量,最小化残差 [公式] 优化 [公式] 。 Gauss-Newton 增量方程:
(3)
这里的 [公式] 反映的是求解的方差(解 [公式] 的方差),而 [公式] 反映的是残差的方差( [公式] 里测量 [公式] 的噪声方差)。
(4)
因为优化的变量是一致的,可以将五个增量方程写成连加:
(5)
在《十四讲》中,对 Sparse Matrix 有这样的描述:
我们提到的 Example 也是这样的: [公式] ,对应的信息矩阵 H 就是:
(6)
也是稀疏的。
利用边际概率移除老的变量[1]
使用边际概率移除变量 [公式] ,信息矩阵的变化过程:
丢弃变量所携带的信息使用边际概率传递给剩余变量。
想要 marg 掉的变量在矩阵的左上角,用 zhuanlan.zhihu.com/p/16 提到的用联合概率分布的信息矩阵求算边缘概率的信息矩阵,可以得出右边矩形框内的结果,相当于将变量 [公式] 固定住。原先 [公式] 是在 [公式] 发生的前提下,相互独立,但与 [公式] 间都存在联系,丢弃 [公式] 后,使得本和它相关的变量之间建立了新的联系,称作 fill-in。    
Example 2
可以将 [公式] 和 [公式] 在信息矩阵中调换位置,后使用边缘概率的求算公式,就能够得到答案。 这是我推导手画的示意图:(不想自己画的就凑合着看吧)

滑动窗口中的 FEJ 算法

Example 3
若在 Example 1 的基础上构建一个新的变量 [公式] :
添加前图模型和信息矩阵是如下这样↓
加上 [公式] 后↓
这种情况下, [公式] 自身的信息矩阵由两部分组成,这使得系统存在潜在风险
图中系统在 [公式] 时刻,系统中状态量为 [公式] 。在 [公式] 时刻,加入了新的观测和状态量 [公式] 。 [公式] 时刻,最小二乘优化结束以后,marg 掉变量 [公式] 。被 marg 的状态量记为 [公式] ,剩余的变量 [公式] 记为 [公式] 。marg 会使 [公式] 所有的变量和对应的测量信息丢弃并通过 marg 操作将部分信息传递给了保留变量 [公式] ,加入 [公式] 后,进行新的一轮优化。 marg 前,变量 [公式] 以及对应测量 [公式] 构建的最小二乘信息矩阵为:
(7)
(8)
[公式] 和 [公式] 的形式一个是 [公式] 一个是 [公式] ,原因是 [公式] 是两个雅可比相乘得到的是 [公式] 与信息矩阵拼凑而成的方块矩阵, [公式] 是雅可比和残差 [公式] 相乘那就和雅可比 [公式] 两项拼凑而成的很像长条的矩阵。 marg 后,变量 [公式] 的测量信息传递给了变量 [公式] :
(9)
这些信息将构建一个先验信息。从上式可以反解出一个残差 [公式] 和对应的雅可比矩阵 [公式]。随着变量 [公式] 的后续不断优化变化,残差 [公式] 或者 [公式] 也将跟着变化,但雅可比 [公式] 则固定不变了。 先验残差的变换用一阶泰勒展开近似:
(10)
[公式] 对 [公式] 的导数就是 [公式] ,因为残差 [公式] 对更新量的导数就是雅可比。 在 [公式] 时刻,新残差 [公式] 和先验信息 [公式] 以及残差 [公式] 构建新的最小二乘问题:
(11)
其中 [公式] 用来将矩阵的维度进行扩张, [公式] 是 prior 先验信息,由于上面提供它的维数问题,这里只做了一个维度扩张,对于(7)式子来说就是对 行 做扩张。对于 [公式] 来说,它的维度需要同时对  和  都做扩张。 [公式] 用来表示除被 marg 掉的测量以外的其他测量。  
由于 marg 的变量及其对应的测量已被丢弃,先验信息 [公式] 关于 [公式] 的雅可比在后续求解中被固定在丢弃前的样子无法更新。 [公式] 中不分辨率与其他残差有联系, [公式] 。这些残差如 [公式] 对 [公式] 的雅可比会随着 [公式] 的迭代更新不断在新的线性化点出计算。 这就导致了信息矩阵出现了两部分:一部分是已经 固定 雅可比的 丢弃 的变量和测量通过边际概率传递的;一部分是新加入变量的残差,是 变化 的。这导致信息矩阵的零空间发生变化,从而在求解时引入错误信息
two way marginalization[2]
  • 当滑动窗口中第二新的图像帧为关键帧,则 marg 最老的帧,以及上面的路标点。
  • 当第二新的图像帧不是关键帧,则丢弃这一帧的视觉测量信息,IMU 预积分不能丢弃需要传递给下一帧。

信息矩阵的零空间变化
求解单目视觉的 BA 问题,信息矩阵 [公式] 不满秩,缺少尺度维度,对应的零空间为 [公式] ,高斯牛顿求解时有:
(12)
[公式] 是零空间的解,可以看作 [公式] 的另一种表达形式(在 [公式] 下的表达形式)。这样是解非齐次方程的通解(即非齐次方程的特解+齐次方程的通解)齐次通解就是这里的 [公式] 零空间。   [公式] 在零空间下变化,并不会改变残差。这意味着可以有多个满足最小化损失函数的解 [公式] 。
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]对于测量系统 [公式] ,其中 [公式] 为测量值, [公式] 为系统状态量, [公式] 为测量噪声向量。 [公式] 是个非线性函数,将状态量映射成测量。对于理想数据,如果以下条件成立,则系统状态量 [公式] 可观:
(13)
[公式] 矩阵不满秩 [公式] 信息矩阵 [公式] 存在零空间 [公式] 多个最小化损失函数的解 [公式] 状态量变化的条件下,测量和损失函数可能不改变。
  • 单目 SLAM 系统 7 自由度不客观:6 自由度姿态(se3)+ 尺度(相机平移距离变化不会影响观测到的观测点变化)
  • 单目 + IMU 系统是 4 自由度不可观:yaw 角(我常称它扭头角)+ 3 自由度位置不可观。roll 和 pitch 由于重力的存在可观,尺度因子由于加速度计的存在而可观。
  投影误差得到的H,从相机视角里,只能得到相对的关系,相机在真实世界坐标系的位置是不知道的,除非添加新的信息;a维接近0,表示零空间有a个自由变量,无穷多解满足最小化重投影误差。 为了解决不可观的问题,使用 FEJ(First Estimated Jacobian)算法,不同残差对同一个状态求雅可比时,线性化点必须一致。[4]
solver 求解 trick
  1. 不满秩的信息矩阵 [公式] (如单目系统有七维零空间)(满秩矩阵的线性齐次方程有唯一零解,列满秩的最小二乘问题有唯一解)使用 LM 算法,加阻尼因子使得系统满秩,可求解,但结果可能向零空间变化。
  2. 添加先验约束,增加系统的可观性。比如 g2o tutorial 中对第一个 Pose 的信息矩阵加上单位阵 [公式] ,有在整体加上 [公式] ,得出 [公式] 第一个Pose fix不变。
  3. orbslam,svo等求 mono BA 问题时,fix 一个相机 pose 和一个特征点,或者 fix 两个相机 pose,为了限定优化值不乱飘。添加超强先验,使得对应的信息矩阵巨大,就能使 [公式]设定对应雅可比矩阵为 0,意味着信息矩阵 [公式] 和 [公式] 等于 0,求解 [公式] 时,只有 [公式] 。
  推荐阅读:
  1. 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.

参考

  1. ^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
  2. ^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
  3. ^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
  4. ^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