手寫VIO（一）概論與預備知識

第一章 VIO概論

當前市面上很少有系統介紹VIO系統的書

一 VIO簡介

VIO（Visual-Inertial Odometry）：以視覺和IMU融合的方法實現定位的里程計

接下來介紹VIO使用到的兩個信息

1. IMU（Inertial MeasureMent Unit）慣性測量單元

• 典型的6軸IMU能以較高頻率（>= 100Hz）返回被測量物體的角速度和加速度
• IMU的缺點在於，容易收到自身溫度、靈片、振動等因素干擾，通過積分直接得到的評議和旋轉將產生漂移

2. 視覺信息

• 通過視覺定位的方法即視覺SLAM，主要利用相機采集的圖片信息作為輸入，采樣頻率較低（15-60Hz居多）
• 定位的時候主要通過圖像特征點或像素推斷相機運動

之所以使用以上兩個硬件主要是由於二者的互補性。

3. 二者的互補性

IMU與視覺定位的優劣勢對比

方案	IMU	視覺
優勢	快速響應 不受成像質量影響 角速度普遍比較准確 可估計絕對尺度	不產生漂移 直接測量旋轉與平移
劣勢	存在零偏 低精度IMU積分位子發散 高精度IMU價格昂貴	受圖像遮擋、運動物體干擾 單目視覺無法測量尺度 單目純旋轉運動無法估計 快速運動時易丟失

可以看出視覺與IMU之間存在一定的互補性質：

• IMU適合於計算短時間 快速的運動
• 視覺適合計算長時間 慢速的運動

同時，可以利用視覺定位信息來估計好吃IMU的零偏，減少IMU由零偏導致的發散和累積誤差。

反之，IMU也可以為視覺提供快速運動的定位。

應用

IMU數據可以多種定位方案融合

• 自動駕駛中通常使用IMU+GPS差分GPS/RTK的融合定位方案，形成GNSS-INS組合導航系統，打倒厘米組定位精度；

• 頭戴式AR/VR頭盔則多使用視覺+IMU的VIO定位系統，形成高幀率定位方案

里程計的后端

根據信息融合的方式進行分類 我們可以把視覺里程計分為松耦合和緊耦合兩種

1. 松耦合

   將 IMU 定位與視覺/GNSS 的位姿直接進行融合，融合過程對二者本身不產生影響，作為后處理方式輸出。典型方案為卡爾曼濾波器。

2. 緊耦合

   融合過程本身會影響視覺和 IMU 中的參數（如 IMU 的零偏和視覺的尺度）。典型方案為 MSCKF 和非線性優化。

為什么要使用緊耦合？

• 單純憑（單目）視覺或 IMU 都不具備估計 Pose 的能力：視覺存在尺度不確定性、IMU 存在零偏導致漂移；
• 松耦合中，視覺內部 BA 沒有 IMU 的信息，在整體層面來看不是最優的。
• 緊耦合可以一次性建模所有的運動和測量信息，更容易達到最優。

二 預備知識

2.1 三維剛體運動的表示

首先需要在機器人/車輛上定義各種坐標系，如：

1. 世界坐標系 W；
2. IMU坐標系 I；
3. 相機坐標系 C；

坐標系之間的變換關系由一個SE(3)給出。如I到W系的變換矩陣：\(T_{W_i}\)

\[T_{W_i} = \left[ \begin{array}{ccc} R_{WI}&t_{WI}\\ 0^{T}&1 \end{array} \right] \in \R^{4\times 4} \]

\(R_{WI}\)為\(3\times 3\)的旋轉矩陣，\(t_{WI}\)為平移向量。

\(T_{WI}\)右乘一個I系下（其次）坐標，將得到該點W系下坐標。

通常默認有以下約定：

