深⼊理解SLAM中的Marginalization
写在前⾯
本⽂主要是对于前半部分的解读，论⽂可谓是短⼩精悍，个⼈花了很多时间去学习和理解，⽂章中不乏会有⼀些理解不到位的情况，欢迎各位⼤神们拍砖，⼀起学习进步。
Abstract
核桃去壳机
基于图优化的SLAM⽅法已经取得了成功，但是图的⼤⼩会随着时间的增长⽽变得很⼤，从⽽变得computational costs。为了解决这个问题，论⽂使⽤分开的边缘化和稀疏化解决这个问题。同时，论⽂还做了以下两个⼯作： 1. 论⽂验证了在边缘化的时候，构建密集因⼦时使⽤的线性化点要使⽤Markov blanket的相对位姿，⽽不是绝对位姿；
2. 在稀疏化的时候，论⽂通过在线的⽅法来确定最相似的稀疏结构，⽽不是通过离线的结构选择； Introduction
⾸先在实践中，有两种⽅法——Generic LInear Contraints(GLC)和Nonlinear Factor Recovery(NFR)。但是这两个⽅法都是将边缘化和稀疏化是耦合的，但是这种⽅法并不能保证所有的信息都能保存在稠密因⼦中，主要原因是因为边缘化和稀疏化都在同⼀时刻的话，稀疏化的线性化点并不是边缘化之后的线性化点，也就是说，稀疏化的时候并没有最⼤程度的利⽤稠密因⼦的信息。 所以在这个论⽂中，作者将这两个操作解耦了，在边缘化之后进⾏稀疏化，保证可以利⽤边缘化留下的信息。最后，在稀疏化的过程中主要构建了⼀个稀疏正则的凸优化来在线决定如何进⾏图的稀疏。本⽂的⼯作主要有：
在局部图上（Markov blanket）进⾏边缘化和稀疏化。在边缘化的时候，在局部地图上到⼀个参考点，并求解最⼤似然问题，相⽐于使⽤全局的位姿，这样的⽅法才能保证⼀致性。边缘化之后的分布将以⾮线性测量的形式为后续的优化提供先验信息； 操作的解耦⼀⽅⾯是为了更好的利⽤边缘化信息，另⼀⽅⾯是为了更好的分散计算量。在稀疏化的⼯程中，⾸先对稠密分布的内部团（团概念参考图模型的相关⽂章）因⼦进⾏稀疏化（就是与被边缘化帧相关的帧），注意这些约束要在稀疏化之前被排除出来，这样保证稀疏化之前能对这些测量进⾏再次的线性化（相当于也没有丢掉，只是以另⼀种形式存在了）；然后构建⼀个L1正则凸优化来决定最终要优化的稀疏结构，这个结构旨在获得⼀组最好的反应Markov blanket中全部信息的稀疏测量值，然后以此来代替稠密因⼦；
PS: Markov blanket
直译为马尔科夫毯⼦，也有翻译为马尔科夫覆盖域，⼀个例⼦如下图1。因为这个概念是全⽂得以推导的基础，因此这⾥有必要着重说⼀下：
1. wiki百科对Markov blanket解释为：“the Markov blanket for a in a contains all the variables that shield the node from the rest of
the network. This means that the Markov blanket of a node is the only knowledge needed to predict the behavior of that node and its children”，我个⼈的理解是说Markov blanket要包含⼀些node，这些node是为了做预测所需要的全部信息，⽽⽹络中的其他信息都可以被屏蔽。
2. 在下图中，可以看到如果我们以节点A来建⽴⼀个Markov blanket，那么此时A的⽗节点、兄弟节点以及⼦节点都是影响A预测的因素，因此他们 和A⼀起构成了⼀个Markov blanket。
3. 下⾯的例⼦其实是⼀个有向图，SLAM中的节点属于⽆向图，因此构建Markov blanket的时候，不需要考虑兄弟节点。
Related Work
为了解决基于图的SLAM系统的计算量，⼈们尝试了很多种⽅法，但是边缘化是⼀种最稳妥的⽅法，因为这样可以很好的保留之前的信息不被丢失，但是边缘化之后会导致图变得稠密，但是可以通过近似的⽅法减少这部分填充。
GLC ⽅法在边缘化节点的markov blanket上应⽤边缘化，但使⽤了内部团的测量，从⽽产⽣n元线性约束来近似覆盖域。 但是，这些线性因⼦是在全局状态下被估计的，这可能导致测量的不⼀致性。
NFR⽅法不同，他的因⼦并不限制为线性的。这些因⼦的信息矩阵是通过最⼩化近似边缘分布与原始边缘分布的KL散度来获得的。除了使⽤全局的状态来进⾏线性化，作者也尝试了局部状态的估计作为
线性化点，但是作者并没有严格的论证。
上述两个⽅法有⼀个共同的特点，就是他们的近似都是采⽤预先决定的稀疏结构对边进⾏加权的⽅法，并不是在线的计算这个稀疏的结构。⽂章为了解决上述的问题，⾸先是把边缘化和稀疏化进⾏了解耦，其次就是在线的计算稀疏结构，最后就是论证了使⽤局部的估计是最好的办法。
Graph-based SLAM
这个章节就是简单介绍了⼀下整个基于图优化的SLAM的⽅法，主要是下⾯三个公式：
1. 观测⽅程，观测认为是满⾜⼀个⾼斯分布
2. 最⼤似然问题：
3. 最⼤似然转换为⾮线性最⼩⼆乘问题，使⽤⾼斯⽜顿⽅法如下：
4. 更新状态变量：
Marginalization
香皂包装⾸先构建⼀个Markov blanket，其中以为中⼼节点，如下图2：
z =ij h x ,x +ij (i j )n ij
=x ^arg z −h x ,x x min (i ,j )∈supp(z )∑∥ij ij (i j )∥Λij 2(1)
δx =(k )arg z −h ,−J δx δx min (i ,j )∈supp(z )∑∥∥∥ij ij (x ^i (k )x ^j (k ))ij (k )∥∥∥Λij 2(2)
=x ^(k +1)+x ^(k )δx (k )
X 1
其中：
红⾊的顶点代表被边缘化的帧，表⽰为；
绿⾊的顶点代表关联帧，表⽰为；
蓝⾊的顶点表⽰⽆关帧，表⽰为；
红⾊的测量表⽰与边缘化帧相关的测量，表⽰为；
绿⾊的测量表⽰与关联帧相关的测量，表⽰为；
夹心取力器蓝⾊的测量表⽰与⽆关帧相关的测量，表⽰为；
当Markov blanket建⽴完成之后，就把边缘化的节点在局部图上边缘化掉，概率推导公式如下：
实际中相当于仅仅⽤构建最⼤似然估计问题，然后把边缘化掉，下⾯就是边缘化的理论推导了：
边缘化求解的先验分布X =m {X }1X =b {X ,X ,X }023X =r {X }4Z =m {Z ,Z }0112Z =c {Z ,Z ,Z }00323Z =r {Z ,Z }0434p x ∣z =(b m )p x ,x ∣z d x =∫x m (b m m )m :N ,Λ(x ^b t −1)
(3)
X ,X ,Z b m m X m X b
实际上，对于求解公式（3）的时候，我们需要使⽤贝叶斯公式求解⼀个最⼤后验的问题，但是因为并没有的先验信息（我个⼈觉得不管是局部还是全局，都没有这部分的先验），所以最终求解的还是⼀个最⼤似然的问题（）：
其中信息矩阵为：
其中使⽤局部估计的线性点，使⽤Schur补就可以得到边缘化之后的的信息矩阵，如下：
得到先验信息之后，整个没有边缘化的MLE（最⼤似然估计）问题（公式1）就可以写作下⾯带先验的⾮线性最⼩⼆乘问题（这个是⼀个引理，下⾯证明）：
X ，X m b p (X ,X ∣Z )∝m b m p (Z ∣X ,X )m b m ,=arg max p z ∣x ,x ={x ^b x ^m }x ,x b m (m b m )arg min z −h x ,x x ,x b m ∑(i ,j )∈supp z (m )∥ij ij (i j )∥Λij 2
(4)
Λ=J ΛJ =(i ,j )∈supp z (m )∑ij ⊤ij ij :[Λmm Λbm Λmb Λbb ]
J ij X b Λ=t Λ−bb ΛΛΛbm mm −1bm ⊤
=x ^arg −x +x min ∥x ^b b ∥Λt 2z −h x ,x (i ,j )∈supp z ,z (r c )∑∥ij ij (i j )∥Λij 2
(5)
引理：
如果Jacobian和信息矩阵都使⽤公式（4）所⽰的局部MLE估计的线性化点，那么边缘化的⾮线性化最⼩⼆乘问题可以最好的近似未边缘化的MLE问题。
证明：
窑链原始局部MLE问题的误差形式：
ibm as400
其中表⽰边缘化边的误差项，表⽰⾮边缘化边的误差项；
求解使得误差最⼩时的状态变量：
可以看到⼜因为要边缘化掉状态量，因此式⼦最后只把最⼩化项添加进去了；
对进⾏近似，有如下形式：
其中表⽰公式(1)对于状态的求导（其实相当于是增量⽅程中的项），即： 然后信息矩阵还是也⼀样，是对状态的求导，不过推导出来和上⾯的信息矩阵⼀样（跟增量⽅程中的⼀ 样），即：
随后就是求导等于0，然后运⾏schur补，可以得到最优的值为：
把上式代⼊原式中可以得到误差仅与的关系，如下： 需要注意的是这⾥的误差和上⾯的不⼀样的，如果需要解的话，需要从和联合求解出来， 不过因为这个值在边缘化之后，就固定了，所以可以忽略这个值继续求解增量等值；
所以最终误差项变为：
进⼀步，如果我们的线性化点是整个局部图的最优值，那么公式（6）中的就会为0，进⽽公式（7）中的也为0，所以公式（7）也就写作公式
（5）。但是实际上，我们在使⽤的时候依旧选择公式（7）的形式，因为我们并不能保证当时的线性化点就是最优的，如果是，那么为0，也不会影响结果。c (x )=c x ,x +m (m b )c x ,x r (b r )
c m c r c (x )=x min c x ,x ,x =x ,x b r min (x m min (m b n ))c x ,x +c x ,x x ,x b r min (r (b r )x m min m (m b ))
X m X m c x ,x m (m b )c ≃m c ,+m (x ^m x ^b )g +T [x −m x ^m x −b x
^b ]Λ21[x −m x ^m x −b x ^b ]T [x −m x ^m x −b x ^b ](6)
g b =∂X ∂e 22e =∂X ∂e
2e =[∂X m ∂e ∂X b ∂e ] where e =[g mm g mb ]z −ij h x ,x ij (i j )
悬浮机器人e 2H Λ=[Λmm Λbm Λmb
Λbb ]
X m x =m −x ^m Λg +Λx −mm −1
(mm mb (b x ^b ))
X b c x ,x ≃x m min m (m b )ζ+g x −+t T (b x ^b )x −Λx −21(b x ^b )T t (b x ^b )
c ,m (x ^m x ^b )g t Λc x ,x r ′(b r )=g x −+x −Λx −t T (b x ^b )21(b x ^b )T t (b x ^b )+z −h x ,x 21
(i ,j )∈supp z ,z (r c )∑∥ij ij (i j )∥Λij 2(7)
g g t g t
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议