VIO 初始化相關文獻調研

前言

SLAM一般采用基于疊代的貝葉斯估計方法或者基于平滑的方法進行狀态估計，這兩種方法都需要精準的狀态初始值，是以魯棒、快速的初始化可以用來提供精确的初始狀态估計，這一環節是實時VINS的核心部分。

初始化主要的工作是研究VIO系統的可觀測性質，IMU的全局位置和yaw是不可觀測量[1, 2]；以下的量是可觀測量：

（O1）相對于水準面的IMU姿态，也就是roll和pitch

（O2）相對于IMU初始幀的IMU軌迹（位置、速度和方向）

（O3）相對于IMU初始幀的特征點位置

（O4）IMU與相機坐标系之間的變換矩陣，也就是相機IMU外參

（O5）IMU陀螺儀和加速計的bias

論文調研

閉式求解的方法：

2011，ICRA，Kneip——Closed-Form Solution for Absolute Scale Velocity Determination Combining Inertial Measurements and a Single Feature Correspondence [3]

假設：

1. 視覺和IMU測量無噪聲
2. 理想的IMU，沒有重力和bias影響
3. IMU和相機的外參已知

方法：

1. 建立IMU位置和速度積分模型
2. 利用IMU相機外參将IMU積分模型和三個連續幀，一個特征點建立轉換關系
3. 建立含有速度和尺度狀态的線性系統求解
4. 估計（O1）速度和（O3）

優缺點：

優點

1. 資料簡單，隻利用三個連續圖像幀和一個特征點比對的視覺資訊

缺點

1. 理想IMU模型
2. 測量值均為無噪聲資料

2014，IJCV，Martinelli——Closed-form solution of visual-inertial structure from motion [4]

假設：

1. 視覺和IMU測量無噪聲
2. 陀螺儀沒有bias

IMU和相機的外參已知

方法：

1. 建立IMU位置積分模型
2. 利用IMU相機外參将IMU積分模型和視覺測量值對齊
3. 建立全狀态的線性方程，求解重力，尺度，速度和加速度計bias
4. 利用重力幅值為9.8，建立有限制的最小二乘問題，進一步優化重力

優缺點：

優點

1. 全狀态初值估計架構，後面的一些方法也是建立在這個整體架構上的
2. 增加了acc bias估計

缺點

1. 求解過程中忽略了 gyro bias 的影響
2. 測量值均為無噪聲資料

2017，RAL，Kaiser——Simultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation [5]

假設：

1. 視覺和IMU測量無噪聲
2. IMU和相機的外參已知

方法（類似[4]）：

1. 建立IMU位置積分模型
2. 利用IMU相機外參将IMU積分模型和視覺測量值對齊
3. 建立全狀态的線性方程，求解重力，尺度，速度和加速度計bias
4. 利用重力幅值為9.8，建立有限制的最小二乘問題，進一步優化重力
5. 分析gyro bias對系統影響較大，建立gyro bias為參數的最小二乘問題，估計gyro bias
6. 分析acc bias對系統影響不大，忽略

優缺點：

優點

1. 增加了gyro bias估計
2. 分析了IMU bias對系統的影響

缺點

1. 求解過程中忽略了 acc bias 的影響
2. 測量值均為無噪聲資料

閉式解改進的方法：

2019，ICRA，Campos——Fast and Robust Initialization for Visual-Inertial SLAM [9]

假設：

1. 視覺和IMU測量無噪聲
2. IMU和相機的外參已知
3. 忽略acc bias影響

方法（類似[4]）：

1. 将IMU預積分和[4]閉式方法結合，建立含有gyro bias，尺度，重力，速度的最小二乘問題，同時估計出特征點的位置

2. 第一次BA，利用之前估計的特征點和位姿，計算海塞矩陣的奇異值，進行可觀性分析，通過奇異值門檻值限定去掉不理想的初始值，主要針對純旋轉和勻速運動導緻系統觀測性能下降

   第二次BA，利用沒有用過的跟蹤點，進行内點檢測，去掉不理想的初始值

優缺點：

優點

1. 使用部分跟蹤點，提高了效率
2. 兩次BA，提高了初始化的精度和魯棒性
3. 結合了預積分和閉式解，提高了效率

缺點

1. 求解過程中忽略了 acc bias 的影響
2. 測量值均為無噪聲資料
3. 隻有20%的軌迹點正确工作，召回率低，初始化時間較長

