wordpress隱藏地址插件西安seo霸屏
主要目標(biāo):理解滑動(dòng)窗口法、位姿圖優(yōu)化、帶IMU緊耦合的優(yōu)化、掌握g2o位姿圖。
第9講介紹了以為BA為主的圖優(yōu)化。BA能精確優(yōu)化每個(gè)相機(jī)位姿與特征點(diǎn)位置。不過在更大的場(chǎng)景中,大量特征點(diǎn)的存在會(huì)嚴(yán)重降低計(jì)算效率,導(dǎo)致計(jì)算量越來越大,無法實(shí)時(shí)化。本講第一部分介紹一種簡(jiǎn)化的BA:位姿圖
目錄
0、名詞解釋
1、滑動(dòng)窗口濾波與優(yōu)化
1.1 實(shí)際環(huán)境下的BA結(jié)構(gòu)
1.2 滑動(dòng)窗口法
2、位姿圖
2.1 位姿圖的意義
2.2 位姿圖的優(yōu)化
3、實(shí)踐:位姿圖優(yōu)化
3.1 g2o原生位姿圖
3.2 李代數(shù)上的位姿圖優(yōu)化
3.3 小結(jié)
0、名詞解釋
- SfM(Structure from Motion)是一種從多個(gè)視角拍攝的對(duì)象或場(chǎng)景中重建其三維結(jié)構(gòu)的方法。
- VO(Visual Odometry)或VIO(Visual-Inertial Odometry)系統(tǒng):解決的是里程計(jì)問題,里程計(jì)部分是由慣導(dǎo),輪速計(jì),視覺,激光等多傳感器融合完成。
1、滑動(dòng)窗口濾波與優(yōu)化
1.1 實(shí)際環(huán)境下的BA結(jié)構(gòu)
- 控制計(jì)算規(guī)模的方法
- 從連續(xù)的視頻中抽出一部分作為關(guān)鍵幀,僅構(gòu)造關(guān)鍵幀與路標(biāo)點(diǎn)之間的BA,非關(guān)鍵幀只用于定位,對(duì)建圖則無貢獻(xiàn)。
- 按照某種原則,取時(shí)間上靠近當(dāng)前時(shí)刻,空間上又可以展開的關(guān)鍵幀,從而保證相機(jī)即使在停止不動(dòng)時(shí),BA結(jié)構(gòu)也不至于縮成一團(tuán)。
- 定義共視圖(與現(xiàn)在的相機(jī)存在共同觀測(cè)的關(guān)鍵幀構(gòu)成的圖)結(jié)構(gòu),在BA優(yōu)化時(shí),按照某些原則在共視圖內(nèi)取關(guān)鍵幀和路標(biāo)優(yōu)化。例如,僅優(yōu)化于當(dāng)前幀有20個(gè)共視路標(biāo)的關(guān)鍵幀,其余部分固定不變。
- 滑動(dòng)窗口也好,共視圖也好,都是對(duì)實(shí)時(shí)計(jì)算的某種工程上的折中。
1.2 滑動(dòng)窗口法
- 滑動(dòng)窗口法比較適合VO系統(tǒng),而不適合大規(guī)模建圖的系統(tǒng)。
- 在邊緣化時(shí),我們考慮關(guān)鍵幀的位姿,即:
,其中
是第k個(gè)關(guān)鍵幀的位姿均值,
為所有關(guān)鍵幀的協(xié)方差矩陣。均值部分是指BA迭代之后的結(jié)果,而
就是對(duì)整個(gè)BA的H矩陣進(jìn)行邊緣化之后的結(jié)果,即上一講的S矩陣。
- 當(dāng)窗口結(jié)構(gòu)發(fā)生改變,這些狀態(tài)變量如何變化?
- 需要在窗口新增一個(gè)關(guān)鍵幀,以及它觀測(cè)到的路標(biāo)點(diǎn)。
- 需要把窗口中的一個(gè)舊的關(guān)鍵幀刪除,也可能刪除它觀測(cè)到的路標(biāo)點(diǎn)。
- ?新增一個(gè)關(guān)鍵點(diǎn)和路標(biāo)點(diǎn)
- 只需按照正常BA流程處理即可,對(duì)所有點(diǎn)進(jìn)行邊緣化時(shí),即得到這N+1個(gè)關(guān)鍵幀的高斯分布參數(shù)。
- 刪除一個(gè)舊的關(guān)鍵幀
- 若考慮邊緣化舊的關(guān)鍵幀
,那么Schur消元過程相當(dāng)于通過矩陣行和列操作消去非對(duì)角線處幾個(gè)非零矩陣塊,顯然將導(dǎo)致右下角路標(biāo)矩陣塊不再是非對(duì)角矩陣。這個(gè)過程稱為邊緣化中的填入(Fill-in)。這時(shí)BA無法按照先前的稀疏方式迭代求解。
- 若對(duì)邊緣化過程進(jìn)行一些改造,也可以保持滑動(dòng)窗口BA的稀疏性。例如,在邊緣化某個(gè)舊的關(guān)鍵幀時(shí),同時(shí)邊緣化它觀測(cè)到的路標(biāo)點(diǎn)。這樣,路標(biāo)點(diǎn)的信息就會(huì)轉(zhuǎn)換成剩下那些關(guān)鍵幀之間的共視信息,從而保持右下角部分的對(duì)角塊結(jié)構(gòu)。
- 還有其他更復(fù)雜的邊緣化策略,例如OKVIS。
- 若考慮邊緣化舊的關(guān)鍵幀
- ?SWF中邊緣化的直觀解釋
- 邊緣化在概率上的意義就是指條件概率。所以,我們邊緣化某個(gè)關(guān)鍵幀,即“保持這個(gè)關(guān)鍵幀當(dāng)前的估計(jì)值,求其他狀態(tài)變量以這個(gè)關(guān)鍵幀為條件的條件概率”。
- 當(dāng)某個(gè)關(guān)鍵幀被邊緣化,它觀測(cè)到的路標(biāo)點(diǎn)就會(huì)產(chǎn)生一個(gè)“這些路標(biāo)應(yīng)該在哪里”的先驗(yàn)信息,從而影響其余部分的估計(jì)值。
- 如果再邊緣化這些路標(biāo)點(diǎn),那么它們的觀測(cè)者將得到一個(gè)“觀測(cè)它們的關(guān)鍵幀應(yīng)該在哪里”的先驗(yàn)信息。
,然后舍去被邊緣化部分的信息。
- ?滑動(dòng)窗口法比較適合VO系統(tǒng),而不適合大規(guī)模建圖系統(tǒng)。
- g2o和 ceres還未直接支持滑動(dòng)窗口法中的邊緣化操作,這里只講理論部分。
- 邊緣化在概率上的意義就是指條件概率。所以,我們邊緣化某個(gè)關(guān)鍵幀,即“保持這個(gè)關(guān)鍵幀當(dāng)前的估計(jì)值,求其他狀態(tài)變量以這個(gè)關(guān)鍵幀為條件的條件概率”。
2、位姿圖
2.1 位姿圖的意義
- 根據(jù)前面的討論,我們發(fā)現(xiàn)特征點(diǎn)在優(yōu)化問題中占據(jù)了絕大部分,實(shí)際上,經(jīng)過若干次觀測(cè)之后,收斂的特征點(diǎn)位置變換很小,發(fā)散的外點(diǎn)則已被剔除。因此更傾向于在優(yōu)化幾次之后就把特征點(diǎn)固定住,只把它們看做位姿估計(jì)的約束,不在實(shí)際地優(yōu)化它們的位置估計(jì)。
- 因此,完全可以構(gòu)建一個(gè)只有軌跡的圖優(yōu)化,而位姿節(jié)點(diǎn)之間的邊,可以由兩個(gè)關(guān)鍵點(diǎn)之間通過特征匹配后的得到的運(yùn)動(dòng)估計(jì)來給定初始值,之后不再優(yōu)化那些路標(biāo)點(diǎn)的位置,只關(guān)心所有相機(jī)位姿之間的聯(lián)系。如此,省去大量特征點(diǎn)優(yōu)化的計(jì)算,只保留關(guān)鍵幀的估計(jì),從而構(gòu)建位姿圖(Pose Graph)。
- 如果我們有額外測(cè)量Pose的傳感器,那么位姿圖也是一種常見的融合Pose測(cè)量的方法。
2.2 位姿圖的優(yōu)化
- 位姿圖優(yōu)化的節(jié)點(diǎn)表示相機(jī)位姿,邊是兩個(gè)位姿節(jié)點(diǎn)之間相對(duì)運(yùn)動(dòng)的估計(jì),該估計(jì)可以來自于特征點(diǎn)法或直接法,也可以來自GPS或IMU積分。
- 假設(shè)我們估計(jì)了
與
之間的一個(gè)運(yùn)動(dòng)
。該運(yùn)動(dòng)表達(dá)可以是:
,或按李群的寫法:
- 上述等式不會(huì)精確成立,因此我們?cè)O(shè)立最小二乘誤差,討論誤差關(guān)于優(yōu)化變量的導(dǎo)數(shù)。誤差:
- 求
對(duì)兩個(gè)優(yōu)化變量
,
的導(dǎo)數(shù)。
- ?若誤差接近0,左右雅可比可以近似為
或
- 雅可比求導(dǎo)后,剩下就和普通圖優(yōu)化一樣了。
- 綜上,所有位姿頂點(diǎn)和位姿-位姿邊構(gòu)成一個(gè)圖優(yōu)化,本質(zhì)上是一個(gè)最小二乘問題,優(yōu)化變量為各個(gè)頂點(diǎn)的位姿,邊來自于位姿觀測(cè)約束。記
為所有邊集合,則總體目標(biāo)函數(shù):
??梢杂酶咚古nD法或列文伯格-馬夸爾特方法求解此問題。
3、實(shí)踐:位姿圖優(yōu)化
3.1 g2o原生位姿圖
g2o_viewer 在g2o/bin下未找到,未安裝成功。安裝libqglviewer-dev-qt5 后,重新安裝g2o解決。參考以下文檔
E: 軟件包 libqglviewer-dev 沒有可安裝候選解決方法-CSDN博客
3.2 李代數(shù)上的位姿圖優(yōu)化
Sophus表達(dá)李代數(shù),用到g2o中定義頂點(diǎn)和邊。
運(yùn)行代碼時(shí),誤差大于書中描述誤差,暫未查出具體原因?
3.3 小結(jié)
- PTAM(Parallel Tracking and Mapping)提出以來,人們意識(shí)到后端優(yōu)化沒必要實(shí)時(shí)響應(yīng)前端的圖像數(shù)據(jù),人們傾向于把前端后端分開,運(yùn)行于兩個(gè)獨(dú)立線程之中,稱為跟蹤和建圖。前端需要實(shí)時(shí)響應(yīng)視頻的速度,而優(yōu)化不必,只要在優(yōu)化完成時(shí)把結(jié)果返回前端即可。所有通常不會(huì)對(duì)后端提出很高的速度要求。