基于視覺慣性融合的SVO
7月初的時候嘗試把IMU作為視覺導航的輔助項，加入到SVO的架構中去，目前已完成基于半直接單目視覺裡程計SVO 的 視覺慣性融合 的程式架構搭建。以下圍繞 ‘IMU資料以什麼樣的方式參與視覺系統導航？’、‘IMU可作用于視覺導航的哪些部分？’、‘融合IMU資料可以得到怎樣的結果？’幾個方面進行講述。
本文僅代表個人觀點，并作為工作學習記錄，若有錯誤或者不妥之處歡迎指出！！！
在HeYijia對于SVO 的改進版本上https://github.com/HeYijia/svo_edgelet進行修改，并且參考VI-ORB代碼https://github.com/jingpang/LearnVIORB以及VINS-Mono https://github.com/HKUST-Aerial-Robotics/VINS-Mono中的IMU使用。
#程式流程圖
原來SVO架構
視覺慣性融合的半直接視覺裡程計程式流程圖大緻如上，在原有的SVO架構上添加了視覺慣性聯合初始化子產品和後端優化子產品。
#1IMU資料以什麼樣的方式參與視覺系統導航？
##IMU預積分
作用： IMU預積分算法可将積分增量與世界系下的慣性狀态解耦，避免線上性化點更新時對測量值的重複積分計算，減少計算複雜度并擷取連續時間内的相對運動增量
IMU測量頻率在200Hz,視覺圖像頻率約20Hz，在兩幀圖像之間，對IMU采用預積分的方式進行運動增量計算。IMU就以這種預積分的方式參與視覺系統各個環節的計算。
在純視覺系統中，我們隻能得到視覺的狀态估計量，無法建構運動模型，是以通過視覺測量方程建構重投影誤差進行運動優化估計。在加上IMU測量資料之後，相當于引入了運動測量。在VIO中，就可以通過IMU的運動測量與視覺測量進行聯合估計，有效提高系統魯棒性。
#2 IMU可作用于視覺導航的哪些部分？
##2.1視覺慣性聯合初始化
視覺慣性聯合初始化是VIO必不可少的一部分，通過将視覺資料與IMU資料對齊，擷取導航必須的初始參數估計。
最最重要的一點就是：單目視覺的尺度恢複！！
其實在SVO上添加IMU 的初衷也是為了恢複單目尺度。在原SVO中，作者将首幀固定，作為第一個關鍵幀，并在後續普通幀上進行光流跟蹤，計算比對特征點的個數以及平均視差，當平移量足夠大 平均視差>50個像素時，才認為找到第二個關鍵幀，随後再通過視差的方差是否超過門檻值來判斷計算E矩陣還是H矩陣。通過前兩個關鍵幀完成視覺初始化之後，得到兩關鍵幀之間的平移量與旋轉量t，R ，也得到了由兩個關鍵幀中多有特幀點建構的初始地圖。
在這一環節中，環境的尺度（包括特征點的坐标以及相機位姿）均以兩關鍵幀之間的平移量t作為機關1。
而IMU得到的測量資料（加速度與角速度）均為世界系下的真實尺度的運動測量，通過IMU 預積分擷取一段時間内的運動增量，并将其與視覺位姿估計的運動增量對比，可以恢複出視覺的真實尺度。這是單目尺度恢複的基本思想。
聯合初始化的理論部分參考論文：Visual-Inertial Monocular SLAM with Map Reuse 通過 4 個步驟将視覺與慣性資訊對齊，解決了視覺慣性系統的初始化問題。具體公式就不進行展開了，找論文都能看到。這篇論文是ORB-SLAM的作者寫的，相應程式中也是使用同樣的初始化方法。
我在程式中同樣使用這種聯合初始化方法，對關鍵幀處的陀螺儀偏差、單目尺度、重力矢量、加速度偏差以及速度進行估計，但讓我很迷的一點就是“初始化的終止條件”？？Visual-Inertial Monocular SLAM with Map Reuse一文中作者把初始化的時間設為一個定值，貌似是做過實驗：15s的初始化時間可以使得所有待估計參數可觀。 但是在仿真過程中發現，15s并不是一個十分靠譜的數。之前Closed-Form Solution of Visual-Inertial Structure from Motion文章中提到過，當機體處于靜止或勻速運動狀态下時，VI-SfM 問題将存在無窮解，此時是無法進行視覺慣性聯合初始化的。這是否表明，若是采用給定15s的初始化時間，那此區間内不能存在靜止或者勻速運動？？？？ 我嘗試過采用不同的起始幀進行圖像跟蹤，初始化估計出來的尺度時好時壞，并且與起始幀有着巨大的關聯。使用某些起始幀，基本上在6、7s很快就能達到初始化參數收斂，用不到15s。而使用相差不遠的幀作為起始幀，卻估計出誤差巨大的尺度。
關于這一點，目前還無法解決，希望有哪位研究過這類問題的同學可以解答一下！！
##2.2運動先驗
視覺靜止的先驗模型：SVO 、LSD-SLAM
勻速運動的先驗模型：ORB-SLAM、PTAM、DSO
在視覺慣性聯合初始化之後，IMU就可以光明正大地參與運動跟蹤了。
在VIO中，通常都會拿IMU作為運動先驗，因為在聯合初始化完成之後，陀螺儀和加速度計的偏置都可以被估計出來啦，而且重力矢量也可觀測。此時的IMU除了一些小的測量誤差以及器件的随機遊走之外，基本上是沒什麼其他誤差的。由于兩幀圖像之間的時間很短，其間的傳感器偏差可視為定值，通過IMU測量資料預積分，可以在新一幀圖像到來之前對運動進行先驗估計。
SVO原程式中，采用的是位姿變化為0的先驗模型，也就是靜止模型：将上一幀位姿作為目前幀的位姿先驗，進行圖像比對。這種方法雖然在能動性上差了一點但是也是有好處的，比如在運動緩慢的情況下、或者靜止的情況下，視覺靜止模型就表現得非常穩定。
而反觀IMU作為先驗，雖然在機體快速運動過程中IMU的測量精度很高，但是也要考慮緩慢運動狀态下的IMU漂移可能引入累積誤差的情況。
出于這一點考慮，我認為将IMU先驗模型與其他靜态更穩定的模型進行組合，再作為運動先驗是一個比較合适的想法。
此處說明一下先驗模型的誤差計算方法：
資料集中一般都提供了groundtruth，使用其中的姿态四元數以及位移矢量作為真值。但是要驗證運動先驗額準确性的話需要基于上一幀的位姿，為避免位姿估計誤差的影響，可以采用計算單幀運動增量的方法驗證先驗模型。
通過先驗模型估計運動增量，再與真實增量求絕對誤差，通過整條運動軌迹求單幀先驗誤差可以得到關于先驗模型誤差的對比曲線。
##2.3後端非線性優化
到上述為止都是關于前端的内容，視覺也好，IMU測量也好，主要目的都是為了估計目前幀的位姿。
在處理完最新一幀圖像之後，就要進入一個局部的優化環節。
SVO原程式中在bundle_adjustment.cpp中是有寫優化的内容的，但是沒有使用，他自帶的local_ba也是采用一種比較簡單的局部優化方法。
當看到VI-ORB中的Optimizer.cc中的各種圖優化函數時，我淩亂了···加了IMU測量的圖優化模型開始有了千奇百怪的頂點和邊，趕緊補了一波g2o的知識，在其他CSDN上可以找到一些特定類型的函數模闆，但是對應到自身程式上時還是需要進一步的加工和考量。
定義頂點和邊的時候比較容易，确定好參與優化的變量以及頂點類型基本上就沒什麼問題；邊的話要确定好每個邊關聯的頂點以及誤差項的計算方法，接下去就直接調用g2o庫就可以了，但其中的部分參數還是有點傷腦筋。例如設定誤差邊時的資訊矩陣該如何選取？資訊矩陣代表的是這個測量值的可信程度，資訊矩陣的大小影響着對應邊的誤差在整個目标函數中的占比。參考版本函數大多都設為weight*InvCov（權重乘協方差的逆）這樣的形式，但是當一個圖中含有多種類型的邊時，不同邊的權重需要如何權衡，如何在多類型邊的圖中選擇合适的權重可能要經過較長時間的測試進行調整。
在視覺後端優化中，為了提高計算效率，通常采用的是滑動視窗優化的方法。SVO 僅在關鍵幀上提取特征點，是以考慮使用基于關鍵幀的滑動視窗進行優化。
是以我在localBA的基礎上又添加了一個windowBA的函數，在windowBA中結合了IMU資料進行聯合優化，具體步驟類似VI-ORB中的Optimizer::LocalBundleAdjustmentNavState,用到了IMU的PVR以及bias。
就運作結果來說，沒有想象中那麼好。
每确定一個關鍵幀後都要進行一次滑窗優化顯然有些浪費資源，并且圖優中包含各種誤差邊的計算、變量優化反複疊代，消耗了較長的時間。
從效果層面，滑窗确實可以提高局部運動的魯棒性，但是無法避免誤差累積。 顯然依靠簡單的滑窗是無法達到高精度的目标的，尤其是在運動劇烈、旋轉較大的資料集上，優化的方法經常出現無法運作的情況···
#總結
目前SVO與IMU融合的算法雖然在部分資料集中可以跑出比較好的效果，但還是存在很多漏洞和不當的地方。隻能說搭建好了架構，可以使用，但無法普遍适用，後續将持續進行改進···