視覺−慣性里程計(VIO)融合SLAM方案

第一章

1. 視覺與IMU融合後的優勢？

  傳感器融合是一個趨勢，也或者說是一個妥協的結果。主要的原因還是由於單一的傳感器不能適用所有的場景，所以寄希望於通過多個傳感器的融合達到理想的定位效果。目前主流的傳感器融合方案包括：視覺+IMU、視覺+激光雷達、IMU+差分GPS+激光雷達。

視覺+IMU融合方案

  視覺傳感器：在大多數紋理豐富的場景中效果很好，但是如果遇到玻璃，白牆等特徵較少的場景，基本上無法工作；快速運動時定位跟蹤容易丟失；單目視覺無法測量尺度。但是由於視覺不產生漂移，可以直接測量旋轉平移。
  IMU：由於零偏和噪聲的存在，導致其長時間使用有非常大的累積誤差；低精度IMU長時間積分位姿容易發散，而高精度的價格普遍較貴；同時IMU 有輸出頻率高、能輸出6DoF測量信息等優點，在短時間內，其相對位移數據有很高的精度。

  因此視覺和 IMU 定位方案存在一定互補性質；所以當視覺傳感器在短時間內快速運動失效時，融合IMU數據，能夠爲視覺提供短時的精準定位，同時利用視覺定位信息來估計IMU的零偏，減少IMU由零偏導致的發散和累積誤差。通過二者的融合，可以解決視覺位姿估計輸出頻率低的問題，同時位姿估計精度有一定的提高，整個系統也更加魯棒。

2. 常見的視覺和IMU融合方案？工業界的應用例子

  視覺和IMU融合可以分爲基於濾波和基於優化兩種方法。按照是否把圖像特徵信息加入狀態向量來進行分類，可以分爲松耦合和緊耦合兩大類。

  松耦合：將視覺傳感器和 IMU分別計算得到的位姿直接進行融合，融合過程對二者本身不產生影響，作爲後處理方式輸出，一般通過 EKF 進行融合。
  緊耦合：將視覺傳感器和 IMU 的狀態通過一個優化濾波器合併在一起，緊耦合需要把圖像特徵加入到特徵向量中，共同構建運動方程和觀測方程，然後進行狀態估計，最終得到位姿信息的過程，其融合過程本身會影響視覺和 IMU 中的參數（如 IMU 的零偏和視覺的尺度）。

  基於濾波的融合方案：

• 松耦合：蘇黎世聯邦理工學院(eth)的 SSF(融合IMU和單個視覺傳感器)和 MSF(融合IMU和多個位置傳感器)；
• 緊耦合：比較經典的算法是MSCKF，ROVIO；

  基於優化的融合方案：

• 松耦合： 基於松耦合優化的工作不多。 Gabe Sibley在IROS2016的一篇文章《Inertial Aided Dense & Semi-Dense Methods for Robust Direct Visual Odometry》提到了這個方法。簡單來說就是把VO計算的位姿變換添加到IMU的優化框架裏面。
• 緊耦合：OKVIS（多目+IMU）、 VINS-Mono、VINS-Fusion、VI-ORB；

  工業界應用舉例：
  VIO目前實現比較好的是VINS、OKVIS和MSCKF_VIO。前兩個是基於非線性優化的方案而且框架比較相似，後者是基於濾波優化的方案，也是Google Tango上使用的方法。同時，VIO在工業上的應用還有蘋果的ARKit和谷歌的ARCore。

3. 學術界VIO研究進展如何？將學習的方法用到VIO的例子

  傳統VIO：

1. PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features（Sensors-2018）
     爲了解決相機軌跡估計問題，建立基於慣性測量和視覺觀測的三維結構圖，提出了點-線視覺慣性里程計（PL-VIO），一種利用點和線特徵的緊密耦合的單目視覺慣性里程計系統。爲了實現慣性測量單元（IMU）和視覺傳感器信息的緊密有效融合，在滑動窗口優化框架下，通過最小化代價函數，將預先集成的IMU誤差項與點線重投影誤差項相結合，對狀態進行優化。
