VINS-Mono之外參標定和視覺IMU聯合初始化

1.前言

本博客主要介紹VINS-Mono初始化時相機與IMU對齊，主要包括相機到IMU的外參估計、陀螺儀偏置、相機位移和估計空間點的尺度、重力加速度、每幀速度，主要參考了深藍學院的VIO課程及博客VINS-Mono理論學習——視覺慣性聯合初始化與外參標定和崔華坤的《VINS 論文推導及代碼解析》。
總體而言，視覺IMU對齊流程主要包括以下幾部分：

1. 若旋轉外參數$q^{b}_{c}$未知，則先估計旋轉外參數
2. 利用旋轉約束估計陀螺儀偏置：$q^{c_{0}}_{b_{k}} = q_{c_{k}}^{c_{0}}\otimes (q^{b}_{c})^{-1}$
3. 利用平移約束估計重力方向，速度，以及尺度初始值$s\bar p^{c_{0}}_{b_{k}} = s \bar p^{c_{0}}_{c_{k}} - R^{c_{0}}_{b_{k}}p^{b}_{c}$
4. 對重力向量$g^{c_{0}}$進行進一步的優化
5. 求解世界座標系$w$和初始相機座標系$c_{0}$之間的旋轉矩陣$q^{w}_{c_{0}}$，並將軌跡對齊到世界座標系中

2. 外參標定 (利用旋轉約束估計外參數旋轉$q^{b}_{c})$

相鄰倆時刻$k$,$k+1$之間，$k$時刻的IMU到$k+1$時刻的相機外參旋轉矩陣有：$R^{b_{k}}_{c_{k+1}}=R^{b_{k}}_{b_{k+1}} \cdot R^{b}_{c} = R^{b}_{c} * R^{c_{k}}_{c_{k+1}}$ 轉換爲四元素有： $q^{b_{k}}_{b_{k+1}} \otimes q^{b}_{c} = q^{b}_{c} \otimes q^{c_{k}}_{c_{k+1}}$ 上式可寫爲：$(\mathcal{L}(q^{b_{k}}_{b_{k+1}})-\mathcal{R}(q^{c_{k}}_{c_{k+1}}))q^{b}_{c} = Q^{k}_{k+1} \cdot q^{b}_{c} = 0$
將多個時刻的上式方程累計起來，並加上魯棒核權重，可構建超定方程：$\begin{bmatrix}w^{0}_{1} \cdot Q^{0}_{1} \\ w^{1}_{2} \cdot Q_{2}^{1} \\ \cdots \\ w^{N-1}_{N} \cdot Q_{N}^{N-1}\end{bmatrix} q^{b}_{c} = Q_{N}\cdot q^{b}_{c} = 0 \tag{1}$ 其中：
$w_{k+1}^{k} = \left\{\begin{matrix} 1, \ \ r^{k}_{k+1}<threshold \\ \frac{threshold}{r^{k}_{k+1}}, \ \ otherwise \end{matrix}\right.$ 由旋轉和軸角之間的關係$tr(R)=1+2\cos\theta$，能得到角度誤差$r$的計算爲：$r^{k}_{k+1}=acos((tr((R^{b_{k}}_{b_{k+1}}\hat{R}^{b}_{c})^{-1}\hat{R}^{b}_{c}R^{c_{k}}_{c_{k+1}})-1)/2) \\ = 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)$
公式(1)的求解同樣採用SVD分解，即最下奇異值對應的奇異向量即爲IMU座標系到相機座標系的旋轉(四元素表示)，具體代碼見initial_ex_rotation.cpp 函數 CalibrationExRotation()

---