• 當某個量表達坐標系的轉換關系，寫在右下腳標，例如\(T_{WB}\)。
• 當表達矢量在某坐標系中取的坐標時，寫在右上角標，如\(v^{\omega}\)表達速度矢量在World系坐標
• IMU坐標系即I系也稱為Body系。
• 定義明確時，有時會省略該腳標，我們會直接談論R,t這樣的量
• 不刻意區分齊次和非齊次坐標，因為在程序中可以自動完成轉換，且無歧義
• 默認以 \(T_{WI}\) 表達並存儲 IMU 的定位信息，而不是 \(T_{IW}\) 。二者實際互為逆，存儲哪一類區別不大，視習慣而定。
• 同理，\(T_{WI}\) 的平移部分可直接視作 IMU 在世界中的坐標，從而進行繪圖或可視化操作。

四元數

旋轉矩陣 R 亦可用四元數 q 描述。

• 四元數 q 有一個實部和三個虛部。我們把實部寫在前：\(q = [q0, q1, q2, q3]^⊤\) 或 \(q = [w, x, y, z]^⊤\)

  其中 \(q_0\) 為實部，\([q_1, q_2, q_3]^⊤\) 為虛部。因為實部為標量，虛部為矢量，所以也可記為：

  \[q = [s, v]^⊤ \]

  其中 s 為標量，v 為虛部的矢量。

四元數乘法

此外，四元數可類似復數，定義加減、模長、逆、共軛等運算，不一一展開。

角速度：

\[\omega = \lim_{\Delta \rarr 0}\frac{\partial \theta}{\Delta t} \]

四元數的導數

李代數

除了利用四元數求導，亦可利用李代數進行旋轉求導。使用旋轉矩陣 R 時，角速度為 ω，那么 R 相對於時間的導數可寫作：

\[\dot {R} = Rω \]

該式被稱為泊松公式（Possion’s equation），其中 ∧ 為反對稱矩陣算子：

在本課程中亦記作：\(ω_×\) 或者 \([ω]_×\)，表達含義相同。

so(3) 導數

在優化帶有旋轉的函數時，通常計算一個增量 ϕ ∈ so(3)，然后用它更新當前估計值：

\[R ← R \exp(ϕ^∧) \]

其中 exp 為 so(3) 至 SO(3) 上的指數映射。
本課程習慣為右乘，實際當中左右乘等價，僅為習慣上的差別。
注：(i) 不同的 R 函數，具體的導數形式也不同。(ii) 在程序中，不必區分 R 是以矩陣存儲或是以四元數存儲，只需按照該式更新即可。

2.2 重要的雅克比求導

常見的一些雅可比（以自變量為 R 舉例）：

1. 旋轉點的左擾動雅可比：

（中間的變換可以考慮向量的叉乘形式）。

2. 旋轉點的右擾動雅克比

3. 旋轉連乘的雅克比：

其中用到了對\(R\):

\[\ln(R\exp(\phi^{\wedge}))^{\vee} = \ln(R)^{\vee}+J^{-1}_r\phi \]

和\(J_r^{-1}\)為SO3上的右雅克比：

4. 旋轉連乘的雅克比：

這里用到了SO(3)的伴隨性質：

有關 SE(3)：由於 SE(3) 李代數性質復雜，在 VIO 中，我們通常使用SO(3) + t 的形式表達旋轉和平移。對平移部分使用矢量更新而非SE(3) 上的更新。

三 后續作業

3.1 VIO 文獻閱讀

閱讀VIO 相關綜述文獻如a，回答以下問題：

1. 視覺與IMU 進行融合之后有何優勢？

答：

視覺定位的優點在於不會產生漂移，可以直接測量旋轉和平移；缺點在於當存在外界圖像遮擋、且運動物體干擾時影響較大，定位不准；當使用單目視覺進行測量時還存在尺度不確定性的問題，而且單目視覺定位無法估計純旋轉運動；當相機快速運動時定位容易丟失。

IMU的優點在於輸出頻率高（100hz以上）能夠快速響應，受外界影響小（不受成像質量影響），角速度普遍比較准確，並且可估計絕對尺度；缺點在於IMU受溫度影響，存在零偏；並且使用低精度IMU得到積分容易發散，長時間跟蹤容易產生累積漂移。而高精度的IMU價格比較昂貴。

2. 有哪些常見的視覺+IMU 融合方案？有沒有工業界應用的例子？

