本文部分內(nèi)容參考了以下博客：

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

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

摘要：本文介紹G2O和基于Sliding Window(SW)在邊緣化技術(shù)的異同。簡(jiǎn)單來(lái)說(shuō)，G2O的邊緣化等同于Schur Complement(SC)；基于SW的邊緣化，里面也用了SC來(lái)簡(jiǎn)化計(jì)算。G2O的邊緣化主要依賴于Hessian矩陣的構(gòu)造，本文將結(jié)合https://blog.csdn.net/heyijia0327/article/details/52822104的博客《SLAM中的marginalization 和 Schur complement》，詳細(xì)介紹H矩陣的構(gòu)造方法、結(jié)構(gòu)特點(diǎn)和Marg前后H的變化。

一、Marg技術(shù)在BA問(wèn)題理論應(yīng)用

BA是用來(lái)優(yōu)化SLAM中camera和landmark的約束問(wèn)題。隨著時(shí)間的增加，camera和landmark越來(lái)越多，BA優(yōu)化的維度也越來(lái)越高，那如何減少BA優(yōu)化的維度，就是所謂的邊緣化技術(shù)。概括之，Marg技術(shù)主要用于解決因?yàn)锽A問(wèn)題中路標(biāo)點(diǎn)和KeyFrame過(guò)多產(chǎn)生H維度過(guò)大的問(wèn)題。ORB-SLAM使用的G2O來(lái)解決BA問(wèn)題，G2O實(shí)際上是直接使用SC來(lái)簡(jiǎn)化了計(jì)算復(fù)雜度，所以G2O的邊緣化等同于SC。

不同于ORB-SLAM，基于SW的邊緣化更細(xì)致，它采用滑動(dòng)窗口來(lái)控制優(yōu)化的KeyFrame數(shù)量，從而控制H矩陣的維度，隨著時(shí)間增加，新關(guān)鍵幀會(huì)進(jìn)來(lái)滑動(dòng)窗口時(shí)，舊關(guān)鍵幀需要被marg掉，然而又不能簡(jiǎn)單的去掉，否則就丟失信息，所以就需要在被marg關(guān)鍵幀之前的約束條件下，來(lái)marg舊關(guān)鍵幀，并加入新KeyFrame來(lái)優(yōu)化。這個(gè)約束條件體現(xiàn)在之前優(yōu)化的Hessian矩陣中，作為當(dāng)前優(yōu)化的先驗(yàn)。

二、詳解G2O的Marginalization方法

2.1基于LM算法的BA

箭頭矩陣開(kāi)頭

以下面的圖示為例：

具體的LM算法流程不講，直接進(jìn)入LM算法中最后解方程的這一步：

其中J是Jacionbian矩陣，實(shí)際上就是Hessian矩陣，記，H矩陣的結(jié)構(gòu)就是上圖，它分成4個(gè)部分，紅色的部分記為Hpp，表示每個(gè)相機(jī)的Hessian矩陣；藍(lán)色的部分記為Hll，表示landmark的Hessian矩陣；右上角的綠色部分記為Hpl，左下角的綠色部分記為Hlp，實(shí)際上，表示路標(biāo)點(diǎn)和對(duì)應(yīng)的相機(jī)之間的關(guān)系，其中的綠色項(xiàng)的說(shuō)明對(duì)應(yīng)的路標(biāo)點(diǎn)和相機(jī)之間存在約束。如此，H矩陣可以寫成塊矩陣的形式，并簡(jiǎn)化方程式：

顯然上面的H矩陣存在著特殊的結(jié)構(gòu)，和均是對(duì)角塊矩陣(注意每個(gè)紅色或者藍(lán)色塊并非對(duì)角矩陣)，正如G2O論文中所述，可以用Schur Complement來(lái)簡(jiǎn)化上述方程：

該變換過(guò)程即Schur Complement (自行查閱資料)，其中很容易計(jì)算，因?yàn)槊總€(gè)藍(lán)色的小矩陣是3*3的，所以計(jì)算非常快，矩陣乘法和加減也很容易，所以得到上述形式計(jì)算很快，簡(jiǎn)化后的維度就是6*pose維，令，注意到H是對(duì)稱矩陣(一般H不是稀疏的了)，所以可使用cholesky(或者LDLT)分解該矩陣，分解速度很快，一般400維的矩陣Eigen分解1ms，自己移植的代碼需要2-3ms；分解后的矩陣都是對(duì)角陣和三角陣，所以計(jì)算很快，最后再求解：