對於$tr(R)=1+2\cos \theta$的推導如下：
旋轉矩陣和軸角的關係爲：$R=\cos\theta I +(1-\cos \theta) aa^{T} - \sin \theta [a]_{\times}$ 其中$a=[a_{1}, a_{2}, a_{3}]^{T}$爲單位旋轉軸，$\theta$爲繞着旋轉軸轉動的角度，$[a]_{\times}=\begin{bmatrix} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} &0\end{bmatrix}$
同時，由於$a$爲單位旋轉軸，有$tr(aa^{T})=1$，故：
$tr(R)=\cos \theta \ tr(I)+(1-\cos \theta) tr(aa^{T}) + \sin\theta \ tr([a]_{\times}) \\ = 3\cos\theta + (1-\cos \theta) = 1+2\cos \theta$ 同時也有$Ra=a$，即旋轉軸上在旋轉後並不發生變化，旋轉軸$a$爲旋轉矩陣$R$的特徵值爲1對應的特徵向量。
關於旋轉矩陣和軸角的關係可參考：https://en.wikipedia.org/wiki/Rotation_matrix

---

3. 視覺IMU聯合初始化

假如我們已經有一個初始的IMU到相機的外參$(R^{b}_{c}, p^{b}_{c})$，同時又有不同時刻幀的位姿($R^{c_{0}}_{c_{k}}, p^{b}_{c}$) (基於第一幀座標系下)，那麼我們可以知道姿態從相機座標系到IMU座標系的關係：$T^{c_{0}}_{c_{k}} = T^{c_{0}}_{b_{k}}T^{b}_{c}$ 則有：$\begin{bmatrix}R^{c_{0}}_{c_{k}} & s\bar{p}^{c_{0}}_{c_{k}} \\ 0 & 1\end{bmatrix} = \begin{bmatrix}R^{c_{0}}_{b_{k}} & s\bar{p}^{c_{0}}_{b_{k}} \\ 0 & 1\end{bmatrix} \begin{bmatrix}R^{b}_{c} & {p}^{b}_{c} \\ 0 & 1\end{bmatrix}$ 將式子展開有【論文公式(14)】：$R^{c_{0}}_{c_{k}}=R^{c_{0}}_{b_{k}}R^{b}_{c} \rightarrow R^{c_{0}}_{b_{k}}=R^{c_{0}}_{c_{k}}(R^{b}_{c})^{-1}$ $s\bar{p}^{c_{0}}_{c_{k}}=R^{c_{0}}_{b_{k}}p^{b}_{c}+s\bar{p}^{c_{0}}_{b_{k}} \rightarrow s\bar{p}^{c_{0}}_{b_{k}}=s\bar{p}^{c_{0}}_{c_{k}}-R^{c_{0}}_{b_{k}}p^{b}_{c} \tag{2}$
視覺IMU聯合初始化中的視覺SFM位姿與IMU位姿的對齊主要在vins_estimator/src/initial/initial_aligment.cpp中的VisualIMUAlignment()函數中。

3.1 陀螺儀偏置的估計 (利用旋轉約束估計$b_{w}$)

