从零手写VIO——（四）基于滑动窗口算法的 VIO 系统：可观性和一致性（下）滑动窗口算法_人工智能

滑动窗口算法

图优化基础

在十四讲中，我们已经接触了图优化以及如何使用 g2o 进行后端优化。

圆圈 Vertex：表示顶点，需要优化估计的变量。
边 Edge：表示顶点之间构建的残差。有一元边 BaseUnaryEdge，二元边 BaseBinaryEdge，多元边 BaseMultiEdge。

Example 1

从零手写VIO——（四）基于滑动窗口算法的 VIO 系统：可观性和一致性（下）滑动窗口算法

图模型：

其中：

这个问题很简单，五条边六个变量，最小化残差
$r$ 优化
$\xi$ 。 Gauss-Newton 增量方程：

这里的
$\Lambda=\Sigma^{-1}_{\text{new}}\ne \Sigma^{-1}$ 反映的是求解的方差（解
$\delta \xi$ 的方差），而
$\Sigma^{-1}$ 反映的是残差的方差（
$r=f(x)-z$ 里测量
$z$ 的噪声方差）。

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

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

我们提到的 Example 也是这样的：
$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}$ ，对应的信息矩阵 H 就是：

也是稀疏的。

利用边际概率移除老的变量^[1]

使用边际概率移除变量
$\xi_1$ ，信息矩阵的变化过程：

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

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

Example 2

可以将
$x_k$ 和
$x_{k+1}$ 在信息矩阵中调换位置，后使用边缘概率的求算公式，就能够得到答案。这是我推导手画的示意图：（不想自己画的就凑合着看吧）

滑动窗口中的 FEJ 算法

Example 3

若在 Example 1 的基础上构建一个新的变量
$\xi_7$ ：

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

加上
$\xi_7$ 后↓

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

图中系统在
$t\in[0,k]\text{s}$ 时刻，系统中状态量为
$\xi_i,i\in[1,6]$ 。在
$k'$ 时刻，加入了新的观测和状态量
$\xi_7$ 。
$k$ 时刻，最小二乘优化结束以后，marg 掉变量
$\xi_1$ 。被 marg 的状态量记为
$x_m$ ，剩余的变量
$\xi_i,i\in[2,5]$ 记为
$x_r$ 。marg 会使
$x_m$ 所有的变量和对应的测量信息丢弃并通过 marg 操作将部分信息传递给了保留变量
$x_r$ ，加入
$\xi_7$ 后，进行新的一轮优化。 marg 前，变量
$x_m$ 以及对应测量
$\cal S_m$ 构建的最小二乘信息矩阵为：

$b_m(k)$ 和
$\Lambda_m(k)$ 的形式一个是
$2×1$ 一个是
$2×2$ ，原因是
$\Lambda_m(k)$ 是两个雅可比相乘得到的是
$\begin{bmatrix}J_m&J_r\end{bmatrix}^T$ 与信息矩阵拼凑而成的方块矩阵，
$b_m(k)$ 是雅可比和残差
$r$ 相乘那就和雅可比
$\begin{bmatrix}J_m&J_r\end{bmatrix}^T$ 两项拼凑而成的很像长条的矩阵。 marg 后，变量
$x_m$ 的测量信息传递给了变量
$x_r$ ：

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

$-J^T\Sigma^{-1}r$ 对
$x_p$ 的导数就是
$-J^T\Sigma^{-1}J$ ，因为残差
$r$ 对更新量的导数就是雅可比。在
$k'$ 时刻，新残差
$r_{27}$ 和先验信息
$b_p(k),\Lambda_p(k)$ 以及残差
$r_{56}$ 构建新的最小二乘问题：

其中
$\Pi=\begin{bmatrix}I_{\text{dim}x_r}&0\end{bmatrix}$ 用来将矩阵的维度进行扩张，
$b_p(k)$ 是 prior 先验信息，由于上面提供它的维数问题，这里只做了一个维度扩张，对于(7)式子来说就是对行做扩张。对于
$\Lambda_p(k)$ 来说，它的维度需要同时对行和列都做扩张。
$\cal S_a$ 用来表示除被 marg 掉的测量以外的其他测量。

