參考這個：
https://blog.csdn.net/wangshuailpp/article/details/78461171

本文旨在對前一階段學習vins-mono開源框架的總結。結合暑假秋招之前報名的深藍學院的《從零開始手寫VIO》課程，本文從VIO原理以及開源代碼分析兩部分進行詳細介紹。PS：提升代碼能力最好的辦法就是從頭到尾擼開源框架，這種體驗是單純推導公式體會不到的。
本文只是VINS-Mono的目錄部分，具體內容鏈接如下，共分爲12部分，對應系統的4大部分：圖像和IMU預處理、初始化、後端滑動窗口優化、閉環檢測和優化。

一、文章概述及框架

Vins-mono是香港科技大學開源的一個VIO算法，用緊耦合的方法，通過單目+IMU恢復出尺度，效果非常棒。
**VINS的功能模塊可包括五個部分：數據預處理、初始化、後端非線性優化、閉環檢測及閉環優化。**代碼中主要開啓了四個線程，分別是：前端圖像跟蹤、後端非線性優化（其中初始化和IMU預積分在這個線程中）、閉環檢測、閉環優化。

1 圖像和IMU預處理

圖像：提取圖像Harris角點，金字塔光流跟蹤相鄰幀，RANSAC去除異常點，最後將跟蹤到的特徵點push到圖像隊列中，並通知後端進行處理。
IMU： 1）IMU積分，得到PVQ 位置、速度、旋轉， 2）計算在後端優化中將用到的相鄰幀的預積分增量， 3）計算預積分誤差的Jacobian矩陣和協方差。

2 初始化

SFM純視覺估計滑動窗所有幀的位姿和3D路標點逆深度 SFM與IMU預積分松耦合，對齊求解初始化參數。

3 後端滑動窗口優化

將視覺約束、IMU約束和閉環約束放在一個大的目標函數中進行非線性優化，求解滑動窗內所有幀的PVQ、bias。

4 閉環檢測和優化

DBow進行閉環檢測，檢測成功後重定位，最後對整個相機軌跡進行閉環優化。

二、VINS-Mono論文以及代碼詳細解讀

三、代碼的文件目錄

1、ar_demo：一個ar應用demo
2、benchmark_publisher：接收併發布數據集的基準值
3、camera_model
   calib：相機參數標定
   camera_models：各種相機模型類
   chessboard：檢測棋盤格
   gpl
   sparse_graph
   intrinsic_calib.cc：相機標定模塊main函數
4、config：系統配置文件存放處
5、feature_trackers：
   feature_tracker_node.cpp ROS 節點函數，回調函數
   feature_tracker.cpp 圖像特徵光流跟蹤
6、pose_graph：
   keyframe.cpp 關鍵幀選取、描述子計算與匹配
   pose_graph.cpp 位姿圖的建立與圖優化
   pose_graph_node.cpp ROS 節點函數，回調函數，主線程
7、support_files：幫助文檔、Bow字典、Brief模板文件
8、vins_estimator
   factor：實現IMU、camera等殘差模型
   initial：系統初始化，外參標定，SFM
   utility：相機可視化，四元數等數據轉換
   estimator.cpp：緊耦合的VIO狀態估計器實現
   estimator_node.cpp：ROS 節點函數，回調函數，主線程
   feature_manager.cpp：特徵點管理，三角化，關鍵幀等
   parameters.cpp：讀取參數
————————————————

四、論文介紹

VIO引出原因：
1）單純視覺： 缺點: 尺度不確定性、單目純旋轉無法估計、快速運動易丟失、受圖像遮擋運動物體干擾。
優點：不產生漂移、直接測量旋轉與平移。
2）單純IMU： 缺點：零偏導致漂移、低精度IMU積分位姿發散
優點：快速響應、可估計絕對尺度、角速度估計準確。
3）結合視覺+IMU： 可用視覺彌補IMU的零偏，減少IMU由於零偏導致的發散和累計誤差，IMU可爲視覺提供快速響應的定位。
融合方案：

1、松耦合： 將 IMU 定位與視覺的位姿直接後處理融合，融合過程對二者本身不產生影響，典型方案爲卡爾曼濾波器.

2、緊耦合： 融合過程本身會影響視覺和 IMU 中的參數（如 IMU 的零偏和視覺的尺度）典型方案爲 MSCKF 和非線性優化。

