一個基於深度學習迴環檢測模組的簡單雙目 SLAM 系統
阿新 • • 發佈：2020-04-05
轉載請註明出處，謝謝
原創作者：Mingrui
原創連結：https://www.cnblogs.com/MingruiYu/p/12634631.html
***
# 寫在前面
最近在搞本科畢設，關於基於深度學習的 SLAM 迴環檢測方法。期間，為了鍛鍊自己的工程實現能力，(也為了增添畢設的工作量，顯得不那麼水），我自己寫了一個簡單的雙目 SLAM 系統，其中嵌入了一種基於深度學習的輕量級迴環檢測模組 (https://github.com/rpng/calc)，目前這種方法是我找到的最輕量級且效果不錯的迴環檢測方法，當然目前只是暫時使用這種方法，如果我能在畢設期間優化出更好的方法的話，我就把它換掉（大概是不可能的了）。
選擇雙目是因為雙目比較簡單（不像單目需要很多工作去初始化、估計深度、減小尺度誤差等），整個系統結構比較清晰和簡單，沒有做很多細節上的優化，所以最終效果沒有特別好。但我個人認為，這樣的結構比較適合 SLAM 的初學者去熟悉一個完整的 SLAM 系統。畢竟如果直接研究 ORB-SLAM2 這種一萬多行程式碼、其中嵌入了各種各樣 trick 的複雜系統，對於 SLAM 初學者是很不友好的（心酸淚）。這也是我厚著臉皮開源這個弱雞 SLAM 系統的原因。
本文會對這個系統的架構以及配置安裝方法進行簡單的介紹。本專案的 GitHub 地址：(https://github.com/Mingrui-Yu/A-Simple-Stereo-SLAM-System-with-Deep-Loop-Closing)。之後若有更新還請參考 GitHub 中的 README。
# 相關依賴
## 參考
### 高翔《視覺 SLAM 十四講：第二版》第十三章雙目視覺里程計
(https://github.com/gaoxiang12/slambook2/tree/master/ch13)。本系統借鑑了該視覺里程計的基礎架構，前端和後端基本使用了相同的方法。
### ORB-SLAM2
(https://github.com/raulmur/ORB_SLAM2)。本系統使用了修改後的 ORB-SLAM2 中提取 ORB 特徵部分 (ORBextractor) 的部分程式碼。
### Lightweight Unsupervised Deep Loop Closure
(https://github.com/rpng/calc)。本系統使用了該方法作為迴環檢測的方法，並將修改後的程式碼嵌入了本系統。
## 庫
### Caffe
使用 CPU 版本即可。其下載和安裝可參考我總結的教程：(https://github.com/Mingrui-Yu/Tutorials/blob/master/Ubuntu/caffe.md)。其用於迴環檢測。
### DeepLCD & ORB-SLAM2
我們使用了魔改後的 [DeepLCD](https://github.com/rpng/calc) 庫和魔改後的 [ORB-SLAM2](https://github.com/raulmur/ORB_SLAM2) 中的一點點程式碼。這些部分已經包括在專案中，無需另外安裝。
### 其他
這裡是一些 SLAM 常規使用內容，具體內容課參照 GitHub 專案中的 README。
* C++11
* Boost filesystem
* Google Logging Library (glog)
* OpenCV
* Eigen
* Sophus
* g2o
* Pangolin
可能還有漏掉的（emmm），有問題的話歡迎大家 issue。
# 配置安裝
本專案的 GitHub 地址：(https://github.com/Mingrui-Yu/A-Simple-Stereo-SLAM-System-with-Deep-Loop-Closing)。
完成上述的配置安裝。
clone 本專案：
```
git clone https://github.com/Mingrui-Yu/A-Simple-Stereo-SLAM-System-with-Deep-Loop-Closing.git
```
build 本專案：
```
mkdir build
cd build
cmake ..
make
```
之後，/bin 資料夾中會有執行程式的可執行檔案；/lib 資料夾中會有 libmyslam.so。
# 例程執行
目前因為時間原因（或者僅僅是懶），只寫了雙目 KITTI 的主程式，其位於 /app/run_kitti_stereo。
首先下載 [KITTI 資料集](http://www.cvlibs.net/datasets/kitti/eval_odometry.php), 國內可通過泡泡機器人彙總的百度網盤[連結](https://www.sohu.com/a/219232053_715754)下載。
執行方式:
```
./bin/run_kitti_stereo config/stereo/gray/KITTI00-02.yaml PATH_TO_DATASET_FOLDER/dataset/sequences/00
```
其中 KITTI00-02.yaml 是相應引數的配置檔案（包括相機引數等），其風格參考 ORB-SLAM2 所提供的配置檔案。
另外，在 yaml 配置檔案中，有幾個引數可以控制系統執行時的顯示效果：
* Camera.fps: 控制系統執行的幀率（大體，不會很準）
* LoopClosing.bShowResult: 是否顯示迴環檢測的匹配結果和重投影結果
* Viewer.bShow: 系統執行時，是否實時顯示視訊幀及地圖
軌跡結果展示（挑了效果最好的一次hhh）：
# 系統架構 & 原理簡介
下面簡單介紹一下整個系統的架構和原理。
參考 ORB-SLAM2，整個系統分三個執行緒：前端，後端，迴環閉合。
## 前端
前端通過特徵點 + 光流法進行追蹤。初始化時，對第一幀左圖提取 ORB feature，使用的是 ORB-SLAM2 中的分格及八叉樹提取方法，因為 OpenCV 中的 ORB 特徵提取有嚴重的分佈不均情況。注意，與普通特徵點法（例如 ORB-SLAM2）不同的是，這裡的 ORB feature 提取沒有使用尺度金字塔，僅僅是在原影象上進行提取。同時，也不需要計算 feature 的描述子。這二者是因為，本系統中 feature 的跟蹤不通過特徵匹配，而是基於光流法。
在左圖中提取 ORB feature 後，通過光流法，找到這些 feature 在同幀右圖中的位置。之後，根據一對 feature 在左右目中的位置關係，進行三角測量，得到其對應的 MapPoint 的深度，並在地圖中建立 MapPoint。同是，根據當前幀建立 KeyFrame，送入到後端中。
之後，每一幀的跟蹤中，會先根據恆速模型，得到一個當前幀位姿的估計初值。之後，通過光流法，找到上一幀的特 feature 徵點在當前幀的位置，得到當前幀的 feature 。再根據當前幀的 feature 及 feature 對應的 MapPoints 的位置，通過 g2o 優化得到當前幀的位姿。此處的優化僅優化當前幀位姿，當前幀觀測到的 MapPoints 僅以約束形式參與優化。如果某個 MapPoints 在剛建立不久（兩幀以內）就成為 Outlier 了，那麼該 MapPoint 會被從地圖中刪除。
因此，並不需要在每一幀的跟蹤時都提取 feature ，每次僅需通過光流法將上一幀的 feature 關聯到當前幀。但每一次光流跟蹤都會有一定 feature 丟失，只有當前幀的 feature 少於一個閾值時，會再進行 ORB feature 點的提取，此時，會將之前剩餘的 feature 作為 mask，附近不提取新的 feature 。再通過三角測量得到新的 MapPoint 並插入地圖。同時，此時會建立 KeyFrame，送入後端。
如果某次追蹤到的特徵點特別特別少，則判定為 LOST。目前，系統 LOST 後的 Relocalzation 還沒有完成，等我苟過畢設（畢竟寫這玩意跟我畢設主題沒太大關係）。
## 後端
後端會對一個 active map 進行維護，同時在 active map 中進行優化。前端送入的 KeyFrame，會在後端進行一定的處理，並插入地圖。而 active map 其實是一個滑動視窗，其中含有一定數量的近期的 KFs 及 它們觀測到的 MapPoints，作為 active KFs 和 active MapPoints。當一個新的 KF 送入後端時，後端會將它同時插入 map 和 active KFs，並將其觀測到的 MapPoints 插入 active MapPoints。如果此時 active map 中的 KF 數目超過了限制，會從中刪除一個根據條件選擇的 KF。
之後，會在 active map 中進行一次優化，優化包括 active KFs 和 active MapPoints。其中，如果某個 active MapPoints 第一次被觀測到時的 KF 不在 active map 中，那麼它會被固定，僅作為約束參與優化。優化後被視為 Outlier 的 MapPoints 會被從地圖中刪除。
另外，新的 KF 會被送入迴環閉合執行緒。
## 迴環閉合
這一部分相對比較複雜。迴環閉合執行緒會維護一個 KeyFrame Database，用於迴環檢測。
對於送入迴環閉合執行緒的新的 KF，首先，會對它進行預處理。第一步，對其 feature 進行擴充和處理。上文說過，前端提取的 feature 不是在尺度金字塔上提取的，而在迴環閉合過程中，因為需要特徵匹配，所以需要尺度金字塔來克服尺度變化帶來的影響。在這一步中，會基於尺度金字塔，將每個 feature 擴充成 8（金字塔層數）個 keypoints，即每層的同一位置都視為一個 keypoint。之後，會對 keyponts 進行篩選：去除其中不能視為 FAST 角點的（響應低於閾值），以及超出邊界的（因為需要計算描述子、角度等，keypoints 的位置需要離影象邊界有一定距離）。根據篩選後的 keypoints，計算該 KF 的 ORB 描述子。以上內容是用於特徵匹配的。另外，為了之後的迴環檢測，預處理中 DeepLCD 中的網路會對整幅 KF 提取一個描述向量。新的 KF 經過上述處理後，會帶著計算好的描述子和描述向量，被儲存進 KeyFrame Database 中。
迴環檢測：對於每一個要查詢是否存在迴環幀（Loop KF）的 Current KF，系統會將 Current KF 與 KeyFrame Database 中所有 KF 的描述向量進行比較，求餘弦相似度作為相似分數。因為查詢速度相當快，所以此處簡單的使用了線性查詢。 找到相似分數最高的 Candidate KF，如果該相似分數高於一個閾值，則認為該 Candidate KF 可以送入下一環節：特徵匹配。
特徵匹配：根據 Candidate KF 和 Current KF 的 ORB 描述子進行特徵匹配。因為存在 keypoints 歸屬於相同的 feature，所以根據 keypoints 匹配可能出現重複的 feature 匹配。所以這裡會從 keypoints 之間的匹配再上升至 feature 之間的匹配，從而去除重複匹配。同時，根據匹配的距離，對匹配對進行篩選，從中選出距離小的有效的匹配對。如果有效匹配對的數量達到一定閾值，則可以送入下一步。
計算當前幀正確位姿：根據 Candidate KF 的 feature 對應的 MapPoints 與 Current KF 的 feature 之間的匹配，首先進行 PnP 求解，這裡使用了 OpenCV 的 solvePnPRansac()。之後，會再進行一次 g2o 優化，同樣，只優化 Current KF 位姿，MapPoints 僅作為約束參與優化。如果優化的 inlier 數目超過一定閾值，則最終正式將 Candidate KF 確認為 Loop KF，並送入接下來的迴環校正模組。
迴環校正：迴環矯正分兩個部分。首先是迴環融合，有了 Current KF 的正確位姿，就可以對其位姿進行調整。同時，active map 中的 active KFs 會根據它們之前與 Current KF 的相對位姿，同步進行調整（固定相對位姿），同理，active MapPoints 也會根據它們與 active KF 的相對位置，進行相應的調整（固定相對位置）。active map 調整完畢後，會進行一次位姿圖優化，來對之前 KFs 的位姿進行全域性調整。位姿圖優化的邊，一種是前端跟蹤時，KF 與相鄰 KF 之間的相對位姿，另一種就是迴環邊。對 KF 的位姿進行優化後，相應的會將所有的 MapPoints 根據與觀測 KF 之間的相對位置進行位置校正。
***
作為一個 SLAM 的初學者的初級工作，這個專案中可能會存在很多問題或錯誤。如果大家發現了任何問題，歡迎來 issue 一下。非常感謝大家的指正和
# 系統架構 & 原理簡介
下面簡單介紹一下整個系統的架構和原理。
參考 ORB-SLAM2，整個系統分三個執行緒：前端，後端，迴環閉合。
## 前端
前端通過特徵點 + 光流法進行追蹤。初始化時，對第一幀左圖提取 ORB feature，使用的是 ORB-SLAM2 中的分格及八叉樹提取方法，因為 OpenCV 中的 ORB 特徵提取有嚴重的分佈不均情況。注意，與普通特徵點法（例如 ORB-SLAM2）不同的是，這裡的 ORB feature 提取沒有使用尺度金字塔，僅僅是在原影象上進行提取。同時，也不需要計算 feature 的描述子。這二者是因為，本系統中 feature 的跟蹤不通過特徵匹配，而是基於光流法。
在左圖中提取 ORB feature 後，通過光流法，找到這些 feature 在同幀右圖中的位置。之後，根據一對 feature 在左右目中的位置關係，進行三角測量，得到其對應的 MapPoint 的深度，並在地圖中建立 MapPoint。同是，根據當前幀建立 KeyFrame，送入到後端中。
之後，每一幀的跟蹤中，會先根據恆速模型，得到一個當前幀位姿的估計初值。之後，通過光流法，找到上一幀的特 feature 徵點在當前幀的位置，得到當前幀的 feature 。再根據當前幀的 feature 及 feature 對應的 MapPoints 的位置，通過 g2o 優化得到當前幀的位姿。此處的優化僅優化當前幀位姿，當前幀觀測到的 MapPoints 僅以約束形式參與優化。如果某個 MapPoints 在剛建立不久（兩幀以內）就成為 Outlier 了，那麼該 MapPoint 會被從地圖中刪除。
因此，並不需要在每一幀的跟蹤時都提取 feature ，每次僅需通過光流法將上一幀的 feature 關聯到當前幀。但每一次光流跟蹤都會有一定 feature 丟失，只有當前幀的 feature 少於一個閾值時，會再進行 ORB feature 點的提取，此時，會將之前剩餘的 feature 作為 mask，附近不提取新的 feature 。再通過三角測量得到新的 MapPoint 並插入地圖。同時，此時會建立 KeyFrame，送入後端。
如果某次追蹤到的特徵點特別特別少，則判定為 LOST。目前，系統 LOST 後的 Relocalzation 還沒有完成，等我苟過畢設（畢竟寫這玩意跟我畢設主題沒太大關係）。
## 後端
後端會對一個 active map 進行維護，同時在 active map 中進行優化。前端送入的 KeyFrame，會在後端進行一定的處理，並插入地圖。而 active map 其實是一個滑動視窗，其中含有一定數量的近期的 KFs 及 它們觀測到的 MapPoints，作為 active KFs 和 active MapPoints。當一個新的 KF 送入後端時，後端會將它同時插入 map 和 active KFs，並將其觀測到的 MapPoints 插入 active MapPoints。如果此時 active map 中的 KF 數目超過了限制，會從中刪除一個根據條件選擇的 KF。
之後，會在 active map 中進行一次優化，優化包括 active KFs 和 active MapPoints。其中，如果某個 active MapPoints 第一次被觀測到時的 KF 不在 active map 中，那麼它會被固定，僅作為約束參與優化。優化後被視為 Outlier 的 MapPoints 會被從地圖中刪除。
另外，新的 KF 會被送入迴環閉合執行緒。
## 迴環閉合
這一部分相對比較複雜。迴環閉合執行緒會維護一個 KeyFrame Database，用於迴環檢測。
對於送入迴環閉合執行緒的新的 KF，首先，會對它進行預處理。第一步，對其 feature 進行擴充和處理。上文說過，前端提取的 feature 不是在尺度金字塔上提取的，而在迴環閉合過程中，因為需要特徵匹配，所以需要尺度金字塔來克服尺度變化帶來的影響。在這一步中，會基於尺度金字塔，將每個 feature 擴充成 8（金字塔層數）個 keypoints，即每層的同一位置都視為一個 keypoint。之後，會對 keyponts 進行篩選：去除其中不能視為 FAST 角點的（響應低於閾值），以及超出邊界的（因為需要計算描述子、角度等，keypoints 的位置需要離影象邊界有一定距離）。根據篩選後的 keypoints，計算該 KF 的 ORB 描述子。以上內容是用於特徵匹配的。另外，為了之後的迴環檢測，預處理中 DeepLCD 中的網路會對整幅 KF 提取一個描述向量。新的 KF 經過上述處理後，會帶著計算好的描述子和描述向量，被儲存進 KeyFrame Database 中。
迴環檢測：對於每一個要查詢是否存在迴環幀（Loop KF）的 Current KF，系統會將 Current KF 與 KeyFrame Database 中所有 KF 的描述向量進行比較，求餘弦相似度作為相似分數。因為查詢速度相當快，所以此處簡單的使用了線性查詢。 找到相似分數最高的 Candidate KF，如果該相似分數高於一個閾值，則認為該 Candidate KF 可以送入下一環節：特徵匹配。
特徵匹配：根據 Candidate KF 和 Current KF 的 ORB 描述子進行特徵匹配。因為存在 keypoints 歸屬於相同的 feature，所以根據 keypoints 匹配可能出現重複的 feature 匹配。所以這裡會從 keypoints 之間的匹配再上升至 feature 之間的匹配，從而去除重複匹配。同時，根據匹配的距離，對匹配對進行篩選，從中選出距離小的有效的匹配對。如果有效匹配對的數量達到一定閾值，則可以送入下一步。
計算當前幀正確位姿：根據 Candidate KF 的 feature 對應的 MapPoints 與 Current KF 的 feature 之間的匹配，首先進行 PnP 求解，這裡使用了 OpenCV 的 solvePnPRansac()。之後，會再進行一次 g2o 優化，同樣，只優化 Current KF 位姿，MapPoints 僅作為約束參與優化。如果優化的 inlier 數目超過一定閾值，則最終正式將 Candidate KF 確認為 Loop KF，並送入接下來的迴環校正模組。
迴環校正：迴環矯正分兩個部分。首先是迴環融合，有了 Current KF 的正確位姿，就可以對其位姿進行調整。同時，active map 中的 active KFs 會根據它們之前與 Current KF 的相對位姿，同步進行調整（固定相對位姿），同理，active MapPoints 也會根據它們與 active KF 的相對位置，進行相應的調整（固定相對位置）。active map 調整完畢後，會進行一次位姿圖優化，來對之前 KFs 的位姿進行全域性調整。位姿圖優化的邊，一種是前端跟蹤時，KF 與相鄰 KF 之間的相對位姿，另一種就是迴環邊。對 KF 的位姿進行優化後，相應的會將所有的 MapPoints 根據與觀測 KF 之間的相對位置進行位置校正。
***
作為一個 SLAM 的初學者的初級工作，這個專案中可能會存在很多問題或錯誤。如果大家發現了任何問題，歡迎來 issue 一下。非常感謝大家的指正和