對於窗口的連續倆幀$b_{k}$和$b_{k+1}$，已經從SFM中得到了旋轉$q^{c_{0}}_{b_{k}}$和$q^{c_{0}}_{b_{k+1}}$，從IMU預積分中得到了相鄰幀的旋轉$\hat{\gamma}^{b_{k}}_{b_{k+1}}$。根絕約束方程，聯立所有相鄰幀，可以得到最小化代價函數【論文公式(15)】:
$\underset{\delta b_{w}}{min}\sum_{k\in\mathcal{B}} \left \| q^{c_{0} \ -1}_{b_{k+1}} \otimes q^{c_{0}}_{b_{k}} \otimes \gamma^{b_{k}}_{b_{k+1}}\right \|^{2} \tag{3}$ 其中$\mathcal{B}$爲滑動窗口內的所有圖像幀，另外對預積分進行一階泰勒展開近似，即對其線性化：$\gamma^{b_{k}}_{b_{k+1}} \approx \hat {\gamma}^{b_{k}}_{b_{k+1}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J^{\gamma}_{b_{w}}\delta b_{w} \end{bmatrix} \tag{4}$ 由於在理想情況下，即當$q^{c_{0} \ -1 }_{b_{k}} \otimes q^{c_{0}}_{b_{k+1}} = \gamma^{b_{k}}_{b_{k+1}}$，這倆個旋轉的差異$(q^{c_{0} \ -1 }_{b_{k}} \otimes q^{c_{0}}_{b_{k+1}})^{-1}\gamma^{b_{k}}_{b_{k+1}}$的旋轉角度爲0，根據四元素的表示，可得虛部爲$\mathbf{0}$，實部爲1：$q^{c_{0} \ -1}_{b_{k+1}} \otimes q^{c_{0}}_{b_{k}} \otimes \gamma^{b_{k}}_{b_{k+1}} = \begin{bmatrix} 1 \\ \mathbf{0} \end{bmatrix}$ 則有：$\gamma^{b_{k}}_{b_{k+1}} = q^{c_{0} \ -1}_{b_{k}} \otimes q^{c_{0}}_{b_{k+1}} \otimes \begin{bmatrix} 1 \\ \mathbf{0} \end{bmatrix} \tag{5}$ 將公式(4)代入公式(5)，得：$\hat {\gamma}^{b_{k}}_{b_{k+1}} \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}} \otimes \begin{bmatrix} 1 \\ \mathbf{0}\end{bmatrix}$ 當只考慮虛部(虛部爲三維向量)的時候，我們有：$J^{\gamma}_{b_{w}}\delta b_{w} =2 \left \lfloor \hat{\gamma}^{b_{k} \ -1}_{b_{k+1}} \otimes q^{c_{0} \ -1}_{b_{k}} \otimes q^{c_{0}}_{b_{k+1}} \right \rfloor_{xyz}$ 將左邊轉爲對稱矩陣，這樣就可以直接用LDLT對係數矩陣進行分解了：$J^{\gamma \ T}_{b_{w}}J^{\gamma}_{b_{w}}\delta b_{w} =2 J^{\gamma \ T}_{b_{w}} \left \lfloor \hat{\gamma}^{b_{k} \ -1}_{b_{k+1}} \otimes q^{c_{0} \ -1}_{b_{k}} \otimes q^{c_{0}}_{b_{k+1}} \right \rfloor_{xyz} \rightarrow Ax=b$ 求出陀螺儀的偏置bias後，需要對IMU預積分進行重新計算。
代碼見vins_estimator/initial/initial_aligment.cpp中的solveGyroscopeBias。

3.2 速度、重力和尺度初始化 (利用預積分約束估計$v^{b_{k}}_{b_{k}}$、$g^{c_{0}}$、$s$)

對於當前部分需要了估計的變量有：【論文式(16)】$\mathcal{X}_{I}^{3(n+1)+3+1}=[v_{b_{0}}^{b_{0}}, v_{b_{1}}^{b_{1}}, \cdots ,v_{b_{n}}^{b_{n}}, g^{c_{0}}, s]$ 其中，$v_{b_{k}}^{b_{k}}$表示$k$時刻(捕獲到圖像的時刻)IMU body座標系的速度在IMU body座標系的表示，$g^{c_{0}}$爲重力向量在第$0$幀相機座標系下的表示，$s$表示尺度因子，將視覺軌跡拉伸到米制單位。

