VIO學(xué)習(xí)總結(jié)

點擊上方“小白學(xué)視覺”，選擇加"星標(biāo)"或“置頂”

重磅干貨，第一時間送達
本文轉(zhuǎn)自|新機器視覺

VIO（visual-inertial odometry）即視覺慣性里程計，有時也叫視覺慣性系統(tǒng)（VINS，visual-inertial system），是融合相機和IMU數(shù)據(jù)實現(xiàn)SLAM的算法，根據(jù)融合框架的區(qū)別又分為緊耦合和松耦合，松耦合中視覺運動估計和慣導(dǎo)運動估計系統(tǒng)是兩個獨立的模塊，將每個模塊的輸出結(jié)果進行融合，而緊耦合則是使用兩個傳感器的原始數(shù)據(jù)共同估計一組變量，傳感器噪聲也是相互影響的，緊耦合算法上比較復(fù)雜，但充分利用了傳感器數(shù)據(jù)，可以實現(xiàn)更好的效果，是目前研究的重點。

單目視覺 SLAM 算法存在一些本身框架無法克服的缺陷，首先是尺度的問題 ，單目 SLAM 處理的圖像幀丟失了環(huán)境的深度信息，即使通過對極約束和三角化恢復(fù)了空間路標(biāo)點的三維信息，但是這個過程的深度恢復(fù)的刻度是任意的，并不是實際的物理尺度，導(dǎo)致的結(jié)果就是單目SLAM 估計出的運動軌跡即使形狀吻合但是尺寸大小卻不是實際軌跡尺寸；由于基于視覺特征點進行三角化的精度和幀間位移是有關(guān)系的，當(dāng)相機進行近似旋轉(zhuǎn)運動的時候，三角化算法會退化導(dǎo)致特征點跟蹤丟失，同時視覺 SLAM 一般采取第一幀作為世界坐標(biāo)系，這樣估計出的位姿是相對于第一幀圖像的位姿，而不是相對于地球水平面 (世界坐標(biāo)系) 的位姿，后者卻是導(dǎo)航中真正需要的位姿，換言之，視覺方法估計的位姿不能和重力方向?qū)R。

重力向量構(gòu)建了視覺坐標(biāo)系和世界坐標(biāo)系的聯(lián)系

通過引入 IMU 信息可以很好地解決上述問題，首先通過將 IMU 估計的位姿序列和相機估計的位姿序列對齊可以估計出相機軌跡的真實尺度，而且 IMU 可以很好地預(yù)測出圖像幀的位姿以及上一時刻特征點在下幀圖像的位置，提高特征跟蹤算法匹配速度和應(yīng)對快速旋轉(zhuǎn)的算法魯棒性，最后 IMU 中加速度計提供的重力向量可以將估計的位置轉(zhuǎn)為實際導(dǎo)航需要的世界坐標(biāo)系中。同時，智能手機等移動終端對 MEMS 器件和攝像頭的大量需求大大降低了兩種傳感器的價格成本；硬件實現(xiàn)上， MEMS 器件也可以直接嵌入到攝像頭電路板上。綜合以上，融合 IMU 和視覺信息的 VINS 算法可以很大程度地提高單目 SLAM 算法性能，是一種低成本高性能的導(dǎo)航方案，在機器人、AR/VR 領(lǐng)域得到了很大的關(guān)注。

 

從另外一個角度理解視覺慣性融合的意義，我們一般采用運動方程和測量方程描述機器人的運動過程，具體到視覺慣性緊耦合運動估計問題，這里的第k 時刻的狀態(tài) 為IMU的運動測量輸入，  實現(xiàn)根據(jù)上一時刻估計的狀態(tài)和k-1到k 過程中IMU的運動測量預(yù)測的當(dāng)前時刻狀態(tài)  ，由于測量噪聲的存在，預(yù)測狀態(tài)  和當(dāng)前真實狀態(tài)  的差異由噪聲項  表示。測量方程中，  為k時刻可以觀測到的所有3D路標(biāo)點集，h在這里對應(yīng)相機投影模型，  為k 時刻對路標(biāo)點集  的觀測，觀測值為  ，觀測噪聲為  ，即k時刻3D路標(biāo)點集  在相機圖像上的投影點集為  。

