歡迎大家再次回到泡泡機器人的課堂。不久前，ORB-SLAM2的作者在原純視覺SLAM的基礎上，針對ORB-SLAM的一些固有問題，提出了一種全新的結合IMU的方法[2]。就在幾天前，王京大神開源了基於該算法的一種實現[1]。本文中，王京大神將為大家詳細講解該實現中的關鍵技術，希望本文可以幫助大家剛好地閱讀和理解相關代碼。
1. 前言
這份學習代碼[1]參考Visual Inertial ORB-SLAM的論文[2]，從ORB-SLAM2[3,4]的基本代碼修改而來。
目前開源的Visual Inertial系統有OKVIS和ROVIO，但用到IMU Preintegration[5,14,15]的暫時沒開源，只在GTSAM[6]中包含了相關的IMU factor，而ORBSLAM作者的這部分代碼暫時也沒有開源。自己在嘗試上面論文的方法之後感覺論文裡初始化挺實用的、整體程序也不很難，所以就在寫完之後放到了github上供大家一起學習交流。
由於能力有限，如果有bug還請包含並指出，作者盡力修正，也歡迎大家一起玩、一起改進。本文檔給出簡單的說明，以及主要的參考資料。
2. ROS入口
這份代碼暫時只寫了rosbag的接口，入口在Examples/ROS/ORB_VIO/ros_vio.cc中。每次讀取一個圖像msg和到這個圖像的一組IMU msg，在Tracking和LocalMapping計算完之後再對下一個圖像msg和下一組IMU msg進行處理（所以並不是實時的）。
（註：馮餘劍同學提供了去掉ROS的版本，在github的issue裡放了連結。）
3. IMU參數
目前只在EuRoc數據集[7,8]上進行了測試，IMU的參數寫死在IMU/imudata.cpp中，噪聲參數來源於數據集中的sensor.yaml，計算參考Kalibr[9,10]的wiki中所述IMU噪聲模型[11]。如這個wiki中所說，datasheet上所給的理論值一般是理想情況下的，實際使用時看情況要放大一些，所以程序中計算時有進行放大（但也只是大致試了幾個值，選了一個看起來好點的）。
另外，EuRoc的數據中IMU和相機之間沒有延時，自己的數據需要Kalibr進行標定。
（註：這兩天在自己的硬體數據上跑起來了，見下文說明）
4. IMU預積分（Preintegration）
（Preintegration的實現在IMU/IMUPreintegrator.h和IMU/IMUPreintegrator.cpp中）
Visual Inertial ORBSLAM中，IMU的使用方式是把相鄰幀或相鄰關鍵幀之間的IMU測量值利用預積分的方式計算一個近似高斯分布的「偽」測量量，這種方式可以把IMU的噪聲參數傳遞給這個偽測量量，得到預積分的不確定度。利用這個預積分作為幀間的觀測量加入優化中。
此外，預積分能給出偽測量量相對於零偏變化的雅克比，這樣在零偏有修正時，可以計算一階近似值，而不用重複計算積分，減小計算量。
預積分的公式參考Forster的論文和補充材料[5,14,15]，需要說明的是早先的兩篇[14,15]中公式有稍許錯誤（3/2和一些下標不對）。SO3上左右雅克比、Adjoint等相關的理論公式可以參考[16]的7.1節，或者聯繫@高博的新書《視覺SLAM十四講》。@謝曉佳也有一份預積分的「部分」總結文檔，可以在qq群文件中找到。但感覺最好都自己手推一遍。
5. 待估計狀態NavState
（NavState的實現在IMU/NavState.h/和IMU/NavState.cpp中）
NavState名字來源於GTSAM，包含位置P、速度V、旋轉R、陀螺零偏bg、加速度計零偏ba。除此以外，考慮到優化過程中要計算零偏的變化量，但預積分的方式保持了零偏不變，所以在NavState中加入了陀螺和加速度計的零偏增量dbg/dba。
每當新建一個關鍵幀時，會利用到目前為止已經估計好的當前幀零偏bg/ba及零偏增量dbg/dba來初始化關鍵幀的零偏為bg+dbg/ba+dba，而把零偏增量初始化為0。在優化更新過程中，只更新增量dbg/dba而保持bg/ba不變（這樣就不用重新計算預積分了）。
NavState的更新方式參考了論文[14]的補充材料[15]，應該也可以有其他方式。
6. VINS初始化
（這部分在LocalMapping.cc的TryInitVIO()函數中）
對單目VI系統來說，個人感覺初始化是比較麻煩的部分。
之前很多的工作可能是用IMU初始化一個初始姿態，然後慢慢估計和收斂尺度，或者由其他傳感器提供尺度，這些相關工作了解還不是特別多，但感覺不那麼「優雅」。感覺比較優雅的方式：沈劭劼老師有專門針對單目VINS無先驗要求的初始化的討論[12,13]，但暫時沒有開源，自己試著寫了個挫挫的視覺前端跑起來並不好用，還是需要個好的視覺前端的……
在此基礎上，ORBSLAM的作者對[13]的做法進行了改進，利用已有的純VSLAM的地圖，分步驟來估計陀螺零偏、加速度計零偏和尺度、重力加速度。按照論文的介紹實現之後，發現可以基本復現論文中兩個數據集初始化的結果（從這個角度看感覺這方法還比較實用）。
具體的方法參考論文，寫的程序中以學習為目的，所以在前15秒，每來一個新的關鍵幀，都會用所有的關鍵幀進行一次計算，把結果存下來，然後可以用pyplotscripts中的文件畫出初始化各變量的變化情況。
需要說明的是，我推導公式的時候，發現和論文中部分符號不太一致，建議自己整理推導下。程序中重力方向認為是(0,0,1)，這應該並不影響最終結果。
7. 新加g2o相關節點/邊
（相關實現在IMU/g2otypes.h和IMU/g2otypes.cpp中）
因為在ORBSLAM的框架下，優化用的庫是g2o，需要根據需要增加g2o的edge/vertex。
具體的對照論文中的圖（原文中Fig 2）。
VertexNavStatePVR中包含了上圖中的位姿P和速度v，VertexNavStateBias中包含了上圖中的IMU零偏b。（按作者圖裡的意思，他可能把PR和V也分開成兩類vertex了。）
EdgeNavStatePVR作為上圖中上面的綠框所指edge，是一個多元邊，連結相鄰兩幀的PVR和兩幀中較早一幀的Bias。它的測量（測量值和不確定度）由預積分給出。
EdgeNavStateBias作為上圖中下面的綠框所指edge，連接相鄰兩幀的bias。它的測量的值保持為0，不確定度由IMU隨機遊走參數確定。
EdgeNavStatePVRPointXYZ作為上圖中左邊藍框所指edge，連接PVR和地圖點。
EdgeNavStatePVRPointXYZOnlyPose作為上圖中右邊藍框所指edge，是一個一元邊，用於poseOptimization中的pose only Bundle Adjustment。
EdgeNavStatePriorPVRBias作為上圖中的灰色框所指edge，包含marginalize之後的當前幀prior信息。（這裡需要說明，計算marginalize的結果用到g2o的computeMarginals()函數，我發現這個函數在cholmod/csparse的solver中才有，而ORBSLAM2中只有Eigen的solver。所以程序裡在Thirdparty/g2o/solvers中增加了cholmod的求解器，相關計算時用這個。）
VertexGyrBias和EdgeGyrBias用於初始化時的陀螺零偏估計。（需要說明的是用到這部分的估計器本質上是一個線性估計器，一步就能收斂。）
其他定義的NavState相關的Edge/Vertex是之前測試15DoF節點時留下的，也就是說把上述VertexNavStatePVR和VertexNavStateBias合在一起。但這樣的一個可能問題是，兩者本質上噪聲本是可以分開考慮的，IMU測量值的噪聲不應該影響bias。如果合在一起，在優化中進行robust kernel相關的操作時也得一起操作，導致bias的更新會受到測量噪聲的影響。所以這部分之後就參考論文中的方式把PVR和bias分開寫了。
8. 程序其他修改
假設讀者對ORBSLAM基本了解，在其基礎上程序中其他修改就是參考論文[2]，在不改變整體架構的前提下，按自己的理解進行一些相關的調整。主要的有：
KeyFrame/Frame中增加必要的數據；
VINS初始化完成後，Local Window從covisible window改為最近N個關鍵幀（N=10）；
新建關鍵幀和去除冗餘關鍵幀時，考慮要讓相鄰關鍵幀之間間隔時間不能太長；
實現論文中Tracking在地圖是否有更新時的兩種不同策略；
實現重定位時的重新估計和一些小細節；
修改閉環時的一些小細節；
……其他的自己也得找一陣，用BeyondCompare吧……
9. 關於自己的硬體
自己所組的IMU+攝像頭硬體是3DM-GX3-25和UI-1221-LE，幀率分別是200Hz和20Hz。IMU設置為200Hz的幀率，相機設置為外部觸發，並且固定曝光。用一個單片機採集和轉發IMU的200Hz數據，每10幀數據觸發一次相機，也就是圖像保持20Hz輸出。
採用這個方式的原因在於想處理好延時。這種方式下，假如IMU-STM32之間串口傳輸耗時變化不大，那麼IMU的數據採集時間點和圖像數據採集時間點（這個時間點是相機曝光時間的中點，參見VI-SENSOR的論文[17]）之間的延時基本是固定的。通過kalibr可以標定出來。
關於kalibr的標定可以再囉嗦一點遇到的小坑。最開始程序在自己的硬體錄的數據上效果很差，會出現莫名其妙的錯誤結果（比如相機位姿方向明顯反了）。之後對相機重新進行內參標定時（用ROS自帶的camera_calibration），把畸變階次提高到k1~k6&p1~p2保證去畸變的效果較好（用的鏡頭視角偏大，畸變明顯）。然後錄kalibr的標定數據時，先對圖像進行去畸變，並且讓運動激勵充分（足夠的三軸加速度和角速度激勵），標定出來的結果就比較合理了（外參的旋轉和平移量和肉眼觀察相近）。感覺這部分還是要仔細處理好的，自己吃了不少虧，嘗試了N多遍……
這裡給一個手持上述傳感器在實驗室跑的結果錄像（非實時，另外沒仔細調，結果並不算完美）。
連結：http://v.youku.com/v_show/id_XMTkxMjgzMzMwOA==.html
10. 總結
總的來說，除了所加的IMU這部分外，改動並不大，這也得益於ORBSLAM這個視覺SLAM框架寫的很好，個人感覺作為一個學習入手的材料挺好的。
所以再次強調，目前寫的這部分只是面向學習的代碼，非實時，沒有細調各處邏輯和參數，也可能有各種bug和可改進之處，歡迎感興趣的同學交流討論一起改進。
另外，本程序在Ubuntu14.04 + ROS indigo + OpenCV 2.4.8(ROS 自帶)下測試，其他環境需要可能需要相應調整。而自己攢的硬體感覺是比較好的，對於性能更低點的比如MPU6050的IMU、捲簾快門的相機，並沒有測試，不確定效果如何。
11. 致謝
感謝@張騰、@謝曉佳、@張明明的指點和討論，感謝@高翔幫忙debug。感謝@胡佳興和@李承陽兩位「給」的IMU。
參考資料
[1] https://github.com/jingpang/LearnVIORB
[2] Raúl Mur-Artal and Juan D. Tardós. Visual-inertial monocular SLAM with map reuse. arXiv preprint arXiv:1610.05949.
[3] Raúl Mur-Artal and Juan D. Tardós. ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras. ArXiv preprint arXiv:1610.06475
[4] https://github.com/raulmur/ORB_SLAM2
[5] Christian Forster, Luca Carlone, Frank Dellaert, Davide Scaramuzza, "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry," IEEE Transactions on Robotics (in press), 2016 .
[6] https://bitbucket.org/gtborg/gtsam/
[7] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. Achtelik and R. Siegwart, The EuRoC micro aerial vehicle datasets, International Journal of Robotic Research, DOI: 10.1177/0278364915620033, early 2016.
[8] http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
[9] Paul Furgale, Joern Rehder, Roland Siegwart (2013). Unified Temporal and Spatial Calibration for Multi-Sensor Systems. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan.
[10] https://github.com/ethz-asl/kalibr
[11] https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model
[12] Shen, S., Mulgaonkar, Y., Michael, N., & Kumar, V. (2016). Initialization-Free Monocular Visual-Inertial State Estimation with Application to Autonomous MAVs. In Experimental Robotics (pp. 211-227). Springer International Publishing.
[13] Yang, Z., & Shen, S. (2016). Monocular Visual-Inertial State Estimation With Online Initialization and Camera-IMU Extrinsic Calibration.
[14] Forster, C., Carlone, L., Dellaert, F., & Scaramuzza, D. (2015). IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation. In Robotics: Science and Systems XI (No. EPFL-CONF-214687).
[15] http://rpg.ifi.uzh.ch/docs/RSS15_Forster_Supplementary.pdf
[16] Barfoot, State Estimation for Robotics: A Matrix-Lie-Group Approach, 2015. http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser15.pdf
[17] Nikolic, J., Rehder, J., Burri, M., Gohl, P., Leutenegger, S., Furgale, P. T., & Siegwart, R., A Synchronized Visual-Inertial Sensor System with FPGA Pre-Processing for Accurate Real-Time SLAM. IEEE International Conference on Robotics and Automation (ICRA), Hongkong, China.
【版權聲明】泡泡機器人SLAM的所有文章全部由泡泡機器人的成員花費大量心血製作而成的原創內容，希望大家珍惜我們的勞動成果，轉載請務必註明出自【泡泡機器人SLAM】微信公眾號，否則侵權必究！同時，我們也歡迎各位轉載到自己的朋友圈，讓更多的人能進入到SLAM這個領域中，讓我們共同為推進中國的SLAM事業而努力！
【注】商業轉載請聯繫劉富強（liufuqiang_robot@hotmail.com）進行授權。普通個人轉載，請保留版權聲明，並且在文章下方放上「泡泡機器人SLAM」微信公眾帳號的二維碼即可。
【編輯】張韻