這樣求解就變得很容易了。

G2O實(shí)現(xiàn)上述過(guò)程實(shí)際上每個(gè)pose和landmark都參與了H矩陣的構(gòu)造，最后通過(guò)SC簡(jiǎn)化矩陣來(lái)加速計(jì)算。個(gè)人認(rèn)為G2O這種方式可以算是marginalization的一種方法，但和Marginalization的本意不一樣。所以，ORB-SLAM中發(fā)現(xiàn)GlobalBA的時(shí)間會(huì)很長(zhǎng)，但是LBA的時(shí)間實(shí)際上并不長(zhǎng)，這是因?yàn)镺RB-SLAM優(yōu)化的是SpanningTree上的KeyFrame(即EssentialGraph上的KF)，它是由ORB-SLAM邏輯控制的，實(shí)際測(cè)試中發(fā)現(xiàn)，如果攝像頭在原地小范圍內(nèi)移動(dòng)，LBA優(yōu)化的pose也是很多的。

那為什么說(shuō)G2O這種方式也算是一種marginalization？(以下圖關(guān)系為例)

圖1是原始的H矩陣，右下角是pose相關(guān)部分，左上角是特征點(diǎn)相關(guān)部分。圖2是把H矩陣中和pose1相關(guān)的部分移動(dòng)到H矩陣的左上角。圖3是marg掉pose1以后的H矩陣，用斜線標(biāo)記的部分是待marg的特征點(diǎn)1，實(shí)際上marg以后的圖3對(duì)應(yīng)前面求時(shí)的矩陣。可以看到圖3和圖2中去掉斜線部分對(duì)比的話，變得稠密了。這里結(jié)合G2O的實(shí)現(xiàn)來(lái)講下為什么會(huì)稠密。

用之前的H矩陣圖例，考察 這個(gè)式子的 部分操作過(guò)程:

如果打算變換掉landmark2所在的行列，那landmark2所對(duì)應(yīng)的Hpl和Hll矩陣塊需要相互運(yùn)算,并和Hpp做減法。記:

的逆矩陣為inv2，其對(duì)應(yīng)的Hpl中綠色部分編號(hào)

對(duì)應(yīng)的Hpp部分編號(hào)

運(yùn)算規(guī)則如下：

Fori=1:2

?????? For j=i:2

?????????????

?????? End For

EndFor

運(yùn)算后Pose矩陣變成：

顏色加深說(shuō)明該塊矩陣進(jìn)行了一次運(yùn)算。整個(gè)H矩陣變成了：

顯然本來(lái)稀疏的Hpp現(xiàn)在因?yàn)閙arg掉landmark2而使得P12被fill-in，變得稠密了，因?yàn)閘andmark2關(guān)聯(lián)著pose1和pose2，所以P12被fill-in，同理，如果marg掉landmark2，因?yàn)?對(duì)應(yīng)著pose1,pose2,pose3，Hpp會(huì)變成：

這里是marg掉landmark的方式，G2O是采用這種處理方式。同理可以可以去marg掉pose，當(dāng)然可以猜測(cè)到結(jié)果會(huì)使得Hll部分被fill-in。現(xiàn)在回頭看

2->3是marg掉pose1之后的各部分矩陣，與剛才的例子不同是，該圖中pose1和pose2也存在著關(guān)聯(lián)，因此發(fā)現(xiàn)Hpl的部分也被fill-in了

圖結(jié)構(gòu)就變成了

理解到這里，應(yīng)該看heyijia的博客相關(guān)部分應(yīng)該沒(méi)困難了，marg landmark部分請(qǐng)閱讀博客剩余部分。博客在該部分最后也說(shuō)了“marg特征點(diǎn)1的這個(gè)過(guò)程這也給我們啟示，就是marg特征點(diǎn)的時(shí)候，我們要marg那些不被其他幀觀測(cè)到的特征點(diǎn)”，比如以紅綠藍(lán)矩陣為例，如果我們只marg掉landmark1，Hpp會(huì)變成：

