本講是VINS最核心部分了，前面經歷了
1）視覺跟蹤feature_tracker、IMU預積分integrationBase類；
2）初始化中SFM純視覺估計滑動窗中所有幀的位姿和3D路標點深度、SFM與IMU預積分鬆耦合對齊求解初始化參數。
在完成前面的初始化環節後，本節將進行第3部分，基於滑動窗的緊耦合後端非線性優化：將視覺約束、IMU約束、閉環約束放到一個大的目標函數中進行非線性優化，求解出滑動窗口中所有幀的PVQ、bias等。
閱讀本文所需知識儲備：
2、《視覺SLAM十四講精品總結》4：非線性優化g2o(BA基礎)
目錄
一、VIO殘差函數的構建
1、需要優化的狀態向量：
滑動窗口內IMU狀態（PVQ、加速度bias、陀螺儀bias）、IMU到Camera的外參、m+1個3D路標點逆深度。
第一個式子是滑動窗口內所有狀態量，n是關鍵幀數量，m是滑動窗內所有觀測到的路標點總數，維度是15*n+6+m。特徵點逆深度爲了滿足高斯系統。
第二個式子xk是在第k幀圖像捕獲到的IMU狀態，包括位置，速度，旋轉（PVQ）和加速度偏置，陀螺儀偏置。
第三個式子是相機外參。
注意：xk只與IMU項和Marg有關；特徵點深度也只與camera和Marg有關；
2、目標函數爲：
視覺慣性BA：這三項分別爲邊緣化的先驗信息、IMU的測量殘差、視覺的重投影誤差
BA優化模型分爲三部分：
1、Marg邊緣化殘差部分（滑動窗口中去掉位姿和特徵點約束）代碼中使用Google開源的Ceres solver解決。
2、IMU殘差部分（滑動窗口中相鄰幀間的IMU產生）
3、視覺誤差函數部分（滑動窗口中特徵點在相機下視覺重投影殘差）
二、視覺約束
這部分要擬合的目標可以通過重投影誤差約束，求解的是對同一個路標點的觀測值和估計值之間的誤差，注意是在歸一化平面上表示。
1.視覺重投影誤差
相關自變量是：該路標點被兩幀觀察到的相機幀位姿、Cam和IMU之間相對位姿、路標點在第一幀的逆深度。
當某路標點在第i幀觀測到並進行初始化操作得到路標點逆深度，當其在第j幀也被觀測到時，估計其在第j幀中的座標爲：
此時的視覺殘差爲：（左側爲根據i幀反推估計的位置，右側爲觀測值）
2、逆深度參數化
逆深度作爲參數原因：1）觀測到的特徵點深度可能非常大，難以進行優化；2）可以減少實際優化的參數變量；3）逆深度更加服從高斯分佈。這裏特徵點的逆深度在第i幀初始化操作中得到。
論文中：
上面的公式是傳統的針孔相機模型，這裏用的是單位半球體的相機觀測殘差。是一個魚眼相機。
在相機的歸一化平面上比較殘差，再將視覺殘差投影到單位球面的正切平面上。由於視覺殘差的自由度是2，所以我們將殘差向量投影到切平面上。
在第i幀第一次觀測到第l個路標點，在第j幀中對該路標點進行觀測的殘差爲：
第一個式子就是殘差的表達式，第二個式子是魚眼相機反投影函數將觀測到的像素座標轉換成單位向量的觀測值數據，b1和b2是此單位向量的切平面上的一組基。第三個式子是重投影估計模型。其實VINS代碼中也可以使用普通的針孔相機模型。
三、IMU約束
1、殘差：
狀態量傳播預測與IMU預積分的殘差。
2、優化變量：
IMU的第i、j時刻下的p位置，v速度，Q旋轉（PVQ），兩個偏置ba,bw
之前對IMU預積分進行了詳細講解。參考：VINS-Mono 代碼詳細解讀——IMU離散中值預積分
3、IMU測量殘差公式推導
簡單來說，如果每一幀IMU都要相對於第一幀求位姿，計算量很大；考慮將PVQ積分公式改爲相對第i時刻的姿態。
原PVQ積分公式：
基於預積分的PVQ積分公式
預積分量爲：
最新的PVQ積分公式變爲：
IMU測量誤差爲：
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 重定位
漂移發生在全局三維位置(x,y,z)和圍繞重力方向的旋轉(yaw)。漂移發生在全局三維位置(x,y,z)和圍繞重力方向的旋轉(yaw)。
參考文章：
VINS-Mono理論學習——後端非線性優化 by Manii
VINS-mono詳細解讀 by 極品巧克力
深藍學院手寫VIO課程
https://blog.csdn.net/iwanderu/article/details/104729332
https://blog.csdn.net/u012871872/article/details/78128087
https://blog.csdn.net/huanghaihui_123/article/details/87361621