G2O和Sliding Window中的Marginalization

本文部分内容参考了以下博客：

https://blog.csdn.net/heyijia0327/article/details/52822104

https://blog.csdn.net/heyijia0327/article/details/53707261

摘要：本文介绍G2O和基于Sliding Window(SW)在边缘化技术的异同。简单来说，G2O的边缘化等同于Schur Complement(SC)；基于SW的边缘化，里面也用了SC来简化计算。G2O的边缘化主要依赖于Hessian矩阵的构造，本文将结合https://blog.csdn.net/heyijia0327/article/details/52822104的博客《SLAM中的marginalization 和 Schur complement》，详细介绍H矩阵的构造方法、结构特点和Marg前后H的变化。

一、Marg技术在BA问题理论应用

BA是用来优化SLAM中camera和landmark的约束问题。随着时间的增加，camera和landmark越来越多，BA优化的维度也越来越高，那如何减少BA优化的维度，就是所谓的边缘化技术。概括之，Marg技术主要用于解决因为BA问题中路标点和KeyFrame过多产生H维度过大的问题。ORB-SLAM使用的G2O来解决BA问题，G2O实际上是直接使用SC来简化了计算复杂度，所以G2O的边缘化等同于SC。

不同于ORB-SLAM，基于SW的边缘化更细致，它采用滑动窗口来控制优化的KeyFrame数量，从而控制H矩阵的维度，随着时间增加，新关键帧会进来滑动窗口时，旧关键帧需要被marg掉，然而又不能简单的去掉，否则就丢失信息，所以就需要在被marg关键帧之前的约束条件下，来marg旧关键帧，并加入新KeyFrame来优化。这个约束条件体现在之前优化的Hessian矩阵中，作为当前优化的先验。

二、详解G2O的Marginalization方法

2.1基于LM算法的BA

箭头矩阵开头

以下面的图示为例：

具体的LM算法流程不讲，直接进入LM算法中最后解方程的这一步：

其中J是Jacionbian矩阵，实际上就是Hessian矩阵，记，H矩阵的结构就是上图，它分成4个部分，红色的部分记为Hpp，表示每个相机的Hessian矩阵；蓝色的部分记为Hll，表示landmark的Hessian矩阵；右上角的绿色部分记为Hpl，左下角的绿色部分记为Hlp，实际上，表示路标点和对应的相机之间的关系，其中的绿色项的说明对应的路标点和相机之间存在约束。如此，H矩阵可以写成块矩阵的形式，并简化方程式：

显然上面的H矩阵存在着特殊的结构，和均是对角块矩阵(注意每个红色或者蓝色块并非对角矩阵)，正如G2O论文中所述，可以用Schur Complement来简化上述方程：

该变换过程即Schur Complement (自行查阅资料)，其中很容易计算，因为每个蓝色的小矩阵是3*3的，所以计算非常快，矩阵乘法和加减也很容易，所以得到上述形式计算很快，简化后的维度就是6*pose维，令，注意到H是对称矩阵(一般H不是稀疏的了)，所以可使用cholesky(或者LDLT)分解该矩阵，分解速度很快，一般400维的矩阵Eigen分解1ms，自己移植的代码需要2-3ms；分解后的矩阵都是对角阵和三角阵，所以计算很快，最后再求解：

这样求解就变得很容易了。

G2O实现上述过程实际上每个pose和landmark都参与了H矩阵的构造，最后通过SC简化矩阵来加速计算。个人认为G2O这种方式可以算是marginalization的一种方法，但和Marginalization的本意不一样。所以，ORB-SLAM中发现GlobalBA的时间会很长，但是LBA的时间实际上并不长，这是因为ORB-SLAM优化的是SpanningTree上的KeyFrame(即EssentialGraph上的KF)，它是由ORB-SLAM逻辑控制的，实际测试中发现，如果摄像头在原地小范围内移动，LBA优化的pose也是很多的。

那为什么说G2O这种方式也算是一种marginalization？(以下图关系为例)

图1是原始的H矩阵，右下角是pose相关部分，左上角是特征点相关部分。图2是把H矩阵中和pose1相关的部分移动到H矩阵的左上角。图3是marg掉pose1以后的H矩阵，用斜线标记的部分是待marg的特征点1，实际上marg以后的图3对应前面求时的矩阵。可以看到图3和图2中去掉斜线部分对比的话，变得稠密了。这里结合G2O的实现来讲下为什么会稠密。

