轉自https://blog.csdn.net/wangshuailpp/article/details/80640663
ORB_SLAM2的作者在2017年提出了具有地圖重用功能的單目視覺慣性定位算法，該算法的具體思路和港科大的VINS有着異曲同工之妙，整體過程可分爲下面幾個部分：
1.整體流程與基礎知識總結
2.基於流型的IMU預積分
3.IMU初始化（視覺慣性聯合初始化）
4.緊耦合優化模型
下面主要通過這三個方面對整個思路做詳細的說明。還是按照VINS相同的方式來講解，首先對整個理論知識做詳細的分析，然後從代碼角度做深入理解。
其中代碼參考王京大神的開源方案：
https://github.com/jingpang/LearnVIORB
論文參考主要下面兩篇：
（1）Visual-Inertial Monocular SLAM with Map Reuse
（2）On-Manifold Preintegration for Real-Time Visual–Inertial Odometry
1.整體流程與基礎知識總結
（1）整體框架
對於整個ORB_SLAM2的框架大家有一定的瞭解，主要分爲三個線程Tracking、LocalMapping和Loopclosing。我對VIO這塊的理解目前只侷限於前兩個線程，因此整體框架就包含前兩個線程中的理解。
（2）基礎知識總結
這裏所提到的基礎知識都會在下面的理論推導公式中使用，因此都是十分重要的公式，需要牢記。
①叉積的基本性質
叉積的定義式
叉積的一個最重要的性質，在後面的雅各比矩陣推導中起到至關重要的作用，需要謹記
②指數一階泰勒近似
這個式子就是以前數學中指數一階泰勒展開，很簡單，因爲旋轉涉及到李羣和李代數的指數轉換關係，所以這個式子在後面的優化雅各比推導中很重要。
③李羣和李代數指數轉換關係
李羣和李代數是指數映射關係，是三維向量（旋轉向量）與三維旋轉矩陣之間的轉換，上圖中可以看到流型的李羣上的切平面（李代數）可以由李羣一階近似（李羣擾動表示李代數擾動）
上面的公式參考視覺SLAM十四講中，原文預積分中給出的是李代數“向量”形式，感覺不太好理解，所以還是用反對稱矩陣來表示，其中Jr是李代數上的右雅各比
同樣的李代數擾動可以表示李羣擾動
指數映射還有另外一個性質，在李羣原點處的一階展開可以推導下面的公式，性質和推導攻入如下
其中注意有這個性質：R*p^*RT = exp((Rp)^)
2.基於流型的IMU預積分
這一部分主要參考論文（2）中的內容，ORB_SLAM2融合慣性數據的算法中預積分處理是參照論文（2）中實現的。前面關於VINS的詳解說過預積分計算過程，那篇使用的是四元數旋轉求解，和這裏的過程是一樣的，主要計算三個量，分別是IMU預積分值，預積分測量值的協方差矩陣以及殘差的雅各比矩陣，各自用途就不多說，有不懂的可以參照VINS的詳解
https://blog.csdn.net/wangshuailpp/article/details/78461171
（1）IMU預積分計算
世界座標系W、IMU座標系B以及相機座標系C的關係和轉換如下圖2所示，所有的位姿和特徵點座標最終都是在世界座標系下表示，我們會將相機座標系下的的狀態量首先轉換到IMU座標系下然後再轉換到世界座標系下。TBC代表在IMU座標系B下相機到IMU之間的外參轉換矩陣，TWB=(RWB,wp)代表世界座標系W下IMU到世界座標的轉換矩陣。關於座標之間的關係比較重要，剛接觸會感覺比較複雜，需要多思考。下面所有的公式和變量前下標代表參考座標系，例如wp就是世界座標系下的位移，後下標第一個下標是參考座標系，例如RWB是世界座標系下。
由IMU的加速度計和陀螺儀可以獲得IMU座標系下的加速度Ba和角速度BwWB，可以表示成下面的公式（27）（28），公式左邊是IMU直接輸出的數據。
旋轉、平移和速度的導數可表示爲公式（29）
世界座標系下的旋轉、平移和速度可由一般的積分公式求得
離散時間下采用歐拉積分可以將上面連續時間積分改寫成下面式（30）
聯立公式（27）（28）可以得到公式（31）
這時候狀態量是世界座標系下的。設現在有i和j兩個相鄰關鍵幀，需要已知i時刻對應的IMU狀態量，求解離散時間下j時刻關鍵幀對應的IMU預積分值，
爲避免重複積分採用IMU預積分方法計算i、j時刻的狀態量的相對變化。也就是將參考座標系由世界座標系轉換到IMU的i時刻座標系下，計算得到IMU狀態量在i和j時刻之間的相對量，這個相對量是隻取積分算式內的值，而不是旋轉、速度和位移的變化量，和VINS的處理方式相同。
從上面的公式中可以看出，整個預積分公式和偏置，噪聲都有關係，相當於一個二元方程，因此需要分別分析偏置和噪聲對系統的影響，噪聲服從高斯正態分佈，相對來說比較簡單，而偏置服從隨機遊走模型，關係很複雜，因此兩者的處理方式不同。對於噪聲分量採用噪聲高斯傳播方程來求解，而偏置採用一階泰勒展開方式。總體思路是首先將偏置在i時刻的bi處進行一階展開，得到公式（44）
這裏的數值就是IMU最終的預積分數值，但值得注意的是在初始化使用的IMU預積分是沒有偏置的一階項，也就只是等式右邊前面一項而已，等式後面的雅各比矩陣是在初始化完成後進行優化之前才計算而來，得到IMU準確的預計分值（真值），和估計值從而得到IMU的殘差。所以我們首先要手推一下上面公式右邊的前面一項，注意這裏所有的指數映射中都省去了寫反對稱符號。
至此IMU預積分的數值就計算出來，也就是等式右邊的第一項。
（2）預積分雅可比和協方差矩陣
下面需要計算出噪聲傳播項的協方差矩陣和對應偏置的雅各比矩陣。這部分推導在附錄部分A和B，和VINS的過程出奇的一致，只不過這裏是將狀態量分開討論的，分別是（deta_Rj,deta_vj,deta_pj）和（deta_Rj-1,deta_vj-1,deta_pj-1）、（nba,nbg）以及（deta_Rj,deta_vj,deta_pj）和（deta_ba,deta_bg）的關係，把兩者合併就和VINS相同。，這部分推導相對簡單就直接貼公式了。
下面是求協方差矩陣：
系統是服從高斯分佈的，協方差矩陣是按照高斯分佈計算得到，是個9*9的矩陣。
下面是求預積分對偏置的雅各比矩陣：
（3）殘差雅可比
位移殘差對應的雅可比如上圖所示，J時刻相應的雅可比可按照圖片方式進行推導，注意對pi和pj時刻的雅可比公式原文有錯誤，推導過程已經給出。對於速度殘差和旋轉的雅可比公式與平移相似，就不進行推導直接給出，至此IMU預計分就完成了。
3.IMU初始化（視覺慣性聯合初始化）
終於來到視覺慣性初始化階段了，這段是視覺和慣性數據融合的第一步，是一段鬆耦合過程。理論上是將準確的視覺數據（但缺少尺度）和快速的IMU數據（需要重力加速度又存在零偏誤差）相結合。具體介紹在VINS的博客中也同樣說過，這部分關於最小二乘、尺度收斂等問題請參考我的博客：視覺SLAM常見的QR分解SVD分解等矩陣分解方式求解滿秩和虧秩最小二乘問題（最全的方法分析總結）
https://blog.csdn.net/wangshuailpp/article/details/80209863
（1）陀螺儀偏置標定（零偏）
這一部分比較簡單，直接聯立N-1個相機做旋轉矩陣的最小二乘即可，然後通過高斯牛頓方法即可得到零偏bg。需要注意一點，當求出零偏後將其代入預積分公式會重新計算一遍預積分值，使預積分數值更加準確。
（2）尺度恢復和重力加速度預估
首先建立預估狀態向量X=[s,gw]，其中s是尺度，gw是世界座標系下的重力加速度也是第一個相機座標系下的重力加速度。ORB_SLAM2中世界座標選取的是第一個相機對應的座標（VINS則不同），這樣做會存在一個問題，因爲第一個相機可能自身存在一定的旋轉傾斜導致整個世界座標看起來是歪着的，畫軌跡的時候有一種傾斜的即視感，所以我覺得還是儘量固定好z方向，使軌跡沒有橫滾和俯仰。這裏使用了三個關鍵幀聯立視覺和IMU預積分數據構建一個AX=B的最小二乘超定方程，至少需要四個關鍵幀，採用奇異值分解求最小二乘問題，速度較慢但精度高。
（3）加速度計偏置標定和尺度重力加速度優化
上面計算過程沒有考慮到加速度計偏置的影響，使得重力加速度和加速度計偏置難以區分，很有可能會導致系統病態性問題，文中提出了重力加速度的大小G，假設其是一個不變值，優化重力加速度的方向。
最後優化的狀態量變成了X=[s,deta_θ,ba],也就是說我們只優化重力加速度的方向，每次優化得到一個deta_θ都會代入到論文中的公式（17）計算出最新的gw然後繼續優化，直到變量收斂。
4.緊耦合優化模型
視覺慣性緊耦合優化部分分爲三個部分，分別是Tracking、LocalMapping和Loopclosing，我只是學習了前兩個部分，所以就簡單的介紹下這兩個內容
（1）Tracking線程中幀間緊耦合優化
論文中說的也比較清楚，分爲兩中情況，分別是有地圖更新和無地圖更新。Tracking線程中一開始是沒有地圖更新的，地圖更新是在LocalMapping和LoopClosing中完成。因此這兩種方式是隨機切換的。首先講一下優化方法和邊緣化策略
紅色虛框是固定的狀態量（fix），不會進行優化但是會在優化的時候作爲約束項優化其他狀態量，淺灰色實框是將要進行邊緣化的狀態量（marginalize），藍色實框是視覺重投影誤差方程，深灰色實框是無地圖更新時候的先驗數據（prior），綠色實框是IMU測量誤差方程。
（a）是出現地圖更新的狀態，一開始j是當前幀，i是上一幀，通過視覺重投影誤差和IMU測量誤差優化當前幀的狀態量（pj,vj,bj），上一幀的狀態量和Map Points不會進行優化而是作爲約束項優化當前狀態量。
（b）是（a）優化後的結果（估計值和海塞矩陣H），同時將優化結果
（c）是無地圖更新狀態，當前幀變成了j+1，此時通過重投影誤差和IMU測量誤差優化當前幀和上一幀j的狀態量（pj,vj,bj, pj+1,vj+1,bj+1），Map Points不會進行優化而是作爲約束項優化j和j+1時刻的狀態量，同時j時刻的優化結果作爲先驗數據優化，然後將j時刻的狀態量邊緣化掉。
（d）邊緣化掉j時刻的j+1時刻優化量（估計值和海塞矩陣H）作爲下一時刻優化的先驗數據。
（e）和（c）的過程一模一樣。
（f）和（d）的過程一模一樣。
然後一直循環（c）（d）過程，直到LocalMapping和LoopClosing中發生地圖更新會回到（a）處重複過程。下面給出兩種優化方式的誤差方程：
①出現地圖更新
首先構建整體優化狀態向量，包含當前時刻j的旋轉，平移速度，位移，加速度計偏置和陀螺儀偏置。整體誤差方程包含視覺重投影誤差和IMU測量誤差。
視覺重投影方程就是簡單的針孔相機重投影方程，並沒有什麼特別其殘差，雅可比計算和信息矩陣與一般視覺無異。
IMU測量誤差方程包含兩部分，分別是P,V,R和ba,bg偏置。其殘差，殘差雅可比矩陣和信息矩陣在IMU預積分過程已經結算完成，直接代入即可。
②沒出現地圖更新
按照前面所說，首先構建整體優化狀態向量，包含當前時刻j+1和上一時刻j的旋轉，平移速度，位移，加速度計偏置和陀螺儀偏置。整體誤差方程包含視覺重投影誤差、IMU測量誤差和先驗誤差。
視覺和IMU的誤差方程和前面相同，下面看一下先驗誤差，是j時刻的狀態量。
（2）LocalMapping線程
整個BA優化的只優化固定的N幀，而且這N幀是共視程度最高的N個關鍵幀，在局部窗口中（Local Windows），地圖中所有的點都是這些N個關鍵幀產生，其他的關鍵幀是在固定窗口中（Fixed Window），只提供與局部地圖點的觀測值，不進行優化。當然局部地圖路標點也需要優化。需要優化的關鍵幀包含有視覺重投影誤差和IMU測量誤差。需要優化的特徵點包含Local和Fixed Window的重投影誤差。
