Visual-Inertial Monocular SLAM with Map Reuse

重用地圖的單目視覺慣導SLAM系統

摘要

近些年來有很多優秀的視覺慣導融合的里程計系統，計算高精度和魯棒性的傳感器的增量運動。但是這些系統都是沒有閉環的，所以導致系統即使回到觀測過的地方還是會有累計誤差。本文作者提出了一個新穎的基于緊耦合的帶有閉環檢測的視覺慣導SLAM系統，他可以在已經建圖的地方重用地圖達到0漂移的定位精度。這個系統可以用在所有的相機上，這里主要介紹存在尺度不確定性的單目相機。本文也提出了一個新穎的IMU初始化的方法可以在短時間內計算很高精度的尺度，重力方向，速度，加速度計和陀螺儀的偏置。在11個序列的飛行數據集上進行了測試，尺度誤差達到1%（厘米級）精度。

本文未開源，但是有大佬復現的代碼：https://github.com/jingpang/LearnVIORB

預備知識

系統的輸入是IMU的測量和單目相機幀，利用針孔模型利用投影模型可以把相機坐標系下的3D點投影到2D的圖像平面：

需要注意的是這個投影方程并沒有考慮相機的畸變，當我們檢測到了圖像中的特征點先要對他們去畸變。

公式解釋：IMU的測量全部都在B系下，公式1IMU的角度測量減去偏置得到角速度真值，再和時間相乘得到時間間隔(K到K+1幀的)內的角度變化量，再加一個Exp相當于變換到了李群的空間，和上一幀的位姿相乘得到當前幀在W系下的位姿。公式2利用當前加速度的測量量減去當前幀的偏置和時間相乘得到速度變換量再轉換到世界坐標系下，再加上重力加速度引起的變化和上一幀的速度相加得到當前幀的速度。公式3當前幀的位置其實就是勻加速運動的位移方程。

視覺慣導的ORB-SLAM

這篇論文是基于ORB-SLAM做的，對于ORB-SLAM的介紹，可以參考解析ORB-SLAM3的文章。下邊主要介紹加上IMU后對ORB-SLAM框架中Tracking、Local Mapping和Loop Closing的影響。

Tracking

主要的變化是添加了相機頻率的IMU位置、速度和偏置的估計。這樣我們就可以得到比恒速模型準的多的機器人當前幀的初始狀態。一旦預測到了機器人的位置，局部地圖中的地圖點就投影到當前幀和當前幀的特征點匹配。然后優化當前幀中特征匹配的重投影誤差和IMU的誤差。這種優化是不同的，取決于地圖是否被局部建圖或閉環線程更新，如圖2所示。

當Tracking在地圖更新后執行的時候，IMU的誤差連接了當前幀j和上一幀i：

公式解釋：這里就是重投影誤差的最小二乘的形式，利用IMU當前幀的位姿得到世界坐標系下地圖點在B系下的表示，然后利用外參可以得到地圖點在C系下的表示，最后利用針孔模型得到該地圖點在當前幀像素坐標系下的表示，得到和匹配點的誤差。

B.局部建圖

在一個新的關鍵幀插入的時候局部建圖線程進行Local BA，他優化最新的N幀(Local window)和所有被觀測到的地圖點，其他共視圖中維護的不在局部滑窗中的關鍵幀雖然參與優化但是不改變位姿。固定窗口中包含了N+1個和滑窗相連的最新的共視關鍵幀，他們可以約束IMU的狀態。

上圖表示出了ORB-SLAM中的BA和ORB-SLAM-VI中BA的區別，這里的優化函數是IMU的誤差和重投影誤差的和，和純視覺的方法相比每個關鍵幀多了九個狀態量（速度和偏置）需要優化。所以要選擇一個合適的窗口的大小來實時的優化。

局部建圖線程還負責關鍵幀的管理。原始的ORB-SLAM策略中會丟棄冗余的關鍵幀，這樣就不會在已經建圖很完善的地方增加很多的關鍵幀。但是這種方法與IMU約束連續關鍵幀的機制是不匹配的，兩個連續關鍵幀的時間相差越長，IMU提供的信息就越弱。因此如果局部窗口中的關鍵幀沒有差過0.5s的我們允許局部建圖線程丟棄冗余關鍵幀。為了能在閉環檢測或者其他時間優化地圖，本文不允許兩個關鍵幀相差3s。如果關閉具有IMU約束的完整BA，只需要約束局部窗口中的時間偏移即可。

C.閉環檢測

閉環的主要作用是機器人又回到原來到過的地方的時候降低里程計帶來的累計誤差。場景充實別模塊匹配最近的關鍵幀和原來的一個關鍵幀。利用兩幀的詞袋匹配可以得到剛體變換，然后執行優化的過程來降低里程計帶來的累計誤差。這種優化在大場景中很耗時，所以執行位姿圖優化，==這樣就可以忽略結構==，也可以很好的收斂。本文可以執行六自由度的位姿圖優化，因為尺度是可觀的。這個系統忽略IMU信息不優化速度和偏置。==速度利用把關聯的關鍵幀旋轉到正確的位姿上來修正。==同時并行的執行一個全局的BA來優化所有的狀態，包括速度和偏置。

IMU初始化

根據由運行一段時間的單目SLAM算法得到的若干關鍵幀來計算視覺-慣導的全局BA的尺度、重力方向、速度和IMU偏置的初始估計。運行一段時間（幾秒鐘）的單目SLAM算法，假設傳感器運動后導致所有的狀態都可觀。

初始化問題可以分成小的子問題

·  陀螺儀偏置

·  尺度和重力估值（假設沒加速度偏置）

·  加速度偏置估計，尺度和重力方向細化

·  速度估計

陀螺儀偏置估計

可以利用兩個連續關鍵幀的orientation估計陀螺儀的偏置，假設偏差的變化忽略不計（bias是緩慢變化的量），可以直接對所有的連續關鍵幀優化常數的bg（最小化陀螺儀的積分和ORB-SLAM計算出來的orientation）：

尺度和重力估計（沒有加速度的偏置）

得到陀螺儀的偏置以后，可以利用預積分得到速度，位置，正確的旋轉加速度的測量來補償陀螺儀的bias。ORB-SLAM計算的相機軌跡具有尺度不確定性，所以在從相機坐標系到IMU的機體坐標系轉的時候需要加一個尺度因子：(imu的所有的狀態都是在B系下測量得到的)

C.加速度偏置估計，尺度和重力細化

同樣可以整理為公式12的形式：

D.速度估計

在公式12和19中都是考慮三個連續的關鍵幀，所以線性的系統中就沒有3N個額外的未知的速度，現在所有的速度都可以用公式18在重力、尺度和偏置已知的情況下來計算。為了計算最近幾幀的速度，這里用公式3。

E.重定位后的bias重新初始化

當系統重新初始化的時候，利用公式9重新初始化陀螺儀偏置，利用公式19重新初始化加速度計偏置，尺度和重力是已知的。利用二十個關鍵幀來初始化這兩個偏置。