整個機器人的運動過程就是運動方程根據(jù)上一時刻狀態(tài)和當(dāng)前時刻運動輸入  預(yù)測當(dāng)前狀態(tài)，利用傳感器的測量方程作為矯正的過程，在純視覺SLAM中，因為缺少運動輸入，預(yù)測方程提供信息不足，我們是直接采用測量方程構(gòu)建重投影誤差進行運動優(yōu)化估計，而引入IMU數(shù)據(jù)之后，用IMU作為運動輸入  可以對狀態(tài)進行很好的預(yù)測，VIO就是利用了運動預(yù)測和測量方程構(gòu)建聯(lián)合估計。具體來說，在假設(shè)噪聲服從高斯分布，對非線性方程進行一階近似的條件下，從最大后驗估計的角度就可以推導(dǎo)出我們在VIO論文中看到的聯(lián)合優(yōu)化目標(biāo)函數(shù)：

學(xué)習(xí)資源

書籍

入門書籍推薦高博的《SLAM十四講》，這本書不僅涵蓋了SLAM和狀態(tài)估計相關(guān)的基本理論知識，而且高博給出了每個理論模塊的編程實現(xiàn)，第一遍建議可以先不細(xì)推公式，把書瀏覽一遍了解主要內(nèi)容，第二遍就可以動手跟著書一點點擼代碼了。之后就可以學(xué)習(xí)目前主流的開源框架，在對SLAM有較深的了解和實踐之后，建議讀《state estimation for robotics》和《Multi View Geometry》,前者是介紹狀態(tài)估計理論非常完備的一本書，舉例來說，在非線性性非高斯章節(jié)中，作者從機器人的運動和測量方程出發(fā)，根據(jù)馬爾科夫假設(shè)和貝葉斯公式，將目前主流的濾波（EKF，UKF，PF）方法統(tǒng)一在貝葉斯估計的框架下。后者涵蓋了SLAM視覺幾何的所有理論知識。

對三維旋轉(zhuǎn)的描述計算需要李群李代數(shù)的知識，高博的書和《state estimation for robotics》都有詳細(xì)講解，簡單來說，使用旋轉(zhuǎn)矩陣表示的旋轉(zhuǎn)方便向量的坐標(biāo)計算，但直接對其優(yōu)化是有約束優(yōu)化問題（9個參數(shù)3個自由度），解決思路就是在三維旋轉(zhuǎn)群的正切空間上對用李代數(shù)表示的旋轉(zhuǎn)誤差量進行無約束優(yōu)化，對正切空間的理解可以參考:

《Lie groups, Lie algebras, projective geometry and optimization

for 3D Geometry, Engineering and Computer Vision》。

 

推公式的時候發(fā)現(xiàn)矩陣白學(xué)了？寶寶不哭，這有一本《the Matrix CookBook》送給你。

VIO 開源框架

VIO目前實現(xiàn)比較好的有vinsmono,okvis,MSCKF。前兩個是基于非線性優(yōu)化的方案而且框架比較相似，后者是基于濾波優(yōu)化的方案，也是Google Tango上使用的方法，MSCKF目前并沒有開源，不過賓夕法尼亞的Kumar實驗室18年有一個相似的工作，目前已經(jīng)開源。此外還有ROVIO。值得注意的是，雖然在純視覺SLAM中，學(xué)界已經(jīng)公認(rèn)基于非線性優(yōu)化方法的SLAM方法效果要好于濾波的方法，但在VIO中，非線性優(yōu)化和濾波方法目前還沒有很明顯的優(yōu)劣之分。我的理解是結(jié)合相機和IMU兩種傳感器的信息，提供了對當(dāng)前狀態(tài)更多的觀測，使得算法對歷史觀測之間約束信息的依賴降低，這也是為什么okvis、vinsmono采用滑動窗口法（global bundle adjustment和filter 的折中）也能取得很好效果的原因。

 

vinsmono的代碼和論文比較一致，是目前大家學(xué)習(xí)參考比較多的開源框架。

VIO中，目前多采用流形空間上預(yù)積分的方法對IMU數(shù)據(jù)進行預(yù)處理，核心思路是在兩幀之間計算IMU的幀間運動增量，在迭代優(yōu)化時直接使用運動增量，提高計算效率。這部分參考論文《On-Manifold Preintegration for Real-Time

Visual-Inertial Odometry》，整篇論文推導(dǎo)的思路是先推導(dǎo)出流形空間上的運動狀態(tài)的表達公式，然后為了能夠整合到最大后驗估計的優(yōu)化框架里，分離出運動表達式中的噪聲項，使其近似滿足高斯分布，之后推導(dǎo)了在bias變化時的運動估計值的更新方法。

