VIO中因子图的运用方式(以ICE和VI-DSO为例)

本来想写篇简单的科普，后来想想还是认真写篇有货的，还是对初学者不太友好。

正常随着SLAM，VIO或别的多传感器融合融态工作的深入，因子图的运用和落地会成为核心中的核心。要做好SLAM，必须掌握因子图运用。最好天天画:)

否则很难从代码和工程关联中完成对应工作，首先感谢SLAM大神们的paper让我们意识到了因子图的重要！

先举一个ICE-BA的例子(来自于崔华坤博士的ICE解析，全文较复杂写得很棒，节选和简化个人理解)：

首先得明白，常用的SLAM或VIO因子中，顶点往往是相机或路标，边是残差，在一些进阶的系统中顶点会有些变化。

这是个最简单的例图，T代表相机位姿，X代表路标。

小红方块代表残差关联3个因子，到中间例图去掉了它转换为了6个影响(在各自的顶点与边)，最后Margin掉路标形成了标准的位姿图Pose Graph，体现出共视关系，这在纯V-SLAM中是基础知识。

这张的复杂度就上升了，但仍然一目了然，到了这个阶段就一定会涉及具体因子图Factor Garph的具体落地了。

在这个阶段一定会涉及到SLAM中最重要的2类矩阵，雅克比Jacobian矩阵(残差矩阵)和Hession矩阵(信息矩阵)。其中每一个数值就代表了因子图中的每一个边。顶点在行与列体现。处理这2个矩阵最关键的因素就是维度。

这时，如果我们要边缘化Maginalized某个帧如Th1，然后再边缘化掉所有路标，并保留先验：

新的关系就这么生成了，注意每一个对应数字，可以自己数一数边。最后一张就是边缘化全部完成的路标图。这里最重要的是：当你每Margin一个顶点，就会在关联顶点生成一条对应的新边(新的信息，作为先验)

最后的矩阵如下：

如果不做因子图会如何呢？ 随便列点边缘化或残差的影响，就得处理如下一大堆复杂的雅克比式子。。。草稿纸都不够用，还很容易出错：

 接下来举VI-DSO例子，这个就非常清爽了：

从上图可见几类关键的因子：

ai,bi,di：直接将2个光度误差和1个逆深度di打包成因子(嗯，这么做也是可以的)，也可以对应到所有。

然后直接指定了联合视觉因子(红块)

定义了先验约束因子，然后3个主组因子：Pose/线速度/Bias，再用2种块描述了IMU因子和Bias随机游走。(这里使用线速度是VI-DSO常用方式)

最下面那个因子是尺度/重力联合因子，因为是讲因子图应用，这个不多提了。

从这2张因子图的递进，可以一目了然当P1被Marginalized之后，边缘化M因子给整个系统带来的全部影响！对照因子图就可以认真去分析整个Jacobian矩阵(残差矩阵)与Hession矩阵的构成了！。