2. Visual-Inertial Odometry with Point and Line Features（IROS-2019）
     提出了一種使用點和線的緊密耦合的單目視覺慣性導航系統（VINS），並採用退化運動分析進行三維線三角測量。基於圖像的線段測量，提出了 兩種基於滑動窗口的三維線三角測量算法。

  深度學習+VIO

1. VINet: Visual-Inertial Odometry as a Sequence-to-Sequence Learning Problem（AAAI-2017）
     VINet是首次使用端到端可訓練的深度神經網絡架構嘗試解決機器人領域視覺慣性里程計的問題，目前所披露的實驗表現出了一定的實用價值。與傳統方法相比具有許多優勢。它消除了對攝像機和IMU進行繁瑣的手動同步的需要，也消除了對IMU和攝像機之間進行手動校準的需要。另一個優勢是，模型可以自然合併特定領域的信息，大大減輕了漂移。
2. DeepVIO: Self-supervised Deep Learning of Monocular Visual Inertial Odometry using 3D Geometric Constraints（IROS-2019）
     第一次使用自監督深度學習網絡（DeepVIO），端到端的單目VIO（從雙目中獲取監督信息）。DeepVIO通過直接融合2D光流特徵（OFF）和慣性測量單元（IMU）數據提供絕對軌跡估計。
3. Unsupervised Depth Completion from Visual Inertial Odometry（ICRA-2019）
     採用了無監督的KITTI深度作爲基準深度數據集，首次提出了視覺慣性+深度數據集的融合方式，進一步增強視覺和慣性傳感器的互補優勢。
4. Selective Sensor Fusion for Neural Visual-Inertial Odometry（CVPR-2019）
     針對目前視覺慣性里程計（VIO）的深度學習方法很少結合穩健的融合策略去處理不完美的輸入數據。提出了一種新的單目端到端VIO傳感器融合框架，該框架融合了單目圖像和慣性測量數據，以估計軌跡，同時提高了對真實問題的魯棒性，如數據丟失和損壞或傳感器同步不良等問題。還提出了兩種基於不同掩蔽策略的融合模式：確定軟融合和隨機硬融合，且實驗結果證明在數據被破壞的情況下，該融合策略與直接融合相比，具有更好的性能。

4. 使用右乘$so(3)$，推導下面導數

4.1 解：
  因爲李代數$so(3)$與$SO(3)$的關係由指數映射給定：
