VINS_Fusion 初始化過程

VINS_Fusion的初始化與VINS_mono差不多.
按照<<手寫VIO>>課程中的說法

1. 背景技術

在介紹VINS的初始化模塊之前,先介紹以下背景知識:

A.IMU預積分

IMU傳感器模型可以表示為如下式子:

\[ω̃^b = ω^b + b^g + n^g\\ ã^b = q_{bw} (a^w +g^w)+b^a+n^a \]

IMU 預積分即:將一段時間內的 IMU 數據直接積分起來就能得到兩時刻 i, j 之間關於 IMU 的測量約束,即 預積分量:

\[偽位移增量：\alpha_{b_ib_j}=\int\int_{t\in[i,j]}(q_{b_ib_t}a^{b_t})\delta t^2\\ 速度預計分量：\beta_{b_ib_j}=\int_{t\in[i,j]}(q_{b_ib_t}a^{b_t})\delta t\\ 兩幀之間的旋轉：q_{b_ib_t}=\int_{t\in[t,j]}q_{b_ib_t} \left[\begin{matrix}0\\1/2w^{b_t}\end{matrix}\right]dt \]

上式中,位移增量為什么是偽位移增量呢,因為加速度為0的時候,相機的速度不一定為0;
根據上述的數學模型,我們可以計算兩個時刻之間IMU的軌跡.

B.視覺幾何基礎

根據得到的兩張圖片,可以通過如下過程來計算相機的變換.

1. 已知兩圖像：特征點提取，匹配（光流，特征描述子）
2. 已知倆圖像特征匹配點:利用對極幾何約束 (E 矩陣,H 矩陣), 計算兩圖像之間的 pose (update to scale).
3. 已知相機 pose, 已知特征點二維坐標: 通過三角化得到三維坐標.
4. 已知 3d 點,2d 特征點:通過 Perspective-n-Point(PnP) 求取新的 相機 pose

根據上述過程,我們可以根據相機的圖片信息得到圖片之間的變換.

兩套軌跡帶來的問題->如何融合？

根據前兩個模型,我們得到了兩套的軌跡.對於這兩套軌跡,我門有以下問題:

1. IMU 怎么和世界坐標系對齊,計算初始時刻的 \(q_{wb_0}\) ?
2. 單目視覺姿態如何和 IMU 軌跡對齊,尺度如何獲取?
3. VIO 系統的初始速度 v,傳感器 bias 等如何估計?
4. IMU 和相機之間的外參數等...

VINS的初始化

1.視覺與IMU之間的聯系

基於這些聯系構建幾何約束

考慮相機坐標系 \(c_0\) 為世界坐標系,則利用外參數 \(q_{bc} , t_{bc}\) 構建等式

\[q_{c_0b_k} = q_{c_0b_k}⊗ q^{-1}_{bc}\\ s\overline{p}_{c_0b_k} = s\overline{p}_{c_0c_k}−R_{c_0b_k}p_ {bc} \]

帶一橫表示帶了尺度。其中,s 為尺度因子,\(\overline{p}\) 表示非米制單位的軌跡。等式(3)等價於

\[\overline{p}_{c_0b_k} = \overline{p}_{c_0c_k}−\frac{1}{s}R_{c_0b_k}p_ {bc}\\ \overline{p}_{c_0c_k} = \frac{1}{s}R_{c_0b_k}p_ {bc}+\overline{p}_{c_0b_k} \]

旋轉沒有尺度信息。

通過該約束來與相機提供的約束打通關系。

2.視覺IMU對齊流程

估計流程

1. 旋轉外參數 \(q_{bc}\) 未知, 則先估計旋轉外參數.（很多時候由於相機與IMU很近，因此平移參數不是很重要）

2. 利用旋轉約束估計陀螺儀 bias.（短時間內是個常數，因此需要估計以得到更准確的值）

   \[q_{c_0b_k} = q_{c_0c_k} ⊗ q^{-1}_{bc} \]

3. 利用平移約束估計重力方向,速度,以及尺度初始值.(把非米制單位拉伸到米制單位)

   \[s\overline{p}_{c_0b_k} = s\overline{p}_{c_0c_k} − R_{c_0b_k}p_{bc} \]

4. 對重力向量 \(g^{c_0}\) 進行進一步優化.

5. 求解世界坐標系 w 和初始相機坐標系 \(c_0\) 之間的旋轉矩陣 \(q_{wc_0}\),
   並將軌跡對齊到世界坐標系。

A. 利用旋轉約束估計外參數旋轉\(Q_{bc}\)