本文貢獻：
1、一個緊耦合、基於優化的單目視覺慣性里程計，具有相機-IMU外部校準和IMU偏置估計。
2、基於有界滑動窗口迭代進行估計。
3、基於滑動窗口裏的關鍵幀維持視覺結構，基於關鍵幀之間的IMU進行預積分維持慣性測量。
4、魯棒性：未知狀態的初始化、相機和IMU外參數的在線標定、球面不統一重投影誤差、迴環檢測、四自由度位姿圖優化（三位置和航向）。

0、總體框架

包括五個部分：數據預處理、初始化、後端非線性優化、閉環檢測、位姿圖優化。

0.1 數據預處理（IMU預積分見標題1）

視覺：
1）提取Harris角點，KLT金字塔光流跟蹤相鄰幀；
2）2 維特徵點先矯正爲不失真的，然後在通過外點剔除後投影到一個單位球面上；
3）去除異常點：先進行F矩陣測試，通過RANSAC去除異常點；
4）關鍵幀選取： 1、當前幀相對最近的關鍵幀的特徵平均視差大於一個閾值就爲關鍵幀（因爲視差可以根據平移和旋轉共同得到，而純旋轉則導致不能三角化成功，所以這一步需要IMU預積分進行補償）2、當前幀跟蹤到的特徵點數量小於閾值視爲關鍵幀；
IMU：
1）兩幀k和k+1之間進行位置、速度、姿態（PVQ）預測；
2）避免每次姿態優化調整後重復IMU傳播，採用預積分算法，計算預積分誤差的雅克比矩陣和協方差項。

0.2 初始化
採用松耦合的傳感器融合方法得到初始值。首先用SFM進行純視覺估計滑動窗內所有幀的位姿以及路標點逆深度，然後與IMU預積分對齊，繼而恢復對齊**尺度s，重力g，imu速度v，和陀螺儀偏置bg。**VINS本文初始化過程中忽視掉了加速度計的bias，因爲加速度計與重力耦合，並且重力向量很大，初始化過程動態過程很短，幅度又不大，加速度計偏置很難觀測到。

A.滑動窗口(Sliding Window)純視覺SfM
1、選擇一個滑動窗，在最後一幀與滑動窗之前幀尋找幀：跟蹤到的點數目大於30個的並且視差超過20的，找到後用5點法本質矩陣初始化恢復出R和t。否則，滑動窗內保留最新圖像幀，繼續等待下一幀。
2、隨意設置一個尺度因子，三角化這兩幀觀測到的所有路標點。再用PnP算法估計滑動窗內所有其餘幀的位姿。滑動窗內全局BA重投影誤差優化所有幀位姿。
3、假設IMU-Camera外參已知，乘上視覺得到的位姿，轉換到IMU座標系下。
B. 視覺慣性校準（IMU預積分與視覺結構對齊）
1、陀螺儀零偏bg標定
旋轉兩種方式： 陀螺儀測量值和視覺觀測值，二者的誤差其實就是陀螺儀偏置bg。
目標函數： visual給出的相鄰幀間的旋轉應等於IMU預積分的旋轉值Q之間的差。

我們得到了陀螺儀偏置bias的初始校準,需要將陀螺儀偏置bg代入到IMU預積分重新計算預積分。

2、速度v、重力g和尺度初始化s

優化變量：速度、重力向量和尺度

目標函數： 相鄰兩幀IMU預積分增量與預測值之間平移、速度（P、V）的差。通過HX=B 利用cholesky分解獲得

結合

通過求解線性最小二乘問題：

3、重力矢量修正
重力向量的大小是已知的，加入了模長限制||g^c0|| = 9.81，這導致三維重力向量只剩2個自由度。
主要做的是優化方向，一個二維向量。

在其切線空間上用兩個變量重新參數化重力，採用球面座標進行參數化：

其中，||g||是已知的重力的大小，爲重力方向的單位向量。b1和b2是跨越切平面的兩個正交基。w1和w2是待優化變量，表示沿着兩個正交基方向的位移。

替換後，Hx=b，變化爲：之後採用最小二乘對變量重新優化。
其中，待優化變量變爲：

0.3 緊耦合後端非線性優化（IMU約束+視覺約束+閉環約束）
初始化後，採用基於滑動窗口的緊耦合單目VIO進行狀態估計。
A 公式

