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重新初始化加速度計偏置，尺度和重力是已知的。利用二十個關鍵幀來初始化這兩個偏置。

備註：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個超乾貨的3D視覺學習社群