[1]知行合一2
SLAM中的marginalization 和 Schur complement
SLAM的Bundle Adjustment上，隨着時間的推移，路標特征點(landmark)和相機的位姿pose越來越多，BA的計算量隨着變量的增加而增加，即使BA的H矩陣是稀疏的，也吃不消。因此，我們要限制優化變量的多少，不能只一味的增加待優化的變量到BA里，而應該去掉一些變量。那么如何丟變量就成了一個很重要的問題！比如有frame1,frame2,frame3 以及這些frame上的特征點pt1…ptn。新來了一個frame4，為了不再增加BA時的變量，出現在腦海里的直接做法是把frame1以及相關特征點pt直接丟棄，只優化frame2,frame3,frame4及相應特征點。然而，這種做法好嗎？
Gabe Sibley [2]在他們的論文中就明確的說明了這個問題。直接丟掉變量，就導致損失了信息，frame1可能能更多的約束相鄰的frame，直接丟掉的方式就破壞了這些約束。在SLAM中，一般概率模型都是建模成高斯分布，如相機的位姿都是一個高斯分布，軌跡和特征點形成了一個多元高斯分布p(x1,x2,x3,pt1…)，然后圖優化或者BA就從一個概率問題變成一個最小二乘問題。因此，從這個多元高斯分布中去掉一個變量的正確做法是把他從這個多元高斯分布中marginalize out.
這marginalize out具體該如何操作呢？Sliding widow Filter [2]中只是簡單的一句應用Schur complement（舍爾補）. 我們知道SLAM中的圖優化和BA都是最小二乘問題，如下圖所示(ref.[1])
構建出來的Hx=b是利用了marg變量的信息，也就是說我們沒有人為的丟棄約束，所以不會丟失信息，但是計算結果的時候，我們只去更新了我們希望保留的那些變量的值。在slam的過程中，BA不斷地加入新的待優化的變量，並marg舊的變量，從而使得計算量維持在一定水平。
marginalize的本意是:只更新部分變量，而不是所有變量。
一句話總結如下：要把一部分變量從多元高斯分布從分離出來，需要把協方差矩陣也給分開，然而協方差矩陣大家都知道，眾多變量之間息息相關，不能簡單地說協方差矩陣的哪一塊就是誰的，需要用到schur complement （舍爾補）來分割。
[2]VINS-Mono代碼分析總結
4.4 系統邊緣化
4.4.1 邊緣化的定義和目的
邊緣化(marginalization)的過程就是將滑窗內的某些較舊或者不滿足要求的視覺幀剔除的過程，所以邊緣化也被描述為將聯合概率分布分解為邊緣概率分布和條件概率分布的過程(說白了，就是利用shur補減少優化參數的過程)。
利用Sliding Window做優化的過程中，邊緣化的目的主要有兩個：
- 滑窗內的pose和feature個數是有限的，在系統優化的過程中，勢必要不斷將一些pose和feature移除滑窗。
- 如果當前幀圖像和上一幀添加到滑窗的圖像幀視差很小，則測量的協方差(重投影誤差)會很大，進而會惡化優化結果。LIFO導致了協方差的增大，而惡化優化結果。
直接進行邊緣化而不加入先驗條件的后果：
- 無故地移除這些pose和feature會丟棄幀間約束，會降低了優化器的精度，所以在移除pose和feature的時候需要將相關聯的約束轉變為一個先驗的約束條件作為prior放到優化問題中。
- 在邊緣化的過程中，不加先驗的邊緣化會導致系統尺度的缺失(參考[6])，尤其是系統在進行退化運動時(如無人機的懸停和恆速運動)。一般來說只有兩個軸向的加速度不為0的時候，才能保證尺度可觀，而退化運動對於無人機或者機器人來說是不可避免的。所以在系統處於退化運動的時候，要加入先驗信息保證尺度的客觀性。
4.4.2 兩種邊緣化措施
這兩種邊緣化的措施主要還是針對懸停和恆速運動等退化運動。
4.4.3 舒爾補邊緣化優化狀態量
4.4.5 滑窗優化(Slide window)
按理說是不應該把滑窗當做一小節來講的，邊緣化、舒爾補都屬於滑窗的范圍，但前面已經總結了。
而且正如賀一加師兄在其博客中提到的，滑窗的三大法寶"Marginalization"，"Schur complement"，"First estimate jacobin"，這些在前面也提到了。
關於這部分內容，可以參考這幾篇博客，講的真是太好了。okvis1，okvis2，知行合一1，知行合一2