在IMU預積分中我們已經推導過基於世界座標系$w$(東北天座標系)的預積分量約束：$R^{b_{k}}_{w}p^{w}_{b_{k+1}} = R^{b_{k}}_{w}(p_{b_{k}}^{w}+\underline{v_{b_{k}}^{w}}\Delta t_{k}-\frac{1}{2}g^{w}\Delta t_{k}^{2})+\alpha_{b_{k+1}}^{b_{k}}$ $R^{b_{k}}_{w}\underline{v^{w}_{b_{k+1}}} = R^{b_{k}}_{w}(\underline{v^{w}_{b_{k}}}-g^{w}\Delta t_{k})+\beta_{b_{k+1}}^{b_{k}}$ 將世界座標系$w$ 換成相機初始時刻座標系$c_{0}$有： $\alpha_{b_{k+1}}^{b_{k}} = R^{b_{k}}_{c_{0}}(s(\bar{p}^{c_{0}}_{b_{k+1}}-\bar{p}^{c_{0}}_{b_{k}})+\frac{1}{2} g^{c_{0}}\Delta t^{2}_{k}-\underline{R^{c_{0}}_{b_{k}}v^{b_{k}}_{b_{k}}}\Delta t_{k})$ $\beta_{b_{k+1}}^{b_{k}} = R^{b_{k}}_{c_{0}}(\underline{R^{c_{0}}_{b_{k+1}}v^{b_{k+1}}_{b_{k+1}}}-\underline{R^{c_{0}}_{b_{k}}v^{b_{k}}_{b_{k}}}+g^{c_{0}} \Delta t_{k})$
將論文式(14) (即本文公式(2)) 代入$\alpha_{b_{k+1}}^{b_{k}}$有：$\delta \alpha^{b_{k}}_{b_{k+1}} = \alpha^{b_{k}}_{b_{k+1}}-R^{b_{k}}_{c_{0}}(s\bar{p}^{c_{0}}_{b_{k+1}}-s\bar{p}^{c_{0}}_{b_{k}}+\frac{1}{2} g^{c_{0}}\Delta t^{2}_{k}-R^{c_{0}}_{b_{k}}v^{b_{k}}_{b_{k}}\Delta t_{k}) \\= \alpha^{b_{k}}_{b_{k+1}}-R^{b_{k}}_{c_{0}}( s\bar{p}^{c_{0}}_{c_{k+1}}-R^{c_{0}}_{b_{k+1}}p^{b}_{c} -(s\bar{p}^{c_{0}}_{c_{k}}-R^{c_{0}}_{b_{k}}p^{b}_{c})+\frac{1}{2} g^{c_{0}}\Delta t^{2}_{k}-R^{c_{0}}_{b_{k}}v^{b_{k}}_{b_{k}}\Delta t_{k}) \\ = \alpha^{b_{k}}_{b_{k+1}}-sR^{b_{k}}_{c_{0}}( \bar{p}^{c_{0}}_{c_{k+1}}-\bar{p}^{c_{0}}_{c_{k}}) + R^{b_{k}}_{c_{0}}R^{c_{0}}_{b_{k+1}}p^{b}_{c} - p^{b}_{c}+v^{b_{k}}_{b_{k}}\Delta t_{k}-\frac{1}{2}R^{b_{k}}_{c_{0}} g^{c_{0}}\Delta t^{2}_{k}) = 0_{3 \times 1}$ 這裏$v_{b_{k}}^{b_{k}}$、$g^{c_{0}}$和$s$均爲待優化變量，將其轉成$Ax=b$的形式：
$R^{b_{k}}_{c_{0}}(\bar{p}^{c_{0}}_{c_{k+1}}-\bar{p}^{c_{0}}_{c_{k}})s-v^{b_{k}}_{b_{k}}\Delta t_{k}+\frac{1}{2}R^{b_{k}}_{c_{0}}g^{c_{0}}\Delta t_{k}^{2} = \alpha^{b_{k}}_{b_{k+1}}+R^{b_{k}}_{c_{0}}R^{c_{0}}_{b_{k+1}}p^{b}_{c}-p^{b}_{c} \tag{6}$ 對於$\beta^{b_{k}}_{b_{k+1}}$, 則有 $\delta \beta_{b_{k+1}}^{b_{k}}=\beta^{b_{k}}_{b_{k+1}}-R^{b_{k}}_{c_{0}}(R^{c_{0}}_{b_{k+1}}v^{b_{k+1}}_{b_{k+1}}-R^{c_{0}}_{b_{k}}v^{b_{k}}_{b_{k}}+g^{c_{0}}\Delta t_{k}) = 0_{3\times 1}$ 這裏的優化變量$v^{b_{k}}_{b_{k}}$、$v^{b_{k+1}}_{b_{k+1}}$、$g^{c_{0}}$，將其轉成$Ax=b$的形式：
$R^{b_{k}}_{c_{0}}R^{c_{0}}_{b_{k+1}}v^{b_{k+1}}_{b_{k+1}}-R^{b_{k}}_{c_{0}}R^{c_{0}}_{b_{k}}v^{b_{k}}_{b_{k}}+R^{b_{k}}_{c_{0}}g^{c_{0}}\Delta t_{k} = \beta^{b_{k}}_{b_{k+1}} \tag{7}$ 將公式(6)(7)寫出矩陣的形式：論文式【(18)(19)】
$H^{b_{k}}_{b_{k+1}} \mathcal{X}_{I} + n^{b_{k}}_{b_{k+1}}= \begin{bmatrix} -I \Delta t_{k} & 0 &\frac{1}{2}R^{b_{k}}_{c_{0}}\Delta t_{k}^{2} & R^{b_{k}}_{c_{0}}(\bar{p}^{c_{0}}_{c_{k+1}}-\bar{p}^{c_{0}}_{c_{k}}) \\ -I & R^{b_{k}}_{c_{0}}R^{c_{0}}_{b_{k+1}} & R^{b_{k}}_{c_{0}}\Delta t_{k} & 0\end{bmatrix}_{6\times10}\begin{bmatrix} v^{b_{k}}_{b_{k}} \\ v^{b_{k+1}}_{b_{k+1}}\\ g^{c^{0}} \\ s\end{bmatrix}_{10 \times 1} + n^{b_{k}}_{b_{k+1}} \\ = \begin{bmatrix} \alpha^{b_{k}}_{b_{k+1}}+R^{b_{k}}_{c_{0}}R^{c_{0}}_{b_{k+1}}p^{b}_{c}-p^{b}_{c} \\ \beta^{b_{k}}_{b_{k+1}}\end{bmatrix}_{6 \times 1} = \hat{z}^{b_{k}}_{b_{k+1}} \tag{8}$其中$R_{b_{k}}^{c_{0}}$、$R^{c_{0}}_{b_{k+1}}$、$\bar{p}_{b_{k}}^{c_{0}}$、$\bar{p}_{c_{k}}^{c_{0}}$可從視覺的SFM獲得，$\Delta t_{k}$爲相鄰倆幀圖像之間的時間間隔。
公式(8)轉換成線性最小二乘問題對狀態量進行求解：$\underset{\mathcal{X_{I}}}{min}\sum_{k\in \mathcal{B}} ||\hat{z}^{b_{k}}_{b_{k+1}}-H^{b_{k}}_{b_{k+1}}\mathcal{X}^{k}_{I}||^{2}$ 從矩陣角度來看，也看看做解方程 $H^{b_{k}}_{b_{k+1}}\mathcal{X}^{k}_{I}=\hat{z}^{b_{k}}_{b_{k+1}} \rightarrow Ax=b$。
具體代碼見 vins_estimator/initial/initial_aligment.cpp中的LinearAlignment().