$R=exp(\phi^{\bigwedge})$
  則旋轉微小量： $\Delta R=exp(\varphi^{\bigwedge})$
  在優化帶有旋轉的函數時，通常計算一個微小增量 $\phi \in so(3)$,然後使用右乘李代數進行更新：$R^{'}=Rexp(\phi^{\bigwedge})$

$\begin{aligned} \frac{d(R^{-1}P)}{dP} &= \lim_{\varphi \to 0} \frac{(R exp(\varphi^{\bigwedge}))^{-1}P-R^{-1}P}{\varphi}\\ &= \lim_{\varphi \to 0} \frac{exp(\varphi^{\bigwedge})^{-1}R ^{-1}P-R^{-1}P}{\varphi}\\ &= \lim_{\varphi \to 0} \frac{exp(\varphi^{\bigwedge})^{T}R ^{-1}P-R^{-1}P}{\varphi}\\ &= \lim_{\varphi \to 0} \frac{(I+\varphi^{\bigwedge})^{T}R ^{-1}P-R^{-1}P}{\varphi}\\ &= \lim_{\varphi \to 0} \frac{(\varphi^{\bigwedge})^{T}R ^{-1}P}{\varphi}\\ &= \lim_{\varphi \to 0} \frac{-\varphi^{\bigwedge}R ^{-1}P}{\varphi}\\ &= \lim_{\varphi \to 0} \frac{R ^{-1}P^{\bigwedge}\varphi}{\varphi}\\ &=R ^{-1}P^{\bigwedge} \end{aligned}$

上式中用到的知識：

• 由於$\varphi^{\bigwedge}$是反對稱函數，因此$(\varphi^{\bigwedge})^{T}=-\varphi^{\bigwedge}$；
• 由於旋轉矩陣$R$是正交矩陣，即$R^{-1}=R^{T}$,對應的李代數$[exp(\varphi^{\bigwedge})]^{-1}=[exp(\varphi^{\bigwedge})]^{T}$；
• 反對稱矩陣與向量的相乘相當於兩個向量的叉乘（前後交換相乘順序需變號），例如：$a\times b=a^{\bigwedge}b=-b\times a=-b^{\bigwedge}a$。必須保證一個反對稱矩陣與一個向量，這裏$R^{-1}P$表示向量（爲一個整體）；

4.2 解：
$\begin{aligned} \frac{dln(R_1R_2^{-1})}{dR_2} &= \lim_{\varphi \to 0}\frac{ln(R_1[R_2exp(\varphi^{\bigwedge})]^{-1})-ln(R_1R_2^{-1})}{\varphi}\\ &= \lim_{\varphi \to 0}\frac{ln(R_1[exp((R_2\varphi)^{\bigwedge})R_2]^{-1})-ln(R_1R_2^{-1})}{\varphi}\\ &= \lim_{\varphi \to 0}\frac{ln(R_1R_2^{-1}[exp((R_2\varphi)^{\bigwedge})]^{-1})-ln(R_1R_2^{-1})}{\varphi}\\ &= \lim_{\varphi \to 0}\frac{ln(R_1R_2^{-1}[exp((R_2\varphi)^{\bigwedge})]^{T})-ln(R_1R_2^{-1})}{\varphi}\\ &= \lim_{\varphi \to 0}\frac{ln(R_1R_2^{-1}exp(((R_2\varphi)^{\bigwedge})^{T}))-ln(R_1R_2^{-1})}{\varphi}\\ &= \lim_{\varphi \to 0}\frac{ln(R_1R_2^{-1}exp(-(R_2\varphi)^{\bigwedge}))-ln(R_1R_2^{-1})}{\varphi}\\ &= \lim_{\varphi \to 0}\frac{ln(R_1R_2^{-1}exp((-R_2\varphi)^{\bigwedge}))-ln(R_1R_2^{-1})}{\varphi}\\ &= \lim_{\varphi \to 0}\frac{ln(R_1R_2^{-1})+J_r^{-1}(ln(R_1R_2^{-1}))(-R_2\varphi)-ln(R_1R_2^{-1})}{\varphi}\\ &= \lim_{\varphi \to 0}\frac{J_r^{-1}(ln(R_1R_2^{-1}))(-R_2\varphi)}{\varphi}\\ &=-J_r^{-1}(ln(R_1R_2^{-1}))R_2 \end{aligned}$

上面用到的知識：

• $SO(3)$的伴隨性質：$Rexp(\phi^{\bigwedge})R^{T}=exp((R\phi)^{\bigwedge})$，第一行到第二行用到該性質；
• 由於 $(R_2\varphi)^{\bigwedge}$ 是反對稱矩陣(方陣)，因此$((R_2\varphi)^{\bigwedge})^{T}=-(R_2\varphi)^{\bigwedge}=(-R_2\varphi)^{\bigwedge}$ ；
• 那麼第四行到第七行可以推導$[exp((R_2\varphi)^{\bigwedge})]^{T}=exp(((R_2\varphi)^{\bigwedge})^{T})=exp((-R_2\varphi)^{\bigwedge})$；
• 第七到第十行可以利用BCH近似得到：$ln(Rexp(\phi ^{\bigwedge}))=ln(R)+J^{-1}_r(ln(R))\phi$；