本文部分內容參考了以下博客：

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代碼深入研究，所以無法用數學表達式詳細描述實現過程，后面有空可能會繼續研究下。

總結

以上是生活随笔為你收集整理的G2O和Sliding Window中的Marginalization的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。