用之前的H矩阵图例，考察 这个式子的 部分操作过程:

如果打算变换掉landmark2所在的行列，那landmark2所对应的Hpl和Hll矩阵块需要相互运算,并和Hpp做减法。记:

的逆矩阵为inv2，其对应的Hpl中绿色部分编号

对应的Hpp部分编号

运算规则如下：

Fori=1:2

       For j=i:2

             

       End For

EndFor

运算后Pose矩阵变成：

颜色加深说明该块矩阵进行了一次运算。整个H矩阵变成了：

显然本来稀疏的Hpp现在因为marg掉landmark2而使得P12被fill-in，变得稠密了，因为landmark2关联着pose1和pose2，所以P12被fill-in，同理，如果marg掉landmark2，因为3对应着pose1,pose2,pose3，Hpp会变成：

这里是marg掉landmark的方式，G2O是采用这种处理方式。同理可以可以去marg掉pose，当然可以猜测到结果会使得Hll部分被fill-in。现在回头看

2->3是marg掉pose1之后的各部分矩阵，与刚才的例子不同是，该图中pose1和pose2也存在着关联，因此发现Hpl的部分也被fill-in了

图结构就变成了

理解到这里，应该看heyijia的博客相关部分应该没困难了，marg landmark部分请阅读博客剩余部分。博客在该部分最后也说了“marg特征点1的这个过程这也给我们启示，就是marg特征点的时候，我们要marg那些不被其他帧观测到的特征点”，比如以红绿蓝矩阵为例，如果我们只marg掉landmark1，Hpp会变成：

只会加重P1的矩阵块颜色，整个Hpp仍然是对角矩阵块。

本节最后说下自己关于Hessian矩阵的理解，有的资料说是信息协方差矩阵，我们知道协方差矩阵描述了随机样本之间的关系(不相关和独立)，不相关一般是指两变量不线性相关。由统计知识可知，不相关不一定独立、独立一定不相关。独立是不相关的充分不必要条件。如此，我们考虑红绿蓝矩阵部分，可以看到ORB-SLAM在构造该矩阵的前提约束条件是每个pose之间、landmark之间和pose与landmark之间都是不相关的，marg过程中因为存在landmark连接关系，会使得pose与pose之间产生关联性。极端点，如果现在存在N个pose，都各自有N个landmark，之间无关系，那么marg过程就不存在fill-in，也就是每个pose之间不可能通过landmark产生关联。回想Harris检测最后也是用Hessian矩阵来判断一个点是否角点的方法：

>特征值都比较大时，即窗口中含有角点

>特征值一个较大，一个较小，窗口中含有边缘

>特征值都比较小，窗口处在平坦区域

特征比较大说明自相关强，就是说本点和区域其他点梯度差异大，那很可能就是角点；都比较小说明自相关弱，和区域内其他点的梯度差不多，可能就是平坦区域了；一大一小说明和一个方向相关性强，一个方向相关性弱(或者说梯度差一大一小->像素差一大一小)，那可能是边缘了。

三、关于基于sliding window的Marginalization一些理解

因为没有具体看源码实现，所以结合自己找的资料和理解，解读下基于sw的marg过程(之前请阅读https://blog.csdn.net/heyijia0327/article/details/53707261讲DSO的Marg部分，暂时可不看FEJ部分)。

上节最后说了Hessian矩阵即是协方差矩阵，它能表示出pose与pose之间的约束关系，如此基于SW的Marg就能用H矩阵来保存旧关键帧和被marg掉关键帧的约束关系，且能和新加入的关键帧进行优化(从第二部分的分析可知，最后求解的pose增量就是由H矩阵决定的)，随着时间的增加，当SW窗口marg旧关键帧时，只需要更新H矩阵即可，从而保证了SW内的优化关键帧的数量，又不丢失旧关键帧的约束关系。

 

四、总结

本文主要详细介绍了G2O的Marginalization的实现过程，并简单扩展到基于sliding window的Marginalization的不同。因为还没有对基于SW的marg代码深入研究，所以无法用数学表达式详细描述实现过程，后面有空可能会继续研究下。