SLAM中的边缘化舒尔补

SLAM的Bundle Adjustment上&＃xff0c;随着时间的推移&＃xff0c;路标特征点和相机的位姿越来越多&＃xff0c;BA的计算量随着变量的增加而增加。因此&＃xff0c;要限制优化变量的多少&＃xff0c;不能只一味的增加待优化的变量到BA里&＃xff0c;而应该去掉一些变量。如何丢变量就是很重要的问题&＃xff01;

比如有frame1,frame2,frame3 以及这些frame上的特征点pt1…ptn。新来了一个frame4&＃xff0c;为了不再增加BA的变量&＃xff0c;出现在脑海里的直接做法是把frame1以及相关特征点pt直接丢弃&＃xff0c;只优化frame2,frame3,frame4及相应特征点。然而&＃xff0c;直接丢掉变量&＃xff0c;就导致损失了信息&＃xff0c;frame1可能能更多的约束相邻的frame&＃xff0c;直接丢掉的方式就破坏了这些约束。

在SLAM中&＃xff0c;一般概率模型都是建模成高斯分布&＃xff0c;如相机的位姿都是一个高斯分布&＃xff0c;轨迹和特征点形成了一个多元高斯分布p(x1,x2,x3,pt1…)&＃xff0c;然后图优化或者BA就从一个概率问题变成一个最小二乘问题。因此&＃xff0c;从这个多元高斯分布中去掉一个变量的正确做法是把他从这个多元高斯分布中边缘化出去。边缘化就是使用Schur complement&＃xff08;舒尔补&＃xff09;

我们知道SLAM中的图优化和BA都是最小二乘问题&＃xff0c;通过高斯牛顿迭代求得&＃xff0c;即

这样我们就能够迭代的更新部分变量&＃xff0c;从而维持计算量不增加。在上面这个过程中&＃xff0c;我们要注意&＃xff0c;构建出来的Hx&＃61;b是利用了marg变量的信息&＃xff0c;也就是说我们没有人为的丢弃约束&＃xff0c;所以不会丢失信息&＃xff0c;但是计算结果的时候&＃xff0c;我们只去更新了我们希望保留的那些变量的值。在slam的过程中&＃xff0c;BA不断地加入新的待优化的变量&＃xff0c;并marg旧的变量&＃xff0c;从而使得计算量维持在一定水平。

ref:

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

《视觉SLAM十四讲》