由于 marg 的变量及其对应的测量已被丢弃，先验信息
$\Lambda_p(k)$ 关于
$x_r$ 的雅可比在后续求解中被固定在丢弃前的样子无法更新。
$x_r$ 中不分辨率与其他残差有联系，
$\xi_2-\xi_7,\xi_5-\xi_6$ 。这些残差如
$r_{27}$ 对
$\xi_2$ 的雅可比会随着
$\xi_2$ 的迭代更新不断在新的线性化点出计算。这就导致了信息矩阵出现了两部分：一部分是已经固定雅可比的丢弃的变量和测量通过边际概率传递的；一部分是新加入变量的残差，是变化的。这导致信息矩阵的零空间发生变化，从而在求解时引入错误信息。

two way marginalization^[2]

当滑动窗口中第二新的图像帧为关键帧，则 marg 最老的帧，以及上面的路标点。
当第二新的图像帧不是关键帧，则丢弃这一帧的视觉测量信息，IMU 预积分不能丢弃需要传递给下一帧。

信息矩阵的零空间变化

求解单目视觉的 BA 问题，信息矩阵
$\Lambda$ 不满秩，缺少尺度维度，对应的零空间为
$N$ ，高斯牛顿求解时有：

$N$ 是零空间的解，可以看作
$\delta x$ 的另一种表达形式（在
$\Lambda \delta x=0$ 下的表达形式）。这样是解非齐次方程的通解（即非齐次方程的特解+齐次方程的通解）齐次通解就是这里的
$N$ 零空间。
$\delta x$ 在零空间下变化，并不会改变残差。这意味着可以有多个满足最小化损失函数的解
$x$ 。

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]：对于测量系统
$z=h(\theta)+\varepsilon$ ，其中
$z\in\Bbb R^n$ 为测量值，
$\theta\in\Bbb R^d$ 为系统状态量，
$\varepsilon$ 为测量噪声向量。
$h(·)$ 是个非线性函数，将状态量映射成测量。对于理想数据，如果以下条件成立，则系统状态量
$\theta$ 可观：

$\Lambda$ 矩阵不满秩
$\Leftrightarrow$ 信息矩阵
$\Lambda$ 存在零空间
$\Leftrightarrow$ 多个最小化损失函数的解
$\Leftrightarrow$ 状态量变化的条件下，测量和损失函数可能不改变。

单目 SLAM 系统 7 自由度不客观：6 自由度姿态（se3）+ 尺度（相机平移距离变化不会影响观测到的观测点变化）

单目 + IMU 系统是 4 自由度不可观：yaw 角（我常称它扭头角）+ 3 自由度位置不可观。roll 和 pitch 由于重力的存在可观，尺度因子由于加速度计的存在而可观。

投影误差得到的H，从相机视角里，只能得到相对的关系，相机在真实世界坐标系的位置是不知道的，除非添加新的信息；a维接近0，表示零空间有a个自由变量，无穷多解满足最小化重投影误差。为了解决不可观的问题，使用 FEJ（First Estimated Jacobian）算法，不同残差对同一个状态求雅可比时，线性化点必须一致。^[4]

solver 求解 trick

不满秩的信息矩阵 $H$ （如单目系统有七维零空间）（满秩矩阵的线性齐次方程有唯一零解，列满秩的最小二乘问题有唯一解）使用 LM 算法，加阻尼因子使得系统满秩，可求解，但结果可能向零空间变化。
添加先验约束，增加系统的可观性。比如 g2o tutorial 中对第一个 Pose 的信息矩阵加上单位阵 $H_{[11]^+}=I$ ，有在整体加上 $I\Delta x=0$ ，得出 $\Delta x=0$ 第一个Pose fix不变。
orbslam，svo等求 mono BA 问题时，fix 一个相机 pose 和一个特征点，或者 fix 两个相机 pose，为了限定优化值不乱飘。添加超强先验，使得对应的信息矩阵巨大，就能使 $\Delta x=0$ ；设定对应雅可比矩阵为 0，意味着信息矩阵 $H$ 和 $b=J^Tr$ 等于 0，求解 $(0+\lambda I)\Delta x=0$ 时，只有 $\Delta x=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

滑动窗口算法

滑动窗口中的 FEJ 算法

参考

您必须 登录 才能发表评论！

您必须登录才能发表评论！