exp 簡單 都是 最小值 cnblogs 特征向量 vision 還要 per

VINS-Mono代碼註釋：https://github.com/gaochq/VINS-Mono/tree/comment
註釋不完整，可以一起交流。

參考文獻

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.
4 Engel J, Sch?ps T, Cremers D. LSD-SLAM: Large-scale direct monocular SLAM[C]//European Conference on Computer Vision. Springer, Cham, 2014:834-849.
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.
6 Shen S, Mulgaonkar Y, Michael N, et al. Initialization-Free Monocular Visual-Inertial State Estimation with Application to Autonomous MAVs[M]// Experimental Robotics. Springer International Publishing, 2016.
7 Sibley G. A Sliding Window Filter for SLAM[J]. University of Southern California, 2006.
8 CamOdoCal: https://github.com/hengli/camodocal
****

3 系統初始化

??在提取的圖像的Features和做完IMU的預積分之後，進入了系統的初始化環節，那麽系統為什麽要進行初始化呢，主要的目的有以下兩個：

• 系統使用單目相機，需要恢復尺度；
• 要對IMU進行初始化，IMU會受到bias的影響，所以要得到IMU的bias(只估計了陀螺儀的Bias);
• VIO的一些初始狀態量未知。

所以我們要從初始化中恢復出尺度、重力、速度以及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} \]

VINS-Mono代碼分析與總結(二) 系統初始化