相鄰兩時刻 k, k + 1 之間有:IMU旋轉積分 \(q_{b_kb_{k+1}}\) ,視覺測量\(q_{c_kc_{k+1}}\) 。則有:

\[q_{b_kb_{k+1}}⊗q_{bc} = q_{bc}⊗q_{c_kc_{k+1}}=0 \]

上式可寫成:

\[([q_{b_kb_{k+1}}]_L-[q_{c_kc_{k+1}}]_R)q_{bc}= Q^{k}_{k+1}q_{bc}=0 \]

其中,\([·]_L , [·]_R\) 表示 left and right quaternion multiplication。

將多個時刻線性方程(6)累計起來(提高信噪比),並加上魯棒核權重得到:

\[\left[\begin{matrix} w^0_1Q^0_1\\w^1_2Q^1_2\\...\\w^{N-1}_NQ^{N-1}_N \end{matrix}\right] q_{bc}=Q_Nq_{bc}=0 \]

其中

\[w^k_{k+1}=\left\{\begin{array}{l} 1,& r^{k}_{k+1}<thresold\\ \frac{thresold}{r^{k}_{k+1}},& otherwise \end{array}\right. \]

由旋轉矩陣和軸角之間的關系 \(tr(R) = 1 + 2 cos θ\), 能得到角度誤差 r
的計算為:

\[r^k_{k+1}=acos((tr(\hat{R}^{-1}_{bc}{R}^{-1}_{b_kb_{k+1}}\hat{R}_{bc}{R}_{b_kb_{k+1}})-1)/2) \]

即從視覺數據計算得到的R應當與Q相同，因此括號中應該為一個單位矩陣

公式(7)的求解同樣采用 SVD 分解,即最小奇異值對應的奇異向量。

具體代碼見：initial_ex_rotation.cpp 函數 CalibrationExRotation()。

B. 基於旋轉約束的Gyroscope Bias

• 估計bias可以使兩個時刻之間IMU的旋轉更准。因為旋轉是通過積分得到的。

• 假定\(q_{c_kc_{k+1}}\)准確，且g估計到的外參准確，則\(q_{c_kb_k}\)准確.

如果外參數 \(q_{bc}\) 已標定好,利用旋轉約束,可估計陀螺儀 bias:

\[arg \min_{\delta b^g}\sum_{k\in\Beta}||2|q^{-1}_{c_0b_{k+1}}⊗q_{c_0b_k}⊗q_{b_kb_{k+1}}|_{xyz}||^2 \]

其中, B 表示所有的圖像關鍵幀集合,另有預積分的一階泰勒近似:

\[q_{b_kb_{k+1}}\approx \hat{q}_{b_kb_{k+1}}⊗ \left[\begin{array}{c} 1\\\frac{1}{2}J^q_{b^g}\delta b^g \end{array}\right] \]

\(q_{b_kb_{k+1}}\)的誤差近似於第二項。

公式(10)為普通的最小二乘問題,求取雅克比矩陣,構建正定方程 HX = b =0 即可以求解 。

具體代碼見:initial_aligment.cpp 函數 solveGyroscopeBias().

C. 初始化速度、重力和尺度因子

研究將IMU與相機對齊之后如何初始化速度、重力和尺度因子。

待估計變量

\[\chi_I=[v^{b_0}_0,v^{b_1}_1,...,v^{b_n}_n,g^{c_0},s]^T \]

其中,\(v_k^{b_k}\)表示k時刻body 坐標系的速度在 body 坐標系下的表示。
\(g^{c_0}\)為重力向量在第 0 幀相機坐標系下的表示。s 表示尺度因子,將視
覺軌跡拉伸到米制單位。

回顧：預積分量約束

世界坐標系w下有

\[\alpha_{b_ib_j}=q_{b_iw}(p_{wb_j}-p_{wb_i}-v^w_i\Delta t+\frac{1}{2}g^w\Delta t^2)\\ \beta_{b_ib_j}=q_{b_iw}(v^w_j-v^w_i+g^w\Delta t) \]

將世界坐標系 w 換成相機初始時刻坐標系 \(c_0\) 有

\[\alpha_{b_kb_{k+1}}=R_{b_kc_0}(s(\overline{p}_{c_0b_{k+1}}-\overline{p}_{c_0b_k}))+\frac{1}{2}g^{c_0}\Delta t^2_k -R_{c_0b_k}v^{b_k}_k\Delta{t_k}\\ \beta{b_kb_{k+1}}=R_{b_kc_0}(R_{c_0b_{k+1}}v_{k+1}^{b_{k+1}}+g^{c_0}\Delta t^2_k -R_{c_0b_k}v^{b_k}_k\Delta{t_k} \]

將公式(3)代入公式(14)進行簡單整理有:

\[\alpha_{b_kb_{k+1}}=sR_{b_kc_0}(\overline{p}_{c_0c_{k+1}}-\overline{p}_{c_0c_k})-R_{b_kc_0}R_{c_0b_{k+1}}p_{bc}+p_{bc}+\frac{1}{2}R_{b_kc_0}g^{c_0}\Delta t^2_k-v_k^{b_k}\Delta t_k \]

將待估計變量放到方程右邊，有：

具體代碼見:initial_aligment.cpp 函數 LinearAlignment().

D. 優化重力向量\(g^{c_0}\)

疑問:為什么需要優化重力向量

利用公式(16)求解重力向量 g_{c_0} 過程中,並沒有加入模長限制
\(∥g^{c_0}∥ = 9.81\)。三維變量 \(g^{c_0}\) 實際只有兩個自由度。

重力向量的參數化

三維向量自由度為 2,可以采用球面坐標
進行參數化:

\[ĝ^{c_0} = ∥g∥·ĝ^{c_0}+w_1b_1 + w2b_2 \]

其中,w1, w2 為待優化變量

\[b_1=\left\{\begin{array}{l} \hat{\overline{g}}^{c_0}\times [1,0,0],& \hat{\overline{g}}^{c_0} \neq [1,0,0]^T\\ \hat{\overline{g}}^{c_0}\times [0,0,1],& otherwise \end{array}\right.\\ b_2 = \hat{\overline{g}}^{c_0}\times b_1 \]

將公式g代入，待優化變量變為：

\[\chi_I= \left[ \begin{matrix} v^{b_k}_k\\v^{b_{k+1}}_{k+1}\\g^{c_0}\\s \end{matrix}\right] \rarr \left[ \begin{matrix} v^{b_k}_k\\v^{b_{k+1}}_{k+1}\\w^{c_0}\\s \end{matrix}\right] \]

公式中的觀測方程變為：

\[\hat{z}^{b_k}_{b_k+1} = \left[\begin{matrix} \alpha_{b_kb_{k+1}}-p_{bc}+R_{b_kc_0}R_{c_0b_{k+1}}p_{bc}-\frac{1}{2}R_{b_kc_0}\Delta t^2_k||g||\hat{\overline{g}}^{c_0} \\ \beta_{b_kb_{k+1}}-R_{b_kc_0}\Delta t_k ||g|| \hat{\overline{g}}^{c_0} \end{matrix}\right] \]

然后采用最小二乘對\(\chi_I\)重新進行優化。

E. 將相機坐標系對齊到世界坐標系

對齊流程

1. 找到 \(c_0\) 到 w 系的旋轉矩陣 \(R_{wc_0} = \exp([θu])\)

   \[$u =\frac{ĝ^{c_0}\times ĝ^w}{∥ĝ^{c_0}×ĝ^w∥} θ = atan2(∥ĝ^{c_0}\times ĝ^w ∥,ĝ^{c_0}·ĝ^w) \]

2. 把所有 \(c_0\) 坐標系下的變量旋轉到 \(w\) 下。

3. 把相機平移和特征點尺度恢復到米制單位。

4. 至此,完成了系統初始化過程

初始化拓展

疑問

1. 加速度 bias 為何沒有估計? 加速度bias一般很小，不一定能估計出來，與重力向量相比，量綱很小

2. 平移外參數 \(p_{bc}\) 為何沒有初始化?

   答：影響不大。1.陀螺儀的bias會影響旋轉，旋轉是一個非線性的東西，平移是線性的東西。旋轉比平移重要。2. 平移外參一般在設備上很小，影響不大，可提前設置一個值，在后續中優化。

其他初始化方法

1. 靜止初始化:直接用加速度測量重力方向,初始速度為 0.
2. 除 vins-mono 外的其他運動初始化方案 a , b 。

• Janne Mustaniemi et al. “Inertial-based scale estimation for structure from motion on mobile devices”. In: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE. 2017, pp. 4394–4401.
• Javier Domínguez-Conti et al. “Visual-Inertial SLAM Initialization: A General Linear Formulation and a Gravity-Observing Non-Linear Optimization”. In: 2018 IEEE International Symposium on Mixed and Augmented Reality (ISMAR). IEEE. 2018, pp. 37–45.