@
一、VIO（Visual-Inertial Odometry）概述
VIO即以視覺（相機）與 IMU 融合實現里程計的方法
IMU（Inertial Measurement Unit），慣性測量單元.
- 典型 6 軸 IMU 以較高頻率（≥ 100Hz）返回被測量物體的角速度
與加速度 - 受自身溫度、零偏、振動等因素干擾，積分得到的平移和旋轉容
易漂移
Visual Odometry 視覺里程計
- 以圖像形式記錄數據，頻率較低（15 − 60Hz 居多）
- 通過圖像特征點或像素推斷相機運動`
1.1 IMU概述
六自由度 IMU 本身由一個陀螺儀和一個加速度計組成，分別測量自身
的角速度和加速度。
手機等電子產品多使用價格低廉的 MEMS IMU（如 MPU 6050），自動駕駛類則多使用幾萬元的 IMU（如 Apollo 中使用的 Novatel SPAN-IGM-A1）
1.2 IMU 與視覺定位方案優勢與劣勢對比：
整體上，視覺和 IMU 定位方案存在一定互補性質：
• IMU 適合計算短時間、快速的運動；
• 視覺適合計算長時間、慢速的運動。
同時，可利用視覺定位信息來估計 IMU 的零偏，減少 IMU 由零偏導致的發散和累積誤差；反之， IMU 可以為視覺提供快速運動時的定位。
1.3 IMU 數據可與多種定位方案融合
• 自動駕駛中通常用 IMU+GPS/差分 GPS/RTK 的融合定位方案，形成 GNSS-INS 組合導航系統，達到厘米組定位精度；
• 頭戴式 AR/VR 頭盔則多使用視覺 +IMU 的 VIO 定位系統，形成高幀率定位方案
1.3.1 松耦合
將 IMU 定位與視覺/GNSS 的位姿直接進行融合，融合過程對二者本身不產生影響，作為后處理方式輸出。典型方案為卡爾曼濾波器。
1.3.2 緊耦合
融合過程本身會影響視覺和 IMU 中的參數（如 IMU 的零偏和視覺的尺度）。典型方案為 MSCKF 和非線性優化。
`為什么要使用緊耦合`：
• 單純憑（單目）視覺或 IMU 都不具備估計 Pose 的能力：視覺存
在尺度不確定性、 IMU 存在零偏導致漂移；
• 松耦合中，視覺內部 BA 沒有 IMU 的信息，在整體層面來看不是
最優的。
• 緊耦合可以一次性建模所有的運動和測量信息，更容易達到最優。
二、預備知識
2.1 數學符號約定
- 普通變量： \(a, b, c\)
- 矩陣和向量：A,B, v
- 集合：\(\mathbb{R}, \mathbb{Z}\)
- 特殊集合： \(\mathcal{F}, \mathcal{G}\)
- 希臘字母和向量： \(\alpha\)
- 李代數： \(\mathfrak{s o}(3), \mathfrak{s e}(3)\)
2.2 三維剛體運動
2.3 四元數
上述運算還可以表述為矩陣相乘的形式：
四元數相關知識可以參考資料：https://download.csdn.net/download/qq_34213260/12504624
2.3.1 四元數和角軸的轉換關系
2.3.2 四元數求導
2.3.3 四元數更新
當我們用計算出來的 角速度ω 對某旋轉更新時
2.4 李代數
2.4.1 李群李代數的關系
由於\(\phi\)是三維向量, 我們可以定義它的模長和它的方向，分別記作 \(\theta\) 和 \(a,\) 於是有 \(\phi=\theta a_{0}\) 這里 \(a\) 是一個長度為 1 的方向向量，即 || \(a \|=1_{\circ}\) 。因此可以推導出
\(\phi=\ln (\boldsymbol{R})^{\vee}=\arccos \frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\)
\(\phi\)的物理含義就是[x,y,z]三軸的角度
上式還有一種更易理解與常用的形式如下：
2.4.2 李代數更新
補充：
$$a \times b = -b \times a$$
2.4.3 李代數求導與擾動模型
2.4.4 有關 SE(3)
由於 SE(3) 李代數性質復雜，在 VIO 中，我們通常使用SO(3) + t 的形式表達旋轉和平移。對平移部分使用矢量更新而非SE(3) 上的更新