前面有提到VIO用到了以IMU作為控制輸入的運動方程，Joan Sola 寫的《Quaternion kinematics for the error-state Kalman filter》對以IMU測量值為運動輸入的運動方程的誤差遞進形式進行了詳細(xì)推導(dǎo)，如果在vinsmono和okvis的運動遞進方程的雅克比矩陣推導(dǎo)遇到困難，或者對四元數(shù)方法進行運動描述不了解，可以仔細(xì)閱讀下這篇文章。文章對坐標(biāo)系的locally和globally的解釋也讓人印象深刻。

VIO的初始化是系統(tǒng)工作非常關(guān)鍵的部分，這部分可以參考vinsmono以及ORB作者寫的的VIO文章《Visual-Inertial Monocular SLAM with Map Reuse》。兩篇文章思路比較相似，先是通過單目運動估計的方法獲取多幀圖像的位姿，然后以此為運動參考估計其他參數(shù)，整個過程和相機IMU標(biāo)定比較相似。

初始化主要完成三部分工作：

1. 為非線性估計系統(tǒng)提供一個運動初值 2.  
2. 估計重力向量在視覺坐標(biāo)系下的投影向量，以此將視覺坐標(biāo)系對齊到世界坐標(biāo)系下。
3. 估計尺度、IMUbias

vinsmono的代碼中協(xié)方差矩陣遞進公式采用的是中值積分的方法，推導(dǎo)思路和Joan Sola文章中的一致，具體推導(dǎo)可以參考:

https://www.zhihu.com/question/64381223/answer/255818747

https://blog.csdn.net/u012871872/article/details/78128087

vonsmono代碼中的融合部分是非常值得學(xué)習(xí)參考的，但是代碼中的視覺處理部分多是直接使用OPENCV的函數(shù)，而且代碼風(fēng)格是C++/C 混合的。所以推薦看下ORBSLAM的代碼，首先編程實現(xiàn)非常規(guī)范，對編程學(xué)習(xí)有很大的參考價值，而且整個代碼對opencv的依賴較低，視覺部分的特征提取，視覺運動估計，都是作者自己編程實現(xiàn)的，對理解視覺幾何的實現(xiàn)有很好的幫助，泡泡有篇公開課對ORBSLAM的代碼進行了梳理:

https://pan.baidu.com/s/1c1QOoHM 密碼: xfjd

 End 

好消息，小白學(xué)視覺團隊的知識星球開通啦，為了感謝大家的支持與厚愛，團隊決定將價值149元的知識星球現(xiàn)時免費加入。各位小伙伴們要抓住機會哦！

下載1：OpenCV-Contrib擴展模塊中文版教程

在「小白學(xué)視覺」公眾號后臺回復(fù)：擴展模塊中文教程，即可下載全網(wǎng)第一份OpenCV擴展模塊教程中文版，涵蓋擴展模塊安裝、SFM算法、立體視覺、目標(biāo)跟蹤、生物視覺、超分辨率處理等二十多章內(nèi)容。

下載2：Python視覺實戰(zhàn)項目52講

在「小白學(xué)視覺」公眾號后臺回復(fù)：Python視覺實戰(zhàn)項目，即可下載包括圖像分割、口罩檢測、車道線檢測、車輛計數(shù)、添加眼線、車牌識別、字符識別、情緒檢測、文本內(nèi)容提取、面部識別等31個視覺實戰(zhàn)項目，助力快速學(xué)校計算機視覺。

下載3：OpenCV實戰(zhàn)項目20講

在「小白學(xué)視覺」公眾號后臺回復(fù)：OpenCV實戰(zhàn)項目20講，即可下載含有20個基于OpenCV實現(xiàn)20個實戰(zhàn)項目，實現(xiàn)OpenCV學(xué)習(xí)進階。

交流群

歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動駕駛、計算攝影、檢測、分割、識別、醫(yī)學(xué)影像、GAN、算法競賽等微信群（以后會逐漸細(xì)分），請掃描下面微信號加群，備注：”昵稱+學(xué)校/公司+研究方向“，例如：”張三 + 上海交大 + 視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會根據(jù)研究方向邀請進入相關(guān)微信群。請勿在群內(nèi)發(fā)送廣告，否則會請出群，謝謝理解~