第一個式子是滑動窗口內所有狀態量，n是幀數，m是滑動窗口內特徵點總數。特徵點逆深度爲了滿足高斯系統。
第二個式子xk是在第k幀圖像捕獲到的IMU狀態，包括位置，速度，旋轉（PVQ）和加速度偏置，陀螺儀偏置。
第三個式子是相機外參。
xk只與IMU項和Marg有關；特徵點深度也只與camera和Marg有關；

視覺慣性BA，最小化邊緣化的先驗信息和IMU、視覺測量殘差之和。
BA優化模型分爲三部分：
1、Marg邊緣化殘差部分（滑動窗口中去掉位姿和特徵點約束）
2、IMU殘差部分（滑動窗口中相鄰幀間的IMU產生）
3、視覺代價誤差函數部分（滑動窗口中特徵點在相機下視覺重投影殘差）
B IMU殘差
殘差：狀態量傳播預測與IMU預積分的殘差.
優化變量：IMU時刻下的p位置，v速度，Q旋轉，兩個偏置ba,bw.

C 視覺殘差
與傳統的針孔相機模型不同，這裏用的是單位半球體的相機觀測殘差。是一個魚眼相機。
在相機的歸一化平面上比較殘差，再將視覺殘差投影到單位球面的正切平面上。由於視覺殘差的自由度是2，所以我們將殘差向量投影到切平面上。

在第i幀第一次觀測到第l個路標點，在第j幀中對該路標點進行觀測的殘差爲：

第一個式子就是殘差的表達式，第二個式子是魚眼相機反投影函數將觀測到的像素座標轉換成單位向量的觀測值數據，b1和b2是此單位向量的切平面上的一組基。第三個式子是重投影估計模型。其實VINS代碼中也可以使用普通的針孔相機模型。
D 邊緣化 Marginalization（詳見標題2）
爲了防止pose和特徵的個數的複雜度隨着時間不斷增長，引入邊緣化，在移除位姿時將關聯的約束轉化爲先驗放入優化問題中。
爲了限制基於優化的VIO計算複雜度，引入邊緣化。有選擇地從滑動窗口中將IMU狀態xK和特徵λ1邊緣化，同時將對應於邊緣狀態的測量值轉換爲先驗。

分爲兩種情況，
1、一種是倒數第二幀如果是關鍵幀的話，將最舊的pose移出Sliding Window，將最舊幀關聯的視覺和慣性數據邊緣化掉。把第一個老關鍵幀及其測量值被邊緣化；Margin_Old作爲先驗值。
2、如果倒數第二幀不是關鍵幀的話，那麼就**只剔除倒數第二幀的視覺觀測，而不剔除它的IMU約束。**原因是邊緣化保證關鍵幀之間有足夠視差而能夠三角化足夠多的地圖點。並且保證了IMU預積分的連貫性。
爲了保持系統的稀疏性，我們不會邊緣化非關鍵幀的所有測量值。

E 相機速率下的狀態估計–只有運動的VIO
採用了一種輕量級的純運動視覺慣性BA，以提升狀態估計速率到相機速率(30Hz)。
代價函數不變。
1、只對固定數量的最新IMU狀態的姿態pose和速度v進行了優化，而不是對滑動窗口中的所有狀態進行優化
2、將特徵深度、外部參數、偏置和舊的IMU狀態這些不希望優化的狀態作爲常量來處理。
與在最先進的嵌入式計算機上可能導致超過50ms的完全緊耦合單目VIO不同，這種純運動的視覺慣性BA只需大約5ms來計算。

0.4 重定位
儘管滑動窗和邊緣化減小了計算複雜度，但是仍舊引進了系統的累計漂移誤差。具體來說，就是全局三維位置（xyz）和圍繞重力方向的旋轉（yaw）。作者採用緊耦合重定位模塊與單目VIO進行組合實現漂移誤差的消除。
目的：局部滑動窗口移動並與過去的位姿對齊。
vins的重定位模塊主要包含迴環檢測,迴環候選幀之間的特徵匹配,****緊耦合重定位三個部分.

