@
一、主要思想
我們的主要目標是更進一步，在初始化中使用地圖估計，適當使用傳感器噪聲模型。我們的新初始化方法基於以下思想:
- 盡管非線性性質的BA，單目SLAM(或視覺里程計)是足夠成熟和穩健的，在結構和運動上獲得非常准確的初始解。與唯一的警告，他們的估計是缺少尺度的
- 視覺SLAM軌跡的不確定性比IMU不確定性小得多，在求IMU變量的第一次解時可以忽略。因此，我們采用只有慣性的最大后驗估計，以實時的SLAM軌跡為常數。
- 受到[9]工作的啟發，我們采用了參數化，明確表示並優化了單目SLAM解決方案的尺度因子
- 不同於[5][7]，我們聯合優化所有IMU變量，考慮位置預積分項與線速度和角速度[8]之間的交叉協方差。
二、主要分為以下三步：
- 僅視覺的最大后驗估計使用BA在，短時間內(通常為2秒)初始化和運行單目ORB-SLAM[10]，以獲得缺少尺度的僅視覺最大后驗估計。同時，計算關鍵幀之間IMU預積及其協方差[8]。
- 僅慣性的最大后驗估計僅慣性優化通過對齊imu軌跡和slam的軌跡，去找到尺度因子、關鍵幀的速度、重力方向和imu的bias
- 僅視覺的最大后驗估計將上一步的解作為一個完整VI-BA的種子，得到聯合最優解。
初始化之后，我們啟動ORB-SLAM VI[5]，執行local VI-BA。我們觀察到，在執行完整的VI-BA 5-10秒后，尺度估計精度可以進一步提高，或者，在計算成本低得多的情況下，重復只進行慣性優化
三、詳細步驟
3.1 僅視覺的最大后驗估計
與ORB-SLAM唯一的區別是，我們以更高的頻率(4到10HZ)強制關鍵幀插入。這樣，IMU在關鍵幀之間的預積分具有較低的不確定性，因為積分時間非常短。經過這段時間，我們得到了一個由10個關鍵幀和數百個點組成的未知尺度的地圖，它是通過ORB-SLAM mapping線程對BA進行優化的。
3.2 僅慣性的最大后驗估計
這一步驟的目標是利用視覺獲得的缺少尺度的軌跡，從最大后驗估計的意義上獲得慣性參數的最優估計。因為我們沒有一個很好的關於慣性參數的估計，在這時使用full BA 將會過於昂貴，同時容易陷入局部最小值。一個中間的解決方案是邊緣化的點，以獲得一個先驗的軌跡和它的(全稠密)協方差矩陣，並在優化IMU參數時使用它。我們選擇一個更有效的解決方案，考慮軌跡為固定的，並執行慣性優化。需要找到的慣性參數為:
\(s \in \mathbb{R}^{+}\) :視覺的尺度因子
\(R_{wg} \in SO(3)\):為重力方向，由兩個角度參數化(pitch和roll)，因此重力在世界參照系中表示為\(g = R_{wg}g_I\), \(g_I = (0;0;G)^T\)是重力的大小
\(\mathbf{b}=\left(\mathbf{b}^{a}, \mathbf{b}^{g}\right) \in \mathbb{R}^{6}\) : 是加速度計和陀螺儀的bias
\(\overline{\mathbf{v}}_{0: k} \in \mathbb{R}^{3}\) : 是缺少尺度的從第一幀到上一幀的body速度
-
我們更喜歡使用無尺度的速度 \(\overline {v_i}\)，而不是真實的速度\(v_i = s \overline {v_i}\)，因為這樣可以簡化初始化過程。
-
由於初始化時間只有1-2秒，隨機游走幾乎沒有影響。所以假設所有關鍵幀的bias都是常數。
-
值得注意的是，這個公式從一開始就考慮了重力大小，而不像[7]和[5]，需要一個單獨的步驟來確定它的值。
我們用\(I_{i,j}\)表示第 i 和第 j 個關鍵幀之間的慣性測量預積分，用\(I_{0:k}\)表示初始化窗口內連續關鍵幀之間IMU預積分的集合
\(r_p\) 先驗誤差
\(r_{I_{{i-1},i}}\) : 預積分誤差，預積分誤差定義如下：
具體的看Foster的IMU預積分論文
重力方向的更新式定義如下：
尺度因子的更新式定義如下：
bias的估計：當運動導致的信息矩陣小的時候直接當0，足夠大了才優化
