VINS-Mono代碼分析與總結(最終版)
VINS-Mono代碼分析總結
參考文獻
1 VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen (techincal report)
2 Solà J. Quaternion kinematics for the error-state KF[M]// Surface and Interface Analysis. 2015.
3 Solà J. Yang Z, Shen S. Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration[J]. IEEE Transactions on Automation Science & Engineering, 2017, 14(1):39-51.
5 Shen S, Michael N, Kumar V. Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs[C]// IEEE International Conference on Robotics and Automation. IEEE, 2015:5303-5310.
7 Sibley G. A Sliding Window Filter for SLAM[J]. University of Southern California, 2006.
8 CamOdoCal: https://github.com/hengli/camodocal
****
前言
??視覺與IMU融合的分類：
- 松耦合和緊耦合：按照是否把圖像的Feature加入到狀態向量區分，換句話說就是松耦合是在視覺和IMU各自求出的位姿的基礎上做的耦合。
濾波法和優化法：
參考
??Vins-Mono是視覺與IMU的融合中的經典之作，其定位精度可以媲美OKVIS，而且具有比OKVIS更加完善和魯棒的初始化以及閉環檢測過程。同時VINS-Mono也為該鄰域樹立了一個信標吧，視覺SLAM的研究和應用會更新偏向於 單目+IMU。因為在機器人的導航中，尤其是無人機的自主導航中，單目不具有RGBD相機(易受光照影響、獲取的深度信息有限)以及雙目相機(占用較大的空間。。。)的限制，能夠適應室內、室外及不同光照的環境，具有較好的適應性。而且在增強和虛擬現實中，更多的移動設備僅具有單個相機，所以單目+IMU也更符合實際情況。
??那麽為什麽要進行視覺與IMU的融合呢，自己總結的主要有以下幾點：- 視覺與IMU的融合可以借助IMU較高的采樣頻率，進而提高系統的輸出頻率。
- 視覺與IMU的融合可以提高視覺的魯棒性，如視覺SLAM因為某些運動或場景出現的錯誤結果。
- 視覺與IMU的融合可以有效的消除IMU的積分漂移。
- 視覺與IMU的融合能夠校正IMU的Bias。
單目與IMU的融合可以有效解決單目尺度不可觀測的問題。
??上面總結了視覺與IMU融合的幾個優點，以及與單目融合可以解決單目相機尺度不可測的問題。但是單目相機的尺度不可測也是具有優點的，參考[4]。單目尺度不確定的優點主要有兩方面：單目尺度的不確定性，可以對不同的規模大小的環境空間之間進行遊走切換，起到無縫連接的作用，比如從室內桌面上的環境和大規模的戶外場景；諸如深度或立體視覺相機，這些具備深度信息的傳感器，它們所提供的可靠深度信息範圍，是有限制的，故而不像單目相機那樣具有尺度靈活性的特點。
1 預積分的推導
1.1 離散狀態下預積分方程
??關於這部分的論文和代碼中的推導，可以參考文獻[2]中Appendx部分“A Runge-Kutta numerical integration methods”中的歐拉法和中值法。
\[
w_{k}^{{}'}=\frac{w_{k+1}+w_{k}}{2}-b_{w}
\tag{1.1}
\]
\[
q _{i+1}=q _{i}\otimes \begin{bmatrix}
1
\0.5w_{k}^{{}'}
\end{bmatrix}
\tag{1.2}
\]
\[
a_{k}^{{}'}=\frac{q_{k}(a_{k}+n_{a0}-b_{a_{k}})+q_{k+1}(a_{k+1}+n_{a1}-b_{a_{k}})}{2}
\tag{1.3}
\]
\[
\alpha _{i+1}=\delta\alpha _{i}+\beta _{i}t+0.5a_{k}^{{}'}\delta t^{2}
\tag{1.4}
\]
\[
\beta _{i+1}=\delta\beta _{i}+a_{k}^{{}'}\delta t
\tag{1.5}
\]
1.2 離散狀態下誤差狀態方程
??論文中Ⅱ.B部分的誤差狀態方程是連續時間域內，在實際代碼中需要的是離散時間下的方程式，而且在前面的預積分方程中使用了中值法積分方式。所以在實際代碼中和論文是不一致的。在推導誤差狀態方程式的最重要的部分是對 \(\delta \theta _{k+1}\) 部分的推導。
??由泰勒公式可得：
\[
\delta \theta _{k+1} = \delta \theta _{k}+\dot{\delta \theta _{k}}\delta t
\tag{1.6}
\]
依據參考文獻[2]中 "5.3.3 The error-state kinematics"中公式(222c)及其推導過程有：
\[
\dot{\delta \theta _{k}}=-[w_{m}-w_{b}]_{\times }\delta \theta _{k}-\delta w_{b}-w_{n}
\]
對於中值法積分下的誤差狀態方程為：
\[
\dot{\delta \theta _{k}}=-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta \theta _{k}-\delta b_{g_{k}}+\frac{n_{w0}+n_{w1}}{2}
\tag{1.7}
\]
將式(1.7)帶入式(1.6)可得：
\[
\delta \theta _{k+1} =(I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t
\tag{1.8}
\]
這部分也可以參考，文獻[2]中“7.2 System kinematics in discrete time”小節。
??接下來先推導 \(\delta \beta _{k+1}\) 部分，再推導 \(\delta \alpha _{k+1}\) 部分。\(\delta \beta _{k+1}\) 部分的推導也可以參考文獻[2]中“5.3.3 The error-state kinematics”公式(222b)的推導。將式(1.5)展開得到：
\[
\delta\beta _{i+1}=\delta\beta _{i}+\frac{q_{k}(a_{k}+n_{a0}-b_{a_{k}})+q_{k+1}(a_{k+1}++n_{a1}-b_{a_{k}})}{2}\delta t
\]
即，
\[
\delta\beta _{i+1}=\delta\beta _{i}+\dot{\delta\beta_{i}}\delta t
\tag{1.9}
\]
文獻[2]中，公式(222b)
\[
\dot{\delta v}=-R[a_{m}-a_{b}]_{\times}\delta \theta-R\delta a_{b}+\delta g-Ra_{n}
\]
對於中值法積分下的誤差狀態方程為：
\[
\begin{align}\nonumber
\dot{\delta\beta_{i}} =&-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta \theta _{k+1} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t)\delta \theta \\\nonumber
&-\frac{1}{2}q_{k}\delta b_{a_{k}}-\frac{1}{2}q_{k+1}\delta b_{a_{k}}-\frac{1}{2}q_{k}n_{a0}-\frac{1}{2}q_{k}n_{a1}
\end{align}
\tag{1.10}
\]
將式(1.8)帶入式(1.10)可得
\[
\begin{align}\nonumber
\dot{\delta\beta_{i}} =&-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}((I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t) \\\nonumber
&-\frac{1}{2}q_{k}\delta b_{a_{k}}-\frac{1}{2}q_{k+1}\delta b_{a_{k}}-\frac{1}{2}q_{k}n_{a0}-\frac{1}{2}q_{k}n_{a1}
\end{align}
\tag{1.11}
\]
同理，可以計算出 \(\delta \alpha _{k+1}\) ，可以寫為：
\[
\delta\alpha _{i+1}=\delta\alpha _{i}+\dot{\delta\alpha_{i}}\delta t
\tag{1.12}
\]
\[
\begin{align}\nonumber
\dot{\delta\alpha_{i}} =&-\frac{1}{4}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta\delta t-\frac{1}{4}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}((I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t)\delta t \\\nonumber
&-\frac{1}{4}q_{k}\delta b_{a_{k}}\delta t-\frac{1}{4}q_{k+1}\delta b_{a_{k}}\delta t-\frac{1}{4}q_{k}n_{a0}\delta t-\frac{1}{4}q_{k}n_{a1}\delta t
\end{align}
\tag{1.13}
\]
最後是加速度計和陀螺儀bias的誤差狀態方程，
\[
\delta b_{a_{k+1}}=\delta b_{a_{k}}+n_{ba}\delta t
\tag{1.14}
\]
\[
\delta b_{w_{k+1}}=\delta b_{w_{k}}+n_{bg}\delta t
\tag{1.15}
\]
??綜合式(1.8)等誤差狀態方程，將其寫為矩陣形式，
\[
\begin{align}\nonumber
\begin{bmatrix}
\delta \alpha_{k+1}\\delta \theta _{k+1}\\delta \beta _{k+1} \\delta b _{a{}{k+1}} \\delta b _{g{}{k+1}}
\end{bmatrix}&=\begin{bmatrix}
I & f_{01} &\delta t & -\frac{1}{4}(q_{k}+q_{k+1})\delta t^{2} & f_{04}\0 & I-[\frac{w_{k+1}+w_{k}}{2}-b_{wk}]_{\times }\delta t & 0 & 0&-\delta t \0 & f_{21}&I & -\frac{1}{2}(q_{k}+q_{k+1})\delta t & f_{24}\0 & 0& 0&I &0 \ 0& 0 & 0 & 0 & I
\end{bmatrix}
\begin{bmatrix}
\delta \alpha_{k}\\delta \theta _{k}\\delta \beta _{k} \\delta b _{a{}{k}} \\delta b _{g{}{k}}
\end{bmatrix} \\\nonumber
&+
\begin{bmatrix}
\frac{1}{4}q_{k}\delta t^{2}& v_{01}& \frac{1}{4}q_{k+1}\delta t^{2} & v_{03} & 0 & 0\ 0& \frac{1}{2}\delta t & 0 & \frac{1}{2}\delta t &0 & 0\ \frac{1}{2}q_{k}\delta t& v_{21}& \frac{1}{2}q_{k+1}\delta t & v_{23} & 0 & 0 \0 & 0 & 0 & 0 &\delta t &0 \ 0& 0 &0 & 0 &0 & \delta t
\end{bmatrix}
\begin{bmatrix}
n_{a0}\n_{w0}\n_{a1}\n_{w1}\n_{ba}\n_{bg}
\end{bmatrix}
\end{align}
\tag{1.16}
\]
其中，
\[
\begin{align}\nonumber
f_{01}&=-\frac{1}{4}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta t^{2}-\frac{1}{4}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}(I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t)\delta t^{2} \\\nonumber
f_{21}&=-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta t-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}(I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t)\delta t \\\nonumber
f_{04}&=\frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})(-\delta t) \\\nonumber
f_{24}&=\frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t)(-\delta t) \\\nonumber
v_{01}&=\frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \\\nonumber
v_{03}&=\frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \\\nonumber
v_{21}&=\frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \\\nonumber
v_{23}&=\frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t
\end{align}
\]
將式(1.16)簡寫為，
\[
\delta z_{k+1} = F\delta z_{k}+VQ
\]
??最後得到系統的雅克比矩陣 \(J_{k+1}\) 和協方差矩陣 \(P_{k+1}\)，初始狀態下的雅克比矩陣和協方差矩陣為單位陣和零矩陣，即
\[
J_{k}=I \\ P_{k}=0
\]
\[
J_{k+1}=FJ_{k}
\tag{1.17}
\]
\[
P_{k+1}=FP_{k}F^{T}+VQV_{T}
\tag{1.18}
\]
2 前端KLT跟蹤
3 系統初始化
??在提取的圖像的Features和做完IMU的預積分之後，進入了系統的初始化環節，那麽系統為什麽要進行初始化呢，主要的目的有以下兩個：
- 系統使用單目相機，如果沒有一個良好的尺度估計，就無法對兩個傳感器做進一步的融合。這個時候需要恢復出尺度；
- 要對IMU進行初始化，IMU會受到bias的影響，所以要得到IMU的bias。
所以我們要從初始化中恢復出尺度、重力、速度以及IMU的bias，因為視覺(SFM)在初始化的過程中有著較好的表現，所以在初始化的過程中主要以SFM為主，然後將IMU的預積分結果與其對齊，即可得到較好的初始化結果。
??系統的初始化主要包括三個環節：求取相機與IMU之間的相對旋轉、相機初始化(局部滑窗內的SFM，包括沒有尺度的BA)、IMU與視覺的對齊(IMU預積分中的 \(\alpha\)等和相機的translation)。
3.1 相機與IMU之間的相對旋轉
??這個地方相當於求取相機與IMU的一部分外參。相機與IMU之間的旋轉標定非常重要，偏差1-2°系統的精度就會變的極低。這部分的內容參考文獻[3]中Ⅴ-A部分，這裏做簡單的總結。
??設相機利用對極關系得到的旋轉矩陣為 \(R^{c_{k}}_{c_{k+1}}\)，IMU經過預積分得到的旋轉矩陣為\(R^{b_{k}}_{b_{k+1}}\)，相機與IMU之間的相對旋轉為 \(R^{b}_{c}\)，則對於任一幀滿足，
\[
R^{b_{k}}_{b_{k+1}}R^{b}_{c}=R^{b}_{c}R^{c_{k}}_{c_{k+1}}
\tag{3.1}
\]
對式(3.1)可以做簡單的證明，在其兩邊同乘 \(^{c}x_{k+1}\) 得
\[
\begin{align}\nonumber
R^{b_{k}}_{b_{k+1}}R^{b}_{c}{^{c}x_{k+1}}&=R^{b}_{c}R^{c_{k}}_{c_{k+1}}{^{c}x_{k+1}} \\\nonumber
R^{b_{k}}_{b_{k+1}}{^{b}x_{k+1}}&=R^{b}_{c}{^{c}x_{k}}
\\\nonumber
^{b}x_{k}&=^{b}x_{k}
\end{align}
\]
將旋轉矩陣寫為四元數，則式(3.1)可以寫為
\[
q^{b_{k}}_{b{k+1}}\otimes q^{b}_{c}=q^{b}_{c}\otimes q^{c_{k}}_{c{k+1}}
\]
將其寫為左乘和右乘的形式，綜合為
\[
[Q_{1}(q^{b_{k}}_{b{k+1}})-Q_{2}(q^{c_{k}}_{c{k+1}})]q^{b}_{c}=Q^{k}_{k+1}q^{b}_{c}=0
\tag{3.2}
\]
其中 \(Q_{1}(q^{b_{k}}_{b{k+1}})\)，\(Q_{2}(q^{c_{k}}_{c{k+1}})\) 分別表示四元數的左乘和右乘形式，
\[
\begin{align}\nonumber
Q_{1}(q)&=\begin{bmatrix}
q_{w}I_{3}+[q_{xyz }]_{\times} & q_{xyz}\\\nonumber
-q_{xyz} & q_{w}
\end{bmatrix} \Q_{2}(q)&=\begin{bmatrix}
q_{w}I_{3}-[q_{xyz }]_{\times} & q_{xyz}\\\nonumber
-q_{xyz} & q_{w}
\end{bmatrix}
\end{align}
\tag{3.3}
\]
這個地方因為四元數的實部在前在後的表達不一樣，而左右乘的形式也不一樣，所以文獻[2]和文獻[3]存在區別。Vins-Mono使用的是Eigen實部在後的表達方式。
那麽對於 \(n\)對測量值，則有
\[
\begin{bmatrix}
w^{0}_{1}Q^{0}_{1}\w^{1}_{2}Q^{1}_{2}\\vdots \w^{N-1}_{N}Q^{N-1}_{N}
\end{bmatrix}q^{b}_{c}=Q_{N}q^{b}_{c}=0
\tag{3.4}
\]
其中 \(w^{N-1}_{N}\) 為外點剔除權重，其與相對旋轉求取得殘差有關，\(N\)為最新的視覺幀的index，其由最終的終止條件決定。殘差可以寫為，
\[
r^{k}_{k+1}=acos((tr(\hat{R}^{b^{-1}}_{c}R^{b_{k}^{-1}}_{b_{k+1}}\hat{R}^{b}_{c}R^{c_{k}}_{c_{k+1}} )-1)/2)
\tag{3.5}
\]
殘差還是很好理解的，在具體的代碼中可以計算公式(3.1)兩邊兩個旋轉的得角度差。在得到殘差之後就可以進一步得到公式(3.4)中的權重，
\[
w^{k}_{k+1}=\left\{\begin{matrix}
1,\qquad r^{k}_{k+1}<threshold\\frac{threshold}{r^{k}_{k+1}},\qquad otherwise
\end{matrix}\right.
\tag{3.6}
\]
一般會將閾值 \(threshold\) 取做 \(5°\)。至此，就可以通過求解方程(3.4)得到相對旋轉，式(3.4)的解為 \(Q_{N}\) 的左奇異向量中最小奇異值對應的特征向量。
??但是，在這裏還要註意 求解的終止條件(校準完成的終止條件) 。在足夠多的旋轉運動中，我們可以很好的估計出相對旋轉 \(R^{b}_{c}\)，這時 \(Q_{N}\) 對應一個準確解，且其零空間的秩為1。但是在校準的過程中，某些軸向上可能存在退化運動(如勻速運動)，這時 \(Q_{N}\) 的零空間的秩會大於1。判斷條件就是 \(Q_{N}\) 的第二小的奇異值是否大於某個閾值，若大於則其零空間的秩為1，反之秩大於1，相對旋轉\(R^{b}_{c}\) 的精度不夠，校準不成功。
3.2 相機初始化
??這一階段的思路就是單目相機的初始化過程，先求取本質矩陣求解位姿，進而三角化特征點，然後PnP求解位姿，不斷重復的過程，直到恢復出滑窗內的Features和相機位姿，代碼比較清晰。要註意的就是坐標系的和位姿的變換，容易混亂。如以下幾個函數：
triangulateTwoFrames ：輸入是相機外參(世界到相機)，求解出的3D點是在世界坐標系下。
cv::solvePnP：該API輸入是世界坐標系的點，求解出的是世界坐標系到相機坐標系的變換，所以一般需要將結果轉置。
3.3 視覺與IMU對齊
??視覺與IMU的對齊主要解決三個問題：
??(1) 修正陀螺儀的bias；
??(2) 初始化速度、重力向量 \(g\)和尺度因子(Metric scale)；
??(3) 改進重力向量 \(g\)的量值；
3.3.1 陀螺儀Bias修正
??發現校正部分使用的都是一系列的約束條件，思路很重要啊。陀螺儀Bias校正的時候也是使用了一個簡單的約束條件：
\[
\underset{\delta b_{w}}{min}\sum_{k\in B}^{ }\left \| q^{c_{0}^{-1}}_{b_{k+1}}\otimes q^{c_{0}}_{b_{k}}\otimes\gamma _{b_{k+1}}^{b_{k}} \right \|^{2}
\tag{3.7}
\]
其中
\[
\gamma _{b_{k+1}}^{b_{k}}\approx \hat{\gamma}_{b_{k+1}}^{b_{k}}\otimes \begin{bmatrix}
1\\frac{1}{2}J^{\gamma }_{b_{w}}\delta b_{w}
\end{bmatrix}
\tag{3.8}
\]
公式(3.7)的最小值為單位四元數 \([1,0_{v}]^{T}\) ,所以將式(3.7)進一步寫為，
\[
\begin{align}\nonumber
q^{c_{0}^{-1}}_{b_{k+1}}\otimes q^{c_{0}}_{b_{k}}\otimes\gamma _{b_{k+1}}^{b_{k}}&=\begin{bmatrix}
1\0
\end{bmatrix} \\\nonumber
\hat{\gamma}_{b_{k+1}}^{b_{k}}\otimes \begin{bmatrix}
1\\frac{1}{2}J^{\gamma }_{b_{w}}\delta b_{w}
\end{bmatrix}&=q^{c_{0}^{-1}}_{b_{k}}\otimes q^{c_{0}}_{b_{k+1}} \\end{align}
\]
\[
\begin{bmatrix}
1\\frac{1}{2}J^{\gamma }_{b_{w}}\delta b_{w}
\end{bmatrix}=\hat{\gamma}_{b_{k+1}}^{b_{k}^{-1}}\otimes q^{c_{0}^{-1}}_{b_{k}}\otimes q^{c_{0}}_{b_{k+1}}
\tag{3.9}
\]
只取式(3.9)式虛部，在進行最小二乘求解
\[
J^{\gamma^{T}}_{b_{w}}J^{\gamma }_{b_{w}}\delta b_{w}=2*J^{\gamma^{T}}_{b_{w}}(\hat{\gamma}_{b_{k+1}}^{b_{k}^{-1}}\otimes q^{c_{0}^{-1}}_{b_{k}}\otimes q^{c_{0}}_{b_{k+1}}).vec
\tag{3.10}
\]
求解式(3.10)的最小二乘解，即可得到 \(\delta b_{w}\)，註意這個地方得到的只是Bias的變化量，需要在滑窗內累加得到Bias的準確值。
3.3.2 初始化速度、重力向量 \(g\)和尺度因子
??在這個步驟中，要估計系統的速度、重力向量以及尺度因子。所以系統的狀態量可以寫為，
\[
X_{I}=[v^{c_{0}}_{b_{0}},v^{c_{0}}_{b_{1}},\cdots ,g^{c_{0}},s]
\tag{3.11}
\]
上面的狀態量都是在 \(c_{0}\) 相機坐標系下。接著，有前面的由預積分部分，IMU的測量模型可知
\[
\begin{align}\nonumber
\alpha^{b_{k}}_{b_{k+1}}&=q^{b_{k}}_{c_{0}}(s(\bar{p}^{c_{0}}_{b_{k+1}}-\bar{p}^{c_{0}}_{b_{k}})+\frac{1}{2}g^{c_{0}}\triangle t_{k}^{2}-v^{c_{0}}_{b_{k}}\triangle t_{k}^{2}) \\\nonumber
\beta ^{b_{k}}_{b_{k+1}}&=q^{b_{k}}_{c_{0}}(v^{c_{0}}_{b_{k+1}}+g^{c_{0}}\triangle t_{k}-v^{c_{0}}_{b_{k}})
\end{align}
\tag{3.12}
\]
在3.1小節，我們已經得到了IMU相對於相機的旋轉 \(q_{b}^{c}\),假設IMU到相機的平移量\(p_{b}^{c}\) 那麽可以很容易地將相機坐標系下的位姿轉換到IMU坐標系下，
\[
\begin{align}\nonumber
q_{b_{k}}^{c_{0}} &= q^{c_{0}}_{c_{k}}\otimes q_{b}^{c} \\\nonumber
s\bar{p}^{c_{0}}_{b_{k}}&=s\bar{p}^{c_{0}}_{c_{k}}+q^{c_{0}}_{c_{k}}p_{b}^{c}
\end{align}
\tag{3.13}
\]
綜合式(3.12)和式(3.13)可得，
\[
\begin{align}\nonumber
\hat{z}^{b_{k}}_{b_{k+1}}&=\begin{bmatrix}
\alpha^{b_{k}}_{b_{k+1}}-q^{c_{0}}_{c_{k+1}}p^{c}_{b}+q^{c_{0}}_{c_{k}}p^{c}_{b}&\\\nonumber
\beta ^{b_{k}}_{b_{k+1}}
\end{bmatrix}=H^{b_{k}}_{b_{k+1}}X_{I}+n^{b_{k}}_{b_{k+1}} \\\nonumber
&\approx \begin{bmatrix}
-q^{b_{k}}_{c_{0}}\triangle t_{k} &0& 1/2q^{b_{k}}_{c_{0}}\triangle t_{k}^{2} &q^{b_{k}}_{c_{0}}(\bar{p}^{c_{0}}_{b_{k+1}}-\bar{p}^{c_{0}}_{b_{k}}) \ -q^{b_{k}}_{c_{0}}& q^{b_{k}}_{c_{0}} &q^{b_{k}}_{c_{0}}\triangle t_{k} & 0
\end{bmatrix}\begin{bmatrix}
v^{c_{0}}_{b_{k}}\v^{c_{0}}_{b_{k+!}}\g^{c_{0}}\s
\end{bmatrix}
\end{align}
\tag{3.14}
\]
這一步的推導也比較簡單，以\(H\)矩陣的第一行，將式(3.13)的(2)式帶入(3.12)的(1)式可得
\[
\begin{align} \nonumber
\alpha^{b_{k}}_{b_{k+1}}&=q^{b_{k}}_{c_{0}}(s(\bar{p}^{c_{0}}_{c_{k+1}}-\bar{p}^{c_{0}}_{c_{k}}+(q_{c_{k+1}}^{c_{0}}-q_{c_{k}}^{c_{0}})p^{b}_{c}+\frac{1}{2}g^{c_{0}}\triangle t_{k}^{2}-v^{c_{0}}_{b_{k}}\triangle t_{k}^{2}) \\ \nonumber
\alpha^{b_{k}}_{b_{k+1}}&-q^{b_{k}}_{c_{0}}(q_{c_{k+1}}^{c_{0}}-q_{c_{k}}^{c_{0}})p^{b}_{c}=q^{b_{k}}_{c_{0}}(s(\bar{p}^{c_{0}}_{c_{k+1}}-\bar{p}^{c_{0}}_{c_{k}}p^{b}_{c})+\frac{1}{2}g^{c_{0}}\triangle t_{k}^{2}-v^{c_{0}}_{b_{k}}\triangle t_{k}^{2})
\end{align}
\]
但是不知道為什麽論文，在做了近似之後漏了上式等式左邊的\(q^{b_{k}}_{c_{0}}\)。
最後求解最小二乘問題
\[
\underset{\delta b_{w}}{min}\sum_{k\in B}^{ }\left \|
\hat{z}^{b_{k}}_{b_{k+1}}-H^{b_{k}}_{b_{k+1}}X_{I}
\right \|^{2}
\]
至此即可求解出所有狀態量，但是對於重力向量 \(g^{c_{0}}\) 還要做進一步的糾正。在糾正\(g^{c_{0}}\) 的過程中，會對速度也做進一步的優化。
3.3.3 糾正重力向量
??這部分和上一小節的內容差不多，就是叠代不斷更新重力向量的過程，要註意重力向量被表達為：
\[
\hat{g} = g\cdot \hat{\bar{g}} + \omega_{1}b_{1}+\omega_{2}b_{2}
\]
4 後端優化
??後端優化是VINS-Mono中除了初始化之外，創新性最高的一塊，也是真真的 緊耦合 部分，而初始化的過程事實上是一個 松耦合。因為初始化過程中的狀態量並沒有放在最底層融合，而是各自做了位姿的計算，但是在後端優化的過程中，所有優化量都是在一起的。
??狀態量
\[
\begin{align}\nonumber
X&=[x_{0},x_{1},\cdots ,x_{n},x^{b}_{c},\lambda_{0},\lambda _{1}, \cdots , \lambda_{m}] \\\nonumber
x_{k}&=[p^{w}_{b_{k}},v^{w}_{b_{k}},q^{w}_{b_{k}},b_{a},b_{g}],\quad k\in[0,n] \\\nonumber
x^{b}_{c}&= [p^{b}_{c},q^{b}_{c}]
\tag{4.1}
\end{align}
\]
優化過程中的誤差狀態量
\[
\begin{align}\nonumber
\delta X&=[\delta x_{0},\delta x_{1},\cdots ,\delta x_{n},\delta x^{b}_{c},\lambda_{0},\delta \lambda _{1}, \cdots , \delta \lambda_{m}] \\\nonumber
\delta x_{k}&=[\delta p^{w}_{b_{k}},\delta v^{w}_{b_{k}},\delta \theta ^{w}_{b_{k}},\delta b_{a},\delta b_{g}],\quad k\in[0,n] \\\nonumber
\delta x^{b}_{c}&= [\delta p^{b}_{c},\delta q^{b}_{c}]
\end{align}
\]
進而得到系統優化的代價函數
\[
\begin{align}\nonumber
\underset{X}{min}
\left\{
\left \|
r_{p}-H_{p}X
\right \|^{2} +
\sum_{k\in B}^{ } \left \|
r_{B}(\hat{z}^{b_{k}}_{b_{k+1}},X)
\right \|^{2}_{P^{b_{k}}_{b{k+1}}} \+\sum_{(i,j)\in C}^{ } \left \|
r_{C}(\hat{z}^{c_{j}}_{l},X)
\right \|^{2}_{P^{c_{j}}_{l}}
\right\}
\end{align}
\tag{4.2}
\]
其中三個殘差項依次是邊緣化的先驗信息、IMU測量殘差以及視覺的觀測殘差。三種殘差都是用馬氏距離來表示的，這個在後面ceres的優化殘差中要特別註意。
??在優化過程中，每一次的高斯叠代，式(4.2)可以進一步被線性化為，
\[\nonumber
\begin{align}
\left\{
\underset{X}{min}
\left \|
r_{p}-H_{p}X
\right \|^{2} +
\sum_{k\in B}^{ } \left \|
r_{B}(\hat{z}^{b_{k}}_{b_{k+1}},X)+H^{b_{k}}_{b_{k+1}}\delta X
\right \|^{2}_{P^{b_{k}}_{b{k+1}}} \+\sum_{(i,j)\in C}^{ } \left \|
r_{C}(\hat{z}^{c_{j}}_{l},X)+H^{c_{j}}_{l}\delta X
\right \|^{2}_{P^{c_{j}}_{l}}
\right \}
\end{align}
\tag{4.3}
\]
其中 \(H^{b_{k}}_{b_{k+1}}, H^{c_{j}}_{l}\) 為IMU測量和視覺測量方程的雅克比矩陣，在後面會有進一步的推導。
進一步將式(4.3)寫作
\[
\begin{align}\nonumber
&(\Lambda _{p}+\sum H^{b_{k}^{T}}_{b_{k+1}}P^{-1}H^{b_{k}}_{b_{k+1}}+\sum H^{c_{j}^{T}}_{l}R^{-1}H^{c_{j}}_{l})\delta x \\\nonumber
=&(b_{p}+\sum H^{b_{k}^{T}}_{b_{k+1}}P^{-1}r_{B}+\sum H^{c_{j}^{T}}_{l}R^{-1}r_{C})
\end{align}
\]
最小化式(4.3)相當於求解線性方程
\[
(\Lambda _{p}+\Lambda _{B}+\Lambda _{C})\delta X=(b_{p}+b_{B}+b_{C})
\tag{4.4}
\]
其中 \(\Lambda _{p}, \Lambda _{B}, \Lambda_{C}\) 分別對應邊緣先驗、IMU和視覺測量的信息矩陣。這部分還需要進一步的理解。
4.1 IMU測量誤差
??這部分內容主要對應在後端的優化過程中IMU測量部分的殘差以及在優化過程中的雅克比矩陣的求解。
??首先推導IMU測量的殘差部分，由文獻[1]中body系下的預積分方程式(4)，可以得到IMU的測量模型式(13)
\[
\begin{bmatrix}
\hat{\alpha }^{b_{k}}_{b_{k+1}}\\hat{\gamma }^{b_{k}}_{b_{k+1}}\\hat{\beta }^{b_{k}}_{b_{k+1}}\0\0
\end{bmatrix}
=\begin{bmatrix}
q^{b_{k}}_{w}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w}+\frac{1}{2}g^{w}\triangle t^{2}-v_{b_{k}}^{w}\triangle t)\p_{b_{k}}^{w^{-1}}\otimes q^{w}_{b_{k+1}}\q^{b_{k}}_{w}(v^{w}_{b_{k+1}}+g^{w}\triangle t-v_{b_{k}}^{w})\b_{ab_{k+1}}-b_{ab_{k}}\b_{wb_{k+1}}-b_{wb_{k}}
\end{bmatrix}
\tag{4.5}
\]
那麽IMU測量的殘差即可寫為
\[
\begin{align}\nonumber
r_{B}(\hat{z}^{b_{k}}_{b_{k+1}},X)=
\begin{bmatrix}
\delta \alpha ^{b_{k}}_{b_{k+1}}\\delta \theta ^{b_{k}}_{b_{k+1}}\\delta \beta ^{b_{k}}_{b_{k+1}}\\delta b_{a}\\delta b_{g}
\end{bmatrix}
&=\begin{bmatrix}
q^{b_{k}}_{w}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w}+\frac{1}{2}g^{w}\triangle t^{2}-v_{b_{k}}^{w}\triangle t)-\hat{\alpha }^{b_{k}}_{b_{k+1}}\2[q_{b_{k+1}}^{w^{-1}}\otimes q^{w}_{b_{k}}\otimes \hat{\gamma }^{b_{k}}_{b_{k+1}}]_{xyz}\q^{b_{k}}_{w}(v^{w}_{b_{k+1}}+g^{w}\triangle t-v_{b_{k}}^{w})-\hat{\beta }^{b_{k}}_{b_{k+1}}\b_{ab_{k+1}}-b_{ab_{k}}\b_{gb_{k+1}}-b_{gb_{k}}
\end{bmatrix}
\end{align}
\tag{4.6}
\]
其中 \([\hat{\alpha }^{b_{k}}_{b_{k+1}},\hat{\gamma }^{b_{k}}_{b_{k+1}},\hat{\beta }^{b_{k}}_{b_{k+1}}]\) 來自於IMU預積分部分。式(4.4)和文獻[1]中的式(22)有些不同，主要是第1項，在兩個式子中是逆的關系，本文中的寫法是為了和代碼保持一致。本質上兩者是沒有區別的，因為殘差很小的時候，第一項接近於單位四元數，所以取逆並沒有什麽影響，只是寫殘差的方式不一樣。
??在式(4.4)中，殘差主要來自於兩幀IMU的位姿、速度及Bias，即 \([p^{w},q^{w},v^{w},b_{a},b_{w}]\)，或者說我們要利用IMU殘差要優化的狀態量也是這5個，如果是兩幀IMU就是10個。在計算雅克比矩陣的時候，分為 \([p^{w}_{b_{k}},q^{w}_{b_{k}}]\) , \([v^{w}_{b_{k}},b_{ab_{k}},b_{wb_{k}}]\)，\([p^{w}_{b_{k+1}},q^{w}_{b_{k+1}}]\) , \([v^{w}_{b_{k}},b_{ab_{k}},b_{wb_{k}}]\) 等四部分， 分別表示為 \(J[0],J[1],J[2],J[3]\)。這部分內容的推導可以參考文獻[2] 中III.B式(14)部分。
??這個地方很容易出錯誤，原因就是沒有搞清楚求偏導的對象。整個優化過程中，IMU測量模型這一塊，涉及到的狀態量是 \(x_{k}\),但是參考文獻[5]中IV.A部分，以及十四講中10.2.2小節的目標函數可知，這個地方的雅克比矩陣是針對變化量 \(\delta x_{k}\)的。所以，在後面求取四部分雅可比的時候，也不是對狀態量求偏導，而是對誤差狀態量求偏導。
??式(4.4)對 \([\delta p^{w}_{b_{k}},\delta \theta ^{w}_{b_{k}}]\) 求偏導得，
\[
J[0]=\begin{bmatrix}
-q^{b_{k}}_{w} & R^{b_{k}}_{w}[(p^{w}_{b_{k+1}}-p_{b_{k}}^{w}+\frac{1}{2}g^{w}\triangle t^{2}-v_{b_{k}}^{w}\triangle t)]_{\times }\0 & [q_{b_{k+1}}^{w^{-1}}q^{w}_{b_{k}}]_{L}[\hat{\gamma }^{b_{k}}_{b_{k+1}}]_{R}\0 & R^{b_{k}}_{w}[(v^{w}_{b_{k+1}}+g^{w}\triangle t-v_{b_{k}}^{w})]_{\times } \0 &0\0 &0
\end{bmatrix}
\tag{4.7}
\]
\(J[0]\) 是 \(15*7\)的矩陣，其中 \(R^{b_{k}}_{w}\) 是四元數對應的旋轉矩陣，求偏導可以看做是對四元數或者旋轉矩陣求偏導，求解過程可以參考文獻[2]。第3項只需右下角的3行3列。第1項可以參考文獻[1]中4.3.4“旋轉向量的雅克比矩陣”推導得到，第3項是可以參考文獻[1]中公式(22)和文獻[9]中3.2部分。
??式(4.4)對 \([\delta v^{w}_{b_{k}},\delta b_{ab_{k}},\delta b_{wb_{k}}]\) 求偏導得，
\[J[1]=
\begin{bmatrix}
-q^{b_{k}}_{w}\triangle t & -J^{\alpha }_{b_{a}} & -J^{\alpha }_{b_{a}}\0 & 0 & -[q_{b_{k+1}}^{w^{-1}}\otimes q^{w}_{b_{k}}\otimes \hat{\gamma }^{b_{k}}_{b_{k+1}}]_{L}J^{\gamma}_{b_{w}}\-q^{b_{k}}_{w} & -J^{\beta }_{b_{a}} & -J^{\beta }_{b_{a}}\ 0& -I &0 \0 &0 &-I
\end{bmatrix}
\tag{4.8}
\]
\(J[1]\) 是 \(15*9\)的矩陣，第5項仍然取右下角的3行3列。
??式(4.4)對 \([\delta p^{w}_{b_{k+1}},\delta \theta ^{w}_{b_{k+1}}]\) 求偏導得，
\[J[2]=
\begin{bmatrix}
-q^{b_{k}}_{w} &0\0 & [\hat{\gamma }^{b_{k}^{-1}}_{b_{k+1}}\otimes q_{w}^{b_{k}}\otimes q_{b_{k+1}}^{w}]_{L} \0 & 0 \ 0& 0 \0 &0
\end{bmatrix}
\tag{4.9}
\]
\(J[2]\) 是 \(15*9\)的矩陣，第5項仍然取右下角的3行3列。
??式(4.4)對 \([\delta v^{w}_{b_{k+1}},\delta b_{ab_{k+1}},\delta b_{wb_{k+1}}]\) 求偏導得，
\[J[3]=
\begin{bmatrix}
0 &0 & 0\0 & 0 &0 \q^{b_{k}}_{w} & 0 & 0\ 0& I &0 \0 &0 &I
\end{bmatrix}
\tag{4.10}
\]
\(J[3]\) 是 \(15*9\)的矩陣。
以上便是IMU測量模型的雅可比的矩陣的推導過程，雅克比矩陣主要還是在Ceres做優化的過程中，高斯叠代要使用到。
4.2 相機測量誤差
??相機測量誤差萬變不離其宗，還是要會到像素坐標差或者灰度差(光度誤差)。Vins-Mono中的相機測量誤差本質還是特征點的重投影誤差，將特征點 \(P\)從相機的\(i\) 系轉到相機的\(j\) 系，即把相機的測量殘差定義為，
\[
r_{C}=(\hat{z}_{l}^{c_{j}},X)=[b_{1},b_{2}]^{T}\cdot (\bar{P}_{l}^{c_{j}}-\frac{P_{l}^{c_{j}}}{\left \| P_{l}^{c_{j}} \right \|})
\tag{4.11}
\]
因為最終要將殘差投影到切平面上，\([b_{1},b_{2}]\)是切平面上的一對正交基。反投影後的\(P_{l}^{c_{j}}\) 寫為，
\[
P_{l}^{c_{j}}=q_{b}^{c}(q_{w}^{b_{j}}(q_{b_{i}}^{w}(q_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda _{l}}+p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})
\]
反投影之前的坐標 \(\bar{P}_{l}^{c_{j}}\),\(\bar{P}_{l}^{c_{i}}\)寫為，
\[
\begin{align}\nonumber
\bar{P}_{l}^{c_{j}}&=\pi_{c}^{-1}(\begin{bmatrix}
\hat{u}_{l}^{c_{j}}\\
\hat{v}_{l}^{c_{j}}
\end{bmatrix}) \\\nonumber
\bar{P}_{l}^{c_{i}}&=\pi_{c}^{-1}(\begin{bmatrix}
\hat{u}_{l}^{c_{i}}\\
\hat{v}_{l}^{c_{i}}
\end{bmatrix})
\end{align}
\]
??參與相機測量殘差的狀態量有，\([p^{w}_{b_{i}},q^{w}_{b_{i}}]\)，\([p^{w}_{b_{j}},q^{w}_{b_{j}}]\)，\([p^{b}_{c},q^{b}_{c}]\)以及逆深度 \(\lambda_{l}\)。所以下面分別對這幾個狀態量對應的誤差狀態量求式(4.9)的偏導，得到高斯叠代過程中的雅克比矩陣。
??式(4.9)對\([\delta p^{w}_{b_{i}},\delta \theta ^{w}_{b_{i}}]\)求偏導，得到\(3*6\)的雅克比矩陣，
\[\nonumber
J[0]=\begin{bmatrix}
q_{b}^{c}q_{w}^{b_{j}} & -q_{b}^{c}q_{w}^{b_{j}}q_{b_{i}}^{w}[q_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda _{l}}+p_{c}^{b}]_{\times }
\end{bmatrix}
\tag{4.12}
\]
??式(4.9)對\([\delta p^{w}_{b_{j}},\delta \theta ^{w}_{b_{j}}]\)求偏導，得到\(3*6\)的雅克比矩陣，
\[\nonumber
J[1]=\begin{bmatrix}
-q_{b}^{c}q_{w}^{b_{j}} & q_{b}^{c}q_{w}^{b_{j}}[q_{b_{i}}^{w}(q_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda _{l}}+p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w}]_{\times }
\end{bmatrix}
\tag{4.13}
\]
??式(4.9)對\([\delta p^{b}_{c},\delta \theta ^{b}_{c}]\)求偏導，得到\(3*6\)的雅克比矩陣，
\[\nonumber
J[2]=
\begin{bmatrix}
q_{b}^{c}(q_{w}^{b_{j}}q_{bi}^{w}-I_{3*3}) & -q_{b}^{c}q_{w}^{b_{j}}q_{b_{i}}^{w}q_{c}^{b}[\frac{\bar{P}_{l}^{c_{i}}}{\lambda _{l}}]_{\times }+[q_{b}^{c}(q_{w}^{b_{j}}(q_{b_{i}}^{w}p_{c}^{b}+p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})]
\end{bmatrix}
\tag{4.14}
\]
??式(4.9)對\(\delta \lambda_{l}\)求偏導，得到\(3*1\)的雅克比矩陣，
\[\nonumber
J[3]=-q_{b}^{c}q_{w}^{b_{j}}q_{b_{i}}^{w}q_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda _{l}^{2}}
\tag{4.15}
\]
以上就是相機測量誤差以及誤差方程的雅克比矩陣的求解過程。
??在相機測量誤差的文件projection_factor.cpp中的ProjectionFactor::check還沒有理解
4.3 閉環修正與優化
??這部分內容默認已經檢測到閉環，只涉及到後續的優化部分。
4.4 系統邊緣化
4.4.1 邊緣化的定義和目的
??邊緣化(marginalization)的過程就是將滑窗內的某些較舊或者不滿足要求的視覺幀剔除的過程，所以邊緣化也被描述為將聯合概率分布分解為邊緣概率分布和條件概率分布的過程(說白了，就是利用shur補減少優化參數的過程)。利用Sliding Window做優化的過程中，邊緣化的目的主要有兩個：
- 滑窗內的pose和feature個數是有限的，在系統優化的過程中，勢必要不斷將一些pose和feature移除滑窗。
如果當前幀圖像和上一幀添加到滑窗的圖像幀視差很小，則測量的協方差(重投影誤差)會很大，進而會惡化優化結果。LIFO導致了協方差的增大，而惡化優化結果？
直接進行邊緣化而不加入先驗條件的後果：- 無故地移除這些pose和feature會丟棄幀間約束，會降低了優化器的精度，所以在移除pose和feature的時候需要將相關聯的約束轉變為一個先驗的約束條件作為prior放到優化問題中
在邊緣化的過程中，不加先驗的邊緣化會導致系統尺度的缺失(參考[6])，尤其是系統在進行__退化運動__時(如無人機的懸停和恒速運動)。一般來說只有兩個軸向的加速度不為0的時候，才能保證尺度可觀，而退化運動對於無人機或者機器人來說是不可避免的。所以在系統處於退化運動的時候，要加入先驗信息保證尺度的客觀性。
以上就可以描述為邊緣化的目的以及在邊緣化中加入先驗約束的原因。4.4.2 兩種邊緣化措施
??這兩種邊緣化的措施主要還是針對懸停和恒速運動等退化運動。下面就邊緣化的過程做簡要的總結。
??設共有 \((B_{0}, B_{1}, \cdots , B_{N} , B_{N+1} ,\cdots, B_{N++n})\) 個狀態量，其中狀態量\(X=\begin{bmatrix} x_{B_{0}}^{B_{0}} & \cdots & x_{B_{N-1}}^{B_{0}}\mid\lambda _{l} \end{bmatrix}\)中的加速度計是經過充分激勵的，那麽狀態量\(x_{B_{N}}^{B_{0}}\)只有滿足下面兩個條件之一的時候才能被加入到滑窗內。
??(1) 兩幀圖像之間的時間差 \(\triangle t\)超過閾值\(\delta\)。
??(2) 排除旋轉運動，兩幀之間共同Features的視差超過閾值\(\varepsilon\)。
其中，條件(1)避免了兩幀圖像之間的IMU長時間積分，而出現漂移。條件(2)保證了系統的運動時，有足夠視差的共視幀能夠被加入到滑窗。
??因為滑窗的大小是固定的，要加入新的Keyframe，就要從滑窗中剔除舊的Keyframe。在VINS-Mono中剔除舊幀有兩種方式，剔除滑窗的首幀或者倒數第二幀(假設滑窗默認是由右向左滑的！)。所以關於剔除舊幀也有一定的剔除規則或者說是邊緣化規則。
??設定一個變量\(S=float/fix\)，\(S\) 是由滑窗內的最新的兩個Keyframe視差決定的，如果視差大於閾值$\varepsilon \(，則邊緣化最舊幀\)x_{B_{0}}^{B_{0}}\(，如果視差小於\)\varepsilon \(，則邊緣化倒數第二幀\)x_{B_{0}}^{B_{N-1}}$。具體可以參考文獻[6]中_Algorithm1。_ __要註意邊緣化的時候，不僅要移除相機位姿，被該相機首次觀測到的Features也要移除。__最終構建出的先驗約束可以寫為，
\[ \nonumber \Lambda ^{+}_{p}=\Lambda _{p}+\sum_{k\in I}^{ }H_{B_{k+1}}^{B_{k}^{T}}P_{B_{k+1}}^{B_{k}^{-1}}H_{B_{k+1}}^{B_{k}}+\sum_{k\in C}^{ }H_{l}^{B_{j}^{T}}P_{l}^{B_{j}^{-1}}H_{l}^{B_{j}} \tag{4.16} \]
其中\(\Lambda _{p}\)即為式(4.4)中的先驗約束條件，矩陣\(H,P\)分別對應被移除的IMU和視覺狀態量對應的雅克比矩陣和協方差矩陣。
??由上面的剔除策略可知，當飛機處於懸停或者微小運動的時候，會一直邊緣化新的視差較小的視覺幀。在保留了舊幀的同時，也保留了加速度信息，保證了尺度的可觀測性。但是，當系統處於速度較大的恒速運動時，加速度信息會伴隨著舊幀移除，因此會發生尺度的漂移。(沈老師也在視頻中提到，系統對恒速運動處理的還不是很好)。
??圖1.(a)所示為第一種邊緣化方式，當狀態量5和狀態量4之間的視差過小的時，在下一狀態量6添加的時候，會把狀態量5給邊緣化，包括特征點\(f_{1}\)也會被剔除。但是要註意的是這個時候狀態量6也處於float狀態，只有比較了狀態量4和6的視差之後，6的狀態才能確定。這種邊緣化措施一般用在無人機處於懸停或者是微小運動的時候。
??圖1.(b)所示為第二種邊緣化方式，如果狀態量4和狀態量5之間有足夠大的視差則邊緣化狀態量0，接受狀態量6，進一步判斷狀態量6的狀態。
圖1 兩種邊緣化方式示意圖4.4.3 舒爾補邊緣化優化狀態量
??這一小節主要總結下面兩個問題：舒爾補邊緣化優化狀態量和式(4.2)的非線性優化過程中的優化一致性(FEJ)。
??設狀態量\(\delta x_{m}\)和\(\delta x_{r}\)分別是需要被邊緣化和保留的狀態量，根據式(4.4)的線性方程及H矩陣的稀疏性和特性，將式(4.4)進一步寫作：
\[\nonumber \begin{bmatrix} A &B \\ C& D \end{bmatrix}\begin{bmatrix} \delta x_{m}\\ \delta x_{r} \end{bmatrix}=\begin{bmatrix} b_{m}\\ b_{r} \end{bmatrix} \tag{4.17} \]
在式(4.17)兩邊同乘矩陣，得
\[\nonumber \begin{bmatrix} I &0 \\ -CA^{-1}& I \end{bmatrix} \begin{bmatrix} A &B \\ C& D \end{bmatrix}\begin{bmatrix} \delta x_{m}\\ \delta x_{r} \end{bmatrix}= \begin{bmatrix} I &0 \\ -CA^{-1}& I \end{bmatrix} \begin{bmatrix} b_{m}\\ b_{r} \end{bmatrix} \]
可得
\[\nonumber (-CA^{-1}B+D)\delta x_{r}=-CA^{-1}b_{m}+b_{r} \tag{4.18} \]
令，
\[ H = (-CA^{-1}B+D),\qquad b=-CA^{-1}b_{m}+b_{r} \]
在進行舒爾補過程中，已經將 \(\delta x_{m}\)給邊緣掉，得到了關於 \(\delta x_{r}\) 新的帶有先驗信息的關系式。然後將這部分帶入到代價函數的邊緣化殘差中即可。也可以說，由邊緣化得到了條件概率 \(p(\delta x_{b}|\delta x_{m})\sim N(-CA^{-1}b_{m},-CA^{-1}B+D)\)。正如文獻[7]中作者說的，"SLAM is tracking a noraml distribution through a large state space"
4.4.4 關於優化一致性(First Estimate Jacobin, FEJ算法)
??FEJ算法貌似是在邊緣化過程中比較重要的內容。付興銀師兄在講解OKVIS的博客中提到，在Pose和Landmark分開邊緣化的時候，要使用FEJ算法，為了只計算一次雅克比矩陣(還沒弄懂)。博哥在畢設論文中提到，在IMU和視覺測量殘差分開邊緣化的時候，要保證優化一致性，確保各部分在線性化的時候具有相同的線性化點。
??個人理解，這個地方應該有兩層意思。回想高斯叠代過程中，我們在求取雅克比矩陣的時候是針對 \(x_{0}\)求取的，在後面的叠代更新過程中，雅克比矩陣是不變的(或者說是求取雅克比矩陣的點是不變的，仍然在 \(x_{0}\)處。只能說是還是在\(x_{0}\)附近計算雅克比，而不是更新之後的\(x\)，)，也就是稱作"fix the inearization point"。，這樣的話雅克比矩陣就是不變的了。但是僅能說，在marg掉某些點之後，和這些點相關聯的點的雅克比矩陣是不變的。
4.4.5 滑窗優化(Slide window)
??按理說是不應該把滑窗當做一小節來講的，邊緣化、舒爾補都屬於滑窗的範圍，但前面已經總結了。而且正如賀一加師兄在其博客中提到的，滑窗的三大法寶"Marginalization"，"Schur complement"，"First estimate jacobin"，這些在前面也提到了，所以這一小節基本沒什麽可以總結的地方。關於這部分內容，可以參考這幾篇博客，講的真是太好了。okvis1，okvis2，知行合一1，知行合一2
4.5 系統關鍵幀選擇
??要註意的是在vins-mono中，添加關鍵幀的時候是在添加完新的圖像幀，做完滑窗優化之後，將滑窗內的倒數第二幀作為關鍵幀，並加入到了關鍵幀序列當中，具體位置。
??Vins-mono在選擇關鍵幀的共有兩個條件：
- 根據平均視差(這裏的視差包括了平移量和旋轉量);
- 根據tracking的狀態(以跟蹤到的Features為主)。
??對於視差的判斷，在特征處理主程序Estimator::processImage就有，但是關於視差的判斷，並沒有看到關於角度的補償，或者是在後面邊緣化的具體過程中存在角度的補償。關於視差判斷的具體思路是：
??建立包含滑窗內所有特征點集合的vectorfeature,若滿足以下條件就邊緣化最舊幀，否則邊緣化倒數第二幀
- 滑窗內的關鍵幀個數小於3
- 如果當前幀的Features在滑窗內被觀測到的個數小於20
- 計算滑窗內所有滿足條件的Feature(至少有三幀觀測到該Feature)的觀測幀倒數第二幀和導數第三幀的視差(歸一化平面上的距離)，如果最終的平均距離大於10個像素點
- 滿足上面條件的Feature的個數為0的時候
5 閉環校正
??直接法不能像特征法那樣直接將拿描述子利用Dbow進行閉環檢測，所以在閉環的時候只能重新對Keyframe提取Brief等描述子建立Database進行閉環檢測，或者使用開源的閉環檢測庫__FabMap__來進行閉環檢測。Vins-Mono選擇了第一種閉環檢測的方法。
5.1 閉環檢測
??Vins-Mono還是利用詞袋的形式來做Keyframe Database的構建和查詢。在建立閉環檢測的數據庫時，關鍵幀的Features包括兩部分：VIO部分的200個強角點和500 Fast角點。然後描述子仍然使用BRIEF(因為旋轉可觀，匹配過程中對旋轉有一定的適應性，所以不用使用ORB)。
5.2 閉環校正
??在閉環檢測成功之後，會得到回環候選幀。所以要在已知位姿的回環候選幀和滑窗內的匹配幀做匹配，然後把回環幀加入到滑窗的優化當中，這時整個滑窗的狀態量的維度是不發生變化的，因為回環幀的位姿是固定的。
6 全局的位姿優化
??因為之前做的非線性優化本質只是在一個滑窗之內求解出了相機的位姿，而且在回環檢測部分，利用固定位姿的回環幀只是糾正了滑窗內的相機位姿，並沒有修正其他位姿(或者說沒有將回環發現的誤差分配到整個相機的軌跡上)，缺少全局的一致性，所以要做一次全局的Pose Graph。全局的Pose Graph較之滑窗有一定的遲滯性，只有相機的Pose滑出滑窗的時候，Pose才會被加到全局的Pose Graph當中。
7 關鍵變量含義和代碼註釋中的一些稱呼
??因為VINS-Mono的代碼寫的有點亂，好多變量的命名過於隨意，不能很好理解，而且為了註釋方便自己也給某些變量或者環節做了命名。
7.1 代碼註釋中的自定義名稱
- 閉環幀和匹配幀
??閉環幀：在閉環匹配成功之後，Keyframe Database中的舊幀;匹配幀：當前滑窗之內，和閉環幀匹配的匹配幀。
7.2 代碼中的關鍵變量
image(estimator.cpp)
??某一幀圖像得到的某個特征點，某一幀圖像得到的特征點，<Feature_id, <camera_id,Feature>>- f_manager
??滑窗的特征點管理器 - keyframe_database(estimator_node.cpp)
??關鍵幀集合，每次在閉環檢測線程中增加新元素，每個關鍵幀在滑窗內的導數第二個位置時被添加。 - keyframe_buf(estimator_node.cpp)
??為閉環檢測提供臨時的Keyframe存儲,只包含滑窗內的倒數第二Keyframe，當其包含的某幀進入閉環檢測環節會被彈出。 retrive_data_vector
??存儲閉環檢測結果，包塊閉環幀和匹配幀的位姿關系、匹配幀的ID、閉環幀的位姿、閉環幀中良好的匹配點以及匹配正良好匹配點的ID。
****8 遺留的問題
1.為什麽至少兩個軸向上的加速度不為0的時候，尺度才是客觀的。
??在文獻6的第3小節提到，在單目和視覺融合的時候，只有當至少兩個軸向的加速度不為0的時候，尺度才是客觀的。針對無人機來說，無人機的正常俯仰、橫滾運動都會在兩個軸向上有加速度。加速度僅存在於一個軸向的情況多屬於平面運動的機器人。
2.當視差角過小的的時候，視覺測量的協方差會很大。
VINS-Mono代碼分析與總結(最終版)