常見的視覺+IMU融合方案包括：

1. VINS-mono及各種改進（VINS_Fusion）
2. MSCKF (基於KF濾波器實現)
3. OKVIS
4. ROVIO
5. VIORB
6. ICE-BA(百度)

工業界應用的例子包括AR Core。虛擬現實領域中AR Kit 通過VIO實現移動設備在空間中的精確定位。在虛擬現實頭顯中的Inside-out 定位中，目前被廣泛采用的方案就是視覺慣性傳感器融合實現 SLAM的 VINS 算法。

3.在學術界，VIO 研究有哪些新進展？有沒有將學習方法用到VIO中的例子？你也可以對自己感興趣的方向進行文獻調研，闡述你的觀點

目前的學術界中，VIO提高的主要方向包括精度、魯棒性、效率三部分；

在VIO中，可以深度學習網絡來預測相機的位姿狀態、場景深度等。還可以使用深度網絡剔除動態物體等以提高VIO系統的魯棒性。

利用了深度學習的例子包括有D3VO（使用深度學習來預測場景深度、光度、姿態等），VINet（使用深度網絡來預測相機位姿）DeepVO（使用網絡來預測位姿）等；

3.2 四元數和李代數更新

課件提到了可以使用四元數或旋轉矩陣存儲旋轉變量。當我們用計算出來的對某旋轉更新時，有兩種不同方式：

\[R\leftarrow R\exp(\omega^\wedge) \]

或

\[q\leftarrow q\otimes[1,\frac{1}{2}\omega]^T \]

請編程驗證對於小量\(\omega = [0.01, 0.02, 0.03]^T\)，兩種方法得到的結果非常接近，實踐當中可視為等同。因此，在后文提到旋轉時，我們並不刻意區分旋轉本身是q 還是R，也不區分其更新方式為上式的哪一種。

答：

編程過程包括如下步驟：

1. 生成初始的四元數和隨機矩陣
2. 利用小量生成旋轉角和旋轉軸，得到對應的旋轉矩陣和四元數
3. 更新四元數和旋轉矩陣
4. 計算差值

部分源代碼如下：

int main()
{
    Vector3d w(0.01,0.02,0.03); //旋轉向量 可以理解為角速度
    //生成一個隨機的旋轉矩陣
    Vector3d v = Vector3d::Random();
    v.normalize();
    // Matrix3d R = Eigen::Matrix3d::Random();
    Matrix3d R = Eigen::AngleAxisd(M_PI/4,v).toRotationMatrix();
    Quaterniond q(R);
    cout<<"初始四元數:"<< endl <<q.coeffs().transpose()<<endl;
    cout<<"初始旋轉矩陣:"<< endl <<R<<endl;

    //獲得旋轉向量對應的旋轉角
    double theta = w.norm(); //旋轉對應的旋轉角
    Vector3d n_w = w/theta; //歸一化得到旋轉角得到的旋轉軸
    //利用旋轉角更新旋轉矩陣
    Matrix3d DeltaR = AngleAxisd(theta,n_w).toRotationMatrix();
    Matrix3d R_update = R*DeltaR;
    cout<<"更新后的旋轉矩陣："<<endl<<R_update<<endl;
    
    //利用小量更新四元數
    Quaterniond deltaq(1,w(0)/2,w(1)/2,w(2)/2);
    Quaterniond q_update = q*deltaq;
    //更新之后對四元數進行歸一化
    //q_update = q_update.normalized(); //1
    q_update.normalize();
    cout<<"更新后的四元數:"<<endl<<q_update.coeffs().transpose()<<endl;

    cout<<"兩種方法得到的結果之差："<<endl;    
    cout<<q_update.toRotationMatrix()-R_update<<endl;

    return 0;
}

生成結果如圖所示：

可以看出最后結果的差值在小數點后六位，因此兩種方法效果類似。

3.3 其他導數推導

使用右乘\(so(3)\)，推導一下導數：

\[\frac{d R^{-1}p}{d R}\\ \frac{d \ln(R_1R_2^{-1})^{\wedge}}{dR_2} \]