上圖展示了重定位步驟。
1中VIO啓動時刻只進行位姿估計（藍色部分），過去狀態一直被記錄（綠色部分）。2中如果最新幀中迴環被檢測到，呈現紅色虛線連接，表示啓動重定位3。4中多個特徵的多個觀測直接用於重定位，從而提高了定位的精度和狀態估計的平滑性。5-7是位姿優化。

A、迴環檢測（只對關鍵幀）

1、採用DBoW2****詞袋位置識別方法進行迴環檢測。經過時間空間一致性檢驗後，DBoW2返回迴環檢測候選幀。
2、除了用於單目VIO的角點特徵外，還添加了500個角點並使用BRIEF描述子，描述子用作視覺詞袋在數據庫裏進行搜索。這些額外的角點能用來實現更好的迴環檢測。
3、VINS只保留所有用於特徵檢索的BRIEF描述子，丟棄原始圖像以減小內存。
4、單目VIO可以觀測到滾動和俯仰角，VINS並不需要依賴旋轉不變性。

B、迴環候選幀之間的特徵匹配

1、檢測到迴環時，通過BRIEF描述子匹配找到對應關係。但是直接的描述子匹配會導致很多外點。
2、本文提出兩步幾何剔除法：
1）2D-2D：使用RANSAC進行F矩陣測試，
2）3D-2D：使用RANSAC進行PnP，基於已知的滑動窗特徵點的3D位置，和迴路閉合候選處圖像的2D觀測（像素座標）。
當內點超過一定閾值時，我們將該候選幀視爲正確的循環檢測並執行重定位。

C、緊耦合重定位

1、重定位過程使單目VIO維持的當前滑動窗口與過去的位姿圖對齊。
2、將所有迴環幀的位姿作爲常量，利用所有IMU測量值、局部視覺測量和從迴環中提取特徵對應值，共同優化滑動窗口。

和之前VIO優化模型不同的是，增加了迴環項，從位姿圖獲得迴環幀的姿態被視爲常數。
在重定位之後（重定位只是基於檢測到的迴環處暫時對滑動窗裏的位姿進行重新優化）下一步要對過去位姿和閉合迴路圖像幀的全局優化。

0.5 全局位姿圖優化
這一步是爲了確保基於重定位結果對過去的位姿進行全局優化。
由於視覺-慣性使得橫滾角和俯仰角完全可以觀測，因此只有（XYZ和yaw航向）4個自由度存在累積漂移。接下來只進行4-DOF的位姿圖優化。

A、位姿圖中添加關鍵幀
當一個關鍵幀被滑動窗口中邊緣化掉後，它會被添加到位姿圖中。該關鍵幀會作爲位姿圖中一個定點，通過下面兩類邊與其他頂點相連接：

1、順序邊（Sequential Edge）： 關鍵幀將建立與之前關鍵幀的幾個順序邊，一個順序邊表示局部滑動窗口中兩個關鍵幀之間的相對轉換，它的值直接從VIO中獲取。令最新邊緣化掉的關鍵幀爲i，它的一個以前的關鍵幀爲j，順序邊只包含相對位置和相對航向.

2、迴路閉合邊（Loop Closure Edge）： 如果最新的邊緣化掉的關鍵幀存在迴路連接，它可以通過位姿圖中的迴路比河邊和迴路閉合幀相連接。迴環邊的值由重定位結果得出。
B、4自由度位姿圖優化
關鍵幀i和j之間的殘差最小化表示爲；

通過最小化以下代價函數，對順序邊和迴環邊的整個圖進行優化：

S是所有順序邊的集合，L是迴環邊的集合。 儘管緊耦合的重定位已經有助於消除錯誤的迴環，但我們添加了另一個Huber範數 ρ(·)，以進一步減少任何可能的錯誤迴環的影響。相反，我們不對順序邊使用任何魯棒範數，因爲這些邊是從VIO中提取出來的，VIO已經包含了足夠多的外點排除機制。
位姿圖優化和重定位(VII-C)異步運行在兩個獨立的線程中。以便在需要重定位時，能立即使用最優化的位姿圖。同樣，即使當前的位姿圖優化尚未完成，仍然可以使用現有的位姿圖配置進行重新定位。這一過程如圖9(b)所示。
C、位姿圖管理
隨着行程距離的增加，位姿圖的大小可能會無限增長，從而限制了長時間系統的實時性。爲此，我們實行了一個下采樣過程：將位姿圖數據庫保持在有限的大小。**所有具有迴環約束的關鍵幀都將被保留，而其他與相鄰幀過近或方向非常相似的關鍵幀可能會被刪除。**關鍵幀被移除的概率和其相鄰幀的空間密度成正比。