只會(huì)加重P1的矩陣塊顏色，整個(gè)Hpp仍然是對(duì)角矩陣塊。

本節(jié)最后說(shuō)下自己關(guān)于Hessian矩陣的理解，有的資料說(shuō)是信息協(xié)方差矩陣，我們知道協(xié)方差矩陣描述了隨機(jī)樣本之間的關(guān)系(不相關(guān)和獨(dú)立)，不相關(guān)一般是指兩變量不線性相關(guān)。由統(tǒng)計(jì)知識(shí)可知，不相關(guān)不一定獨(dú)立、獨(dú)立一定不相關(guān)。獨(dú)立是不相關(guān)的充分不必要條件。如此，我們考慮紅綠藍(lán)矩陣部分，可以看到ORB-SLAM在構(gòu)造該矩陣的前提約束條件是每個(gè)pose之間、landmark之間和pose與landmark之間都是不相關(guān)的，marg過(guò)程中因?yàn)榇嬖趌andmark連接關(guān)系，會(huì)使得pose與pose之間產(chǎn)生關(guān)聯(lián)性。極端點(diǎn)，如果現(xiàn)在存在N個(gè)pose，都各自有N個(gè)landmark，之間無(wú)關(guān)系，那么marg過(guò)程就不存在fill-in，也就是每個(gè)pose之間不可能通過(guò)landmark產(chǎn)生關(guān)聯(lián)。回想Harris檢測(cè)最后也是用Hessian矩陣來(lái)判斷一個(gè)點(diǎn)是否角點(diǎn)的方法：

>特征值都比較大時(shí)，即窗口中含有角點(diǎn)

>特征值一個(gè)較大，一個(gè)較小，窗口中含有邊緣

>特征值都比較小，窗口處在平坦區(qū)域

特征比較大說(shuō)明自相關(guān)強(qiáng)，就是說(shuō)本點(diǎn)和區(qū)域其他點(diǎn)梯度差異大，那很可能就是角點(diǎn)；都比較小說(shuō)明自相關(guān)弱，和區(qū)域內(nèi)其他點(diǎn)的梯度差不多，可能就是平坦區(qū)域了；一大一小說(shuō)明和一個(gè)方向相關(guān)性強(qiáng)，一個(gè)方向相關(guān)性弱(或者說(shuō)梯度差一大一小->像素差一大一小)，那可能是邊緣了。

三、關(guān)于基于sliding window的Marginalization一些理解

因?yàn)闆](méi)有具體看源碼實(shí)現(xiàn)，所以結(jié)合自己找的資料和理解，解讀下基于sw的marg過(guò)程(之前請(qǐng)閱讀https://blog.csdn.net/heyijia0327/article/details/53707261講DSO的Marg部分，暫時(shí)可不看FEJ部分)。

上節(jié)最后說(shuō)了Hessian矩陣即是協(xié)方差矩陣，它能表示出pose與pose之間的約束關(guān)系，如此基于SW的Marg就能用H矩陣來(lái)保存舊關(guān)鍵幀和被marg掉關(guān)鍵幀的約束關(guān)系，且能和新加入的關(guān)鍵幀進(jìn)行優(yōu)化(從第二部分的分析可知，最后求解的pose增量就是由H矩陣決定的)，隨著時(shí)間的增加，當(dāng)SW窗口marg舊關(guān)鍵幀時(shí)，只需要更新H矩陣即可，從而保證了SW內(nèi)的優(yōu)化關(guān)鍵幀的數(shù)量，又不丟失舊關(guān)鍵幀的約束關(guān)系。

?

四、總結(jié)

本文主要詳細(xì)介紹了G2O的Marginalization的實(shí)現(xiàn)過(guò)程，并簡(jiǎn)單擴(kuò)展到基于sliding window的Marginalization的不同。因?yàn)檫€沒(méi)有對(duì)基于SW的marg代碼深入研究，所以無(wú)法用數(shù)學(xué)表達(dá)式詳細(xì)描述實(shí)現(xiàn)過(guò)程，后面有空可能會(huì)繼續(xù)研究下。

總結(jié)

以上是生活随笔為你收集整理的G2O和Sliding Window中的Marginalization的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。