答：推導過程如下：

\[\begin{aligned} \frac{\partial R^{-1}p}{\partial R}&=\lim_{\phi\rarr 0} \frac{{(R\exp(\phi)^{\wedge})}^{-1}p-R^{-1} p }{\phi} \\&=\lim_{\phi \rarr 0}\frac{{(\exp(\phi^{\wedge})^{-1}R^{-1}}p-R^{-1} p }{\phi} \\&=\lim_{\phi \rarr 0}\frac{(\exp(\phi^{\wedge})^{-1}-I)R^{-1}p}{\phi} \\&=\lim_{\phi \rarr 0}\frac{((I+{\phi^\wedge})^{-1}-I)R^{-1}p}{\phi} \\&=\lim_{\phi \rarr 0}\frac{((I-{\phi^\wedge})-I)R^{-1}p}{\phi} \\&=\lim_{\phi \rarr 0}\frac{-{\phi^\wedge}R^{-1}p}{\phi} \\&=\lim_{\phi \rarr 0}\frac{(R^{-1}p)^{\wedge}\phi}{\phi} \\&=(R^{-1}p)^{\wedge} \end{aligned} \]

用到的性質如下：

一：\(R\)、\(\exp(\phi^{\wedge})\)和\(R\exp(\phi^{\wedge})\)都代表旋轉矩陣，旋轉矩陣的逆與轉置相等，這里所有的逆都相當於轉置，也就是用的矩陣轉置的性質\((R\exp(\phi^{\wedge}))^{-1} = exp(\phi^{\wedge})^{-1}R^{-1}\)(來自網絡)
二：泰勒展開：\(\exp{\varphi^{\wedge}}\approx I+\varphi^{\wedge}\)
三：反對稱矩陣:\((\varphi^{\wedge})^T = -\varphi^{\wedge}\)
四：《視覺-SLAM十四講》P42,概括為如下公式：\(a\times b=a^\wedge b=-b\times a=-b\times a\)。

\[\begin{aligned} \frac{d \ln(R_1R_2^{-1})^{\vee}}{dR_2}&= \lim_{\varphi \rarr 0}\frac{\ln (R_1(R_2\exp{(\phi^{\wedge})})^{-1})^{\vee}-\ln(R_1R_2^{-1})^{\vee}}{\varphi} \\&= \lim_{\varphi \rarr 0}\frac{\ln(R_1(\exp{\phi^{\wedge})}^{-1}R_2^{-1})^{\vee}-\ln(R_1R_2^{-1})^{\vee}}{\varphi} \\&= \lim_{\varphi \rarr 0}\frac{\ln(R_1R_2^{-1}R_2(\exp{\phi^{\wedge})}^{-1}R_2^{-1})^{\vee}-\ln(R_1R_2^{-1})^{\vee}}{\varphi} \\&= \lim_{\varphi \rarr 0}\frac{\ln(R_1R_2^{-1}R_2(\exp{\phi^{\wedge})}^{-1}R_2^{T})^{\vee}-\ln(R_1R_2^{-1})^{\vee}}{\varphi} \\&= \lim_{\varphi \rarr 0}\frac{\ln(R_1R_2^{-1}(\exp{(R_2\phi)^{\wedge})}^{-1})^{\vee}-\ln(R_1R_2^{-1})^{\vee}}{\varphi} \\&= \lim_{\varphi \rarr 0}\frac{\ln(R_1R_2^{-1})^{\vee}+J_r^{-1}(\ln(R_1R_2^{-1})^{\vee})(-R_2\varphi)-\ln(R_1R_2^{-1})^{\vee}}{\varphi} \\&= -J_r^{-1}(\ln(R_1R_2^{-1})^{\vee})R_2 \end{aligned} \]

重要的幾個點

其中第三行第四行用到了伴隨矩陣的性質（在SLAM第四講中有學）

第四行到第五行使用了如下性質：

\[\ln(R\exp(\varphi^{\wedge}))^{\vee} = \ln R^{\vee}+J^{-1}_r(\ln R^{\vee}) \]