1、IMU預積分

1.1、IMU模型

測量值：加速度計a^{、陀螺儀w}, 加上了bias遊走和隨機白噪聲。
真實值：加速度計a、陀螺儀w。
實際情況下，可以獲得測量值a^和w，需要反推真實值。一般忽略隨機遊走高斯噪聲n
w=w^-bg; a=qwb(a^-ba)-gw;

1.2、連續時間IMU運動模型，積分 PVQ（兩幀之間）

將第k幀和第k+1幀所有的IMU進行積分，可得到第k+1幀的 PVQ，作爲視覺估計的初始值。

a和w是IMU測量的加速度和角速度，相對於Body座標系。

1.3、運動模型的離散積分（前後IMU）

從第 i個IMU時刻到第 i+1個IMU時刻的積分過程。兩個相鄰時刻k到k+1的位姿是由第k時刻測量值a^,w計算得出的。
這與Estimator::processIMU（）函數中Ps[j]、Rs[j]、Vs[j]是一致的，代碼中j就是此處的i+1
IMU積分出來第 j 時刻數值作爲第 j 幀圖像初始值。

歐拉法

中值法

1.4、 IMU預積分

每次qwbt優化更新後，都要重新進行積分，運算量較大。
將積分模型轉爲預積分模型：
**PVQ積分公式中的積分項變爲相對於第i時刻的姿態，**而不是相對於世界座標系的姿態:

1.5、預積分量

預積分量只與IMU測量值有關。

1.6、預積分誤差

一段時間內IMU構建的預積分量作爲測量值，與估計值進行相減。

1.7、預積分離散形式（IMU增量）

中值法：k到k+1時刻位姿由兩時刻的測量值a w的平均值來計算。

1.8、bias 預積分量（bias發生變化）

因爲 i 時刻的 bias 相關的預積分計算是通過迭代一步一步累計遞推的，可以算但是太複雜。所以對於預積分量直接在 i 時刻的 bias 附近用一階泰勒展開來近似，而不用真的去迭代計算。

https://zhuanlan.zhihu.com/p/60299750
https://blog.csdn.net/u014527548/article/details/86599047

2、基於舒爾補的邊緣化

基於高斯牛頓的非線性優化理論可知，H*delta_x=b可以寫成：

其中，delta_xa和delta_xb分別是希望marg掉的部分和保留部分。
VINS中需要邊緣化滑動窗口中的最老幀，目的是希望不再計算這一幀的位姿或者與其相關的路標點，但是希望保留該幀對窗口內其餘幀的約束關係。我們基於與移除狀態相關的所有邊緣化測量值構造一個先驗。新的先驗項被添加到現有的先驗項中。

2.1、舒爾補

2.2 marg後形成的先驗

xa爲需要marg的變量，假設爲相機pose,我們更關心如何求解希望保留的xb，而不再求解xa（即marg的變量改爲0，左乘時左上是0），這裏是要變化爲上三角。

即：new_H*delta_xb=new_b;
形成新的信息矩陣new_H具體流程：

**注意：**去掉了x1,但是之前和x1相連的所有量x2 x3 x4 x5 在marg掉x1後變得兩兩相連。

2.3 具體例子

2.3.1 原來的信息矩陣H的構成
上述最小二乘問題，對用的高斯牛頓求解爲：

矩陣乘法公式寫成連加：
**雅克比J和信息矩陣H的稀疏性：**由於每個殘差只和某幾個狀態量有關，因此，雅克比矩陣求導時，無關項的雅克比爲 0。

將五個殘差的信息矩陣加起來，得到樣例最終的信息矩陣 Λ, 可視化如下:

2.3.2 舒爾補後形成新的信息矩陣new_H,並構造爲先驗

2.3.3 新測量信息和先驗構成新的系統

參考文獻：
VINS技術路線與代碼詳解 by 五行缺帥wangshuailpp
VINS-Mono論文學習與代碼解讀——目錄與參考 by Manii
VIO-Doc——崔華坤
 VINS-mono詳細解讀 by 極品巧克力