一、前言
視覺里程計與傳統的里程計不同，不使用碼盤等設備，只利用攝像頭拍攝的連續圖像幀就可以計算里程，非常方便，因而用途廣泛。本文介紹SLAM中的視覺里程計的設計與實現。
有了前面的基礎，我們就可以著手做一個簡單的視覺里程計（Visual Odometry，VO）。VO作為SLAM系統的前端，負責接收圖像序列，通過特征匹配等方法估計相機在相鄰幀間的運動，從而獲得累積的里程信息。
二、VO系統框架
首先，讓我們來看看VO應該由哪些模塊組成。下圖為VO系統框架圖。
- Frame代表每一幀圖像，存儲著拍攝該幀圖像時的相機位姿、圖像的彩色圖和深度圖、以及是否為關鍵幀。
- Camera代表相機模型，與實際拍攝時的相機對應，只包含內參數，不包含外參數，因此在整個VO過程中只需要一個Camera對象。
- Map代表一個局部地圖，既包含關鍵幀又包含路標點。關鍵幀和路標點會根據適當的規則添加進地圖中。但需要注意，這里的地圖是局部地圖而不是全局地圖，只包含了當前位置附近的路標點，距離更遠的路標點會被刪除。
- MapPoint代表路標點，又稱為地圖點。這些點都是通過特征提取篩選出來的點，因而包含了特征描述符。由于同一個特征點會被多個幀觀測到，因此也包含了這些能觀測到該路標點的幀的指針。同時，為了控制地圖規模，記錄了路標點匹配成功的次數和被觀測到的次數，兩者比值一旦過小就丟棄該路標點（因為該點出現次數很多但對VO沒有幫助）。
有了這幾個模塊，一個簡單的VO就可以搭建起來了。
三、VO算法流程圖
接下來我們詳細介紹VO算法的流程。以最簡單的RGBD-VO為例，避免復雜的初始化過程。
首先，程序啟動后等待第一幀抵達，執行初始化操作。這里的初始化只需要將第一幀設置為關鍵幀，同時把該幀中觀測到的所有路標點添加進地圖。
后續幀抵達后，提取關鍵點，計算描述子，與地圖中的路標點匹配。與地圖中的路標點匹配是為了提高匹配的成功率，如果只與上一幀或上一關鍵幀匹配的話很容易導致匹配結果太少。
匹配成功后，執行PnP位姿估計。這里使用的是RANSAC PnP加上非線性優化的方式估計相機位姿，使用g2o構造圖優化問題。
接下來判斷PnP位姿估計是否成功。如果內點個數過少或者估計出的運動距離過大，則認為位姿估計失敗。如果成功則優化地圖點并決定是否添加當前幀為關鍵幀，如果失敗則結束VO。其中，優化地圖點是一個刪除距離較遠或無效點的過程，通過判斷路標點匹配成功的次數與被觀測到的次數之間的比值是否低于某閾值，從而決定是否刪除這個點。總之，優化地圖點的目的是使地圖規模不至于太大且只包含與當前幀最接近的局部環境。
整個流程如下圖所示。
四、運行效果和存在的問題
使用TUM數據集rgbd_dataset_freiburg1_desk測試效果如下。
左圖為當前視頻幀及局部地圖中路標點的投影，右圖為世界坐標系和當前幀的相機坐標系位置。從后三張圖可以發現，特征點位置出現了一些明顯的偏差，這可能是由于相機位姿估計不準確造成的。說明這個簡單的VO在運動過快的情況下產生了漂移。
在完整的SLAM中，既有前端的VO，又有后端優化和回環檢測，從而可以在一定程度上降低VO的漂移問題。我們將在后續文章中一一介紹。
本文的VO例程完整代碼地址：https://github.com/jingedawang/VO
該代碼依賴于OpenCV 3.2.0，opencv-viz，Sophus，g2o，Eigen等庫，請特別檢查viz模塊是否正確安裝。另外，如果Sophus庫找不到，請手動編寫FindSophus.cmake。
五、參考資料
《視覺SLAM十四講》第9講 實踐：設計前端 高翔