優化疊代的方法：

2017，RAL，Mur-Artal——Visual-Inertial Monocular SLAM with Map Reuse [6]

假設：

1. 先運作單目SLAM使得變量可觀
2. IMU和相機的外參已知

方法：

1. 利用IMU旋轉預積分模型和單目估計的旋轉，建立最小二乘問題，疊代估計gyro bias
2. 忽略acc bias，利用外參和IMU位置預積分模型建立線性系統，SVD估計尺度和重力
3. 利用Body系和2中估計重力的旋轉建立線性方程，SVD估計acc bias和優化尺度，重力方向
4. 利用IMU速度預積分模型和外參建立線性系統，SVD估計速度

優缺點：

優點

1. 疊代估計 IMU 初始狀态所有參數，包括acc bias
2. 分步估計初始化參數，轉換為幾個分問題，提高效率

缺點

1. 勻速運動時，IMU的加速度為0，初始化的尺度将變得不準
2. 依賴前面的單目SLAM進行的初始姿态估精度

2017，IROS，Qin——Robust initialization of monocular visual-inertial estimation on aerial robots [7] and VINS-MONO

假設：

1. 忽略 acc bias影響
2. IMU和相機的外參已知

方法：

1. SfM求解滑窗内所有幀的位姿，同時相應轉換到Body系中（initial_sfm.cpp—construct）

   相機旋轉和IMU旋轉預積分模型建立旋轉限制，建立最小二乘問題，估計gyro bias （solveGyroscopeBias）

2. IMU位置和速度預積分模型，外參和SfM的姿态，建立最小二乘問題，估計速度，重力和尺度 （LinearAlignment）
3. 利用重力幅值為9.8，建立重力的正切空間擾動模型，代入3的問題中，進一步優化重力（RefineGravity）
4. 對齊滑窗第一幀和世界坐标系，然後将所有估計值轉換到世界坐标系中

優缺點：

優點

1. 視覺與IMU對齊，将視覺對齊到重力系下後，使俯仰角變的可觀，地圖優化時變成了4自由度的位姿圖優化，并且可使單目情況下的SFM恢複尺度。并且提供了首次滑窗的狀态量的初值
2. 支援陀螺儀bias的初始估計
3. 分步估計，轉換為幾個分問題，提高效率

缺點

1. 當做勻速運動時，IMU的加速度為0，初始化的尺度将變得不準
2. 忽略了acc bias
3. 依賴前面的SfM進行的初始姿态估精度

2018，ICRA，Huang——Online Initialization and Automatic Camera-IMU Extrinsic Calibration for Monocular Visual-Inertial SLAM [10]

假設：

1. 相機和IMU測量值硬體時間戳同步

方法（類似 [6] 分步求解）：

1. ORB-SLAM 單目估計位姿
2. 通過IMU和相機坐标系在同一時間段内的旋轉變換，建立外參旋轉（四元數表達）為變量的線性方程（方法同 [11]），估計外參旋轉量
3. 利用已經估計出的外參旋轉量，IMU旋轉預積分模型和單目估計的旋轉，建立最小二乘問題，疊代估計gyro bias
4. 忽略acc bias，利用估計出的外參旋轉量和IMU位置預積分模型建立線性系統，SVD估計尺度因子，重力和外參平移量
5. 利用Body系和4中估計出的重力之間的旋轉建立線性方程，SVD估計acc bias，優化尺度，重力方向和外參平移量
6. 利用前面幾步估計出的尺度因子，acc bias，重力，外參，通過IMU速度預積分模型計算每一個關鍵幀的速度

優缺點：

優點

1. 相對于 [6]和[7]，增加了外參的估計，也就是求解了VIO初始化中所有的參數

缺點

1. 當做勻速運動時，IMU的加速度為0，初始化的尺度将變得不準
2. 依賴前面的單目SLAM進行的初始姿态估精度

2017，TASE，Yang——Monocular visual-inertial state estimation with online initialization and camera-IMU extrinsic calibration [12]

假設：

1. 足夠的運動激勵，使得尺度可觀
2. 線性初始化過程很短，忽略IMU biases

方法：