3.3 優化重力向量$g^{c_{0}}$

爲什麼需要優化重力向量？
利用公式(8)求解的重力向量並沒有模長的限制，即$||g^{c_{0}}||=9.81$，因此重力向量$g^{c_{0}}$實際上只有倆個自由度。

採用球面座標，在切面空間上用兩個變量重新參數化重力，將其表示爲：$\hat{g^{c_{0}}} = ||g|| \cdot \hat{\bar{g}}^{c_{0}}+w_{1}\overrightarrow{b}_{1}+w_{2}\overrightarrow{b}_{2} = ||g|| \cdot \hat{\bar{g}}^{c_{0}} + \overrightarrow{b}w$ 其中$\hat{\bar{g}}^{c_{0}}$爲上一部分公式(8)得到的重力向量初始值的單位向量，$w_{2 \times 1} = [w_{1}, w_{2}]$爲待優化變量，$\overrightarrow{b}_{3\times 2} = [\overrightarrow{b}_{1},\overrightarrow{b}_{2}]$​中的$\overrightarrow{b}_{1},\overrightarrow{b}_{2}$在$||g|| \cdot \hat{\bar{g}}^{c_{0}}$的正切平面上且正交，定義如下：$\overrightarrow{b}_{1}= \left\{\begin{matrix} (\hat{\bar{g}}^{c_{0}} \times [1, 0 ,0]), \ \hat{\bar{g}}^{c_{0}} \neq [1,0,0]^{T} \\ (\hat{\bar{g}}^{c_{0}} \times [1, 0 ,0]), \ otherwise \end{matrix}\right. \ , \ \overrightarrow{b}_{2} = \hat{\bar{g}}^{c_{0}} \times \overrightarrow{b}_{1}$ 因此優化變量將從$g^{c_{0}}$(三個自由度)變爲$w^{c_{0}}$(倆個自由度)：
$H^{b_{k}}_{b_{k+1}} \mathcal{X}_{I} + n^{b_{k}}_{b_{k+1}}= \begin{bmatrix} -I \Delta t_{k} & 0 &\frac{1}{2}R^{b_{k}}_{c_{0}}\Delta t_{k}^{2} \overrightarrow{b} & R^{b_{k}}_{c_{0}}(\bar{p}^{c_{0}}_{c_{k+1}}-\bar{p}^{c_{0}}_{c_{k}}) \\ -I & R^{b_{k}}_{c_{0}}R^{c_{0}}_{b_{k+1}} & R^{b_{k}}_{c_{0}}\Delta t_{k} \overrightarrow{b} & 0\end{bmatrix}_{6\times 9}\begin{bmatrix} v^{b_{k}}_{b_{k}} \\ v^{b_{k+1}}_{b_{k+1}}\\ w \\ s\end{bmatrix}_{9 \times 1} + n^{b_{k}}_{b_{k+1}} \\ = \begin{bmatrix} \alpha^{b_{k}}_{b_{k+1}}+R^{b_{k}}_{c_{0}}R^{c_{0}}_{b_{k+1}}p^{b}_{c}-p^{b}_{c}-\frac{1}{2}R^{b_{k}}_{c_{0}}\Delta t_{k}||g|| \cdot \hat{\bar{g}}^{c_{0}}\\ \beta^{b_{k}}_{b_{k+1}}-R^{b_{k}}_{c_{0}}\Delta t_{k}||g|| \cdot \hat{\bar{g}}^{c_{0}}\end{bmatrix}_{6 \times 1} = \hat{z}^{b_{k}}_{b_{k+1}} \tag{8}$
採用最小二乘對待優化變量$\mathcal{X}_{I}$進行重新優化。
具體代碼見 vins_estimator/initial/initial_aligment.cpp中的RefineGrivity().

3.4 對齊導航世界座標系

由於我們的求解都是在第一幀相機座標系$c^{0}$的，因此需要計算$R^{w}_{c_{0}}$將所有$c^{0}$座標系下的變量轉到世界座標系下，令
$R^{w}_{c_{0}}=exp([\theta \mathbf{a}])$ 令$\hat{g}^{c_{0}}$爲重力細化後得到的$c^{0}$座標系的重力向量，$g^{w}=[0,0,9.81]^{T}$爲世界座標系下的重力向量，故單位旋轉軸$\mathbf{a}$爲: $\mathbf{a}=\frac{\hat{g}^{c_{0}} \times \hat{g}^{w}}{||\hat{g}^{c_{0}} \times \hat{g}^{w}||}$ 旋轉角度$\theta$爲：$\theta=atan \frac{\hat{g}^{c_{0}} \times \hat{g}^{w}}{\hat{g}^{c_{0}} \cdot \hat{g}^{w}} =atan \frac{||\hat{g}^{c_{0}}|| \cdot ||\hat{g}^{w}|| \sin \theta}{||\hat{g}^{c_{0}}|| \cdot ||\hat{g}^{w}|| \cos \theta}$

問題1: 加速度的偏置$b_{at}$爲什麼沒有估計？
由於線性加速度的模型爲：$\hat{a}_{t} = a_{t}+b_{at}+R_{w}^{t}g^{w}+n_{a}$，由於有重力的作用，加速度偏置$b_{a}$小$R_{w}^{t}g^{w}$幾個數量級，很難在初始化的時候估計重力向量的同時估計出加速度偏置，故沒必要初始化加速度偏置的初始值$b_{at}$

問題2：平移外參數$p^{b}_{c}$爲何沒有初始化？
由於平移相比於旋轉而言，對於系統來說是線性的，因爲平移與其他變量量幾乎爲加法運算。同時，IMU與相機可能距離很近，因此在初始化過程沒有必要初始化$p^{b}_{c}$，而是留到非線性優化的時候。