1. 五點法+Ransac估計相對視覺位姿
2. 通過IMU和相機坐标系在同一時間段内的旋轉變換，建立外參旋轉（四元數表達）為變量的線性方程（方法同 [11]），估計外參旋轉量，之後的步驟中外參旋轉固定為常值
3. 利用IMU預積分位置、速度和重力，建立線性IMU測量模型
4. 利用投影模型，建立線性相機測量模型
5. 使用3和4建立MLE問題，轉換為資訊矩陣和資訊向量的線性方程求解速度，姿态（重力），特征深度（尺度）和外參平移量
6. 建立全誤差狀态的最小二乘問題，轉換為資訊矩陣和資訊向量的線性系統求解誤差狀态，疊代優化狀态

優缺點：

優點

1. 相對于[6]和[7]，增加了外參的估計，也就是求解了VIO初始化中所有的參數
2. 建立整體誤差函數，進行優化求解，系統相對不複雜
3. 初始化過程中增加收斂校驗和終止條件
4. 引入雙向邊緣化的視差檢測，增強系統魯棒性（規避懸停，勻速運動的尺度不可觀的退化運動）

缺點

1. 依賴前面的單目SLAM進行的初始姿态估精度
2. 線性初始化過程忽略IMU biases

2020，ICRA，Campos——Inertial-only optimization for visual-inertial initialization [13]

假設：

1. 單目SLAM相對于IMU是精準的
2. 外參已知

方法：

1. 純單目SLAM，産生10個關鍵幀和上百個地圖點，将視覺位姿利用外參轉換為機體位姿
2. 狀态包含尺度，重力方向，IMU biases和每一幀速度，IMU預積分為測量值，建立含先驗的MAP問題
3. 用估計出的尺度因子将相機位姿，速度和3D地圖點進行尺度拉伸，然後用估計出的重力方向将這些量旋轉對齊至z軸，IMU預積分根據估計出的biases重新計算
4. 視覺-IMU聯合BA，當平均加速度小于某個門檻值則初始化成功完成

優缺點：

優點

1. 慣性參數的估計建立為最優估計問題（MAP問題），考慮了IMU噪聲
2. 一次估計出所有的慣性參數，避免資料不一緻問題
3. 估計過程中不需要忽略biases，因為将其已知的資訊作為機率先驗進入MAP

缺點

1. 依賴前面的單目SLAM進行的初始姿态估精度
2. 外參需要提前知道

參考文獻

[1] E. Jones and S. Soatto, “Visual-inertial navigation, mapping and localization: A scalable real-time causal approach,” International Journal of Robotics Research, vol. 30, no. 4, pp. 407–430, Apr. 2011.

[2] J. Kelly and G. Sukhatme, “Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration,” International Journal of Robotics Research, vol. 30, no. 1, pp. 56–79, Jan. 2011.

[3] L. Kneip, et al, “Closed-Form Solution for Absolute Scale Velocity Determination Combining Inertial Measurements and a Single Feature Correspondence,” ICRA, 2011.

[4] A. Martinelli, “Closed-form solution of visual-inertial structure from motion,” Int. J. Comput. Vis., vol. 106, no. 2, pp. 138–152, 2014.

[5] J. Kaiser, A. Martinelli, F. Fontana, and D. Scaramuzza, “Simultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation,” IEEE Robotics and Automation Letters, vol. 2, no. 1, pp. 18–25, 2017.

[6] R. Mur-Artal and J. D. Tard´os, “Visual-inertial monocular slam with map reuse,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 796–803, April 2017.

[7] T. Qin and S. Shen, “Robust initialization of monocular visual-inertial estimation on aerial robots,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Vancouver, Canada, 2017, pp. 4225–4232.

[8] T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.

[9] C. Campos, et al, “Fast and Robust Initialization for Visual-Inertial SLAM,” ICRA, 2019.

[10] Huang, et al, “Online Initialization and Automatic Camera-IMU Extrinsic Calibration for Monocular Visual-Inertial SLAM,” ICRA, 2018.

[11] Dong-Si, et al, “Estimator Initialization in Vision-aided Inertial Navigation with Unknown Camera-IMU Calibration,” ICRA, 2012.

[12] Z. Yang and S. Shen, “Monocular visual-inertial state estimation with online initialization and camera-IMU extrinsic calibration,” IEEE Trans. Autom. Sci. Eng., vol. 14, no. 1, pp. 39–51, 2017.

[13] C. Campos, J. M. M. Montiel, and J. D. Tardos, “Inertial-only optimization for visual-inertial initialization,” in IEEE International Conference on Robotics and Automation (ICRA), 2020.