/****************************************************************************/
 *
 *                  (c)    光明工作室  2017-2037  COPYRIGHT
 *
 *   光明工作室團隊成員大部分來自全國著名985、211工程院校。具有豐富的工程實踐經驗，
 *本工作室熱忱歡迎大家的光臨。工作室長期承接嵌入式開發、PCB設計、演算法模擬等軟硬體設計。
 *
 *
 *1）基於C8051、AVR、MSP430微控制器開發。
 *2）基於STM32F103、STM32F407等ARM處理器開發。（IIC、SPI、485、WIFI等相關設計）
 *3）基於C6678、DM388等DSP處理器開發。（視訊、網路、通訊協議相關設計）
 *4）基於QT、C#軟體開發。
 *5）基於OPENCV、OPENGL影象處理演算法開發。（基於LINUX、WINDOWS、MATLAB等）
 *6）無人機飛控、地面站程式開發。（大疆、PIX、 qgroundcontrol、missionplanner、MAVLINK）
 *7) ROS機器人作業系統下相關開發。
 *8）LINUX、UCOSII、VXWORKS作業系統開發。
 *
 *
 *                                                 聯絡方式：
 *                                                 QQ:2468851091 call：18163325140
 *                                                 Email:2468851091@qq.com
 *
/ ****************************************************************************/   

MulinB按：最近在學習SLAM演算法，這裡作為閱讀筆記記錄和總結一下。這裡關注的主要是基於視覺的Visual SLAM或Visual Odometry，也包括一部分影象和IMU融合的Visual-Inertial Odometry相關演算法。

注：下文中部分連結指向Google Scholar及Youtube，有些使用者可能無法訪問。

一、概述

Simultaneous Localization and Mapping (SLAM)原本是Robotics領域用來做機器人定位的，最早的SLAM演算法其實是沒有用視覺camera的（Robotics領域一般用Laser Range Finder來做SLAM）。本文主要關注基於camera影象做SLAM的演算法，即Visual SLAM演算法。本文對SLAM和Visual SLAM不做區分。

其實SLAM是一個研究了30年的topic了，然而到目前仍然沒有廣泛走進人們的生活。個人認為，其中一個原因是SLAM對實時性要求比較高，而要做到比較精確、穩定、可靠、適合多種場景的方案一般計算量相對較大，目前移動式裝置的計算能力還不足夠支撐這麼大的計算量，為了達到實時效能，往往需要在精確度和穩定性上做些犧牲。因此在具體的應用中，往往需要根據移動裝置所具有的感測器組合、計算能力、使用者場景等，選擇和深度定製合適的SLAM演算法。比如，無人駕駛汽車和手機端AR類應用的SLAM演算法就非常不同。無人駕駛汽車中，感測器的使用可以更多樣化，其中在建地圖時和實時定位時的演算法又很不一樣，需要根據具體情況下使用的感測器型別和可以獲得的地圖輔助資料型別等資訊進行綜合性的方案設計，以達到精度和穩定性最大化。而在手機端的AR類應用中，如何利用標配的單目+MEMS IMU在旗艦類手機上得到更精確、更穩定的演算法是關鍵任務。本文主要focus在後者。

關於SLAM的基礎入門，這裡不多囉嗦，列一些不錯的學習資源如下：

• Tomasz Malisiewicz的部落格有一篇不錯關於SLAM的文章，其中很多link的資源都很好

• 國內有個不錯的SLAM組織，叫“泡泡機器人”，其中有很多不錯的SLAM課程，可以關注其微信公眾號檢視往期課程。另外該組織的不少成員都有部落格，有很多不錯的演算法詳細介紹的文章

另外再列一些比較強的研究SLAM的組織和牛人：

• ICL的Andrew Davison組：老牌SLAM研究組，專注SLAM十幾年，很多SLAM領域的大牛都出自這個組，比如KinectFusion和DTAM的作者Richard Newcombe、G2O的作者Hauke Strasdat等

• Oxford的ActiveVision組：PTAM出自這個組，Andrew Davison也畢業自這個組

• Gatech的Frank Dellaert組：GTSAM出自這個組

• TUM的Daniel Cremers組：LSD-SLAM出自這個組

• UZH的Davide Scaramuzza：SVO和OCamCalib出自這個組

• ETH Zurich的ASL組：OKVIS和BRISK的作者Stefan Leutenegger畢業自這個實驗室

• Monash Univ的Tom Drummond：他曾經在Cambridge做過教授，FAST detector的作者Edward Rosten是他的學生，PTAM的作者Georg Klein也是他的學生

• Juan D. Tardos：ORB-SLAM出自他的學生

• UMN MARS Lab的Stergios Roumeliotis：MSCKF 1.0出自當時還在這個實驗室讀書的Anastasios Mourikis，另外一篇關於quaternion algebra的非常棒的tutorial也出自這個實驗室

• Anastasios Mourikis：MSCKF 1.0出自他之手，MSCKF 2.0出自他的學生Mingyang Li

二、Monocular SLAM演算法筆記

Visual SLAM的兩篇開山之作應該是Davison ICCV 2003和Nister CVPR 2004 Visual Odometry的兩篇paper了。Andrew Davison是從Robotics領域的視角出發，將經典的non-camera的SLAM演算法應用到使用single camera做SLAM的方案中，使用了經典的EKF框架，經過後續改進就是TPAMI 2007的MonoSLAM[2]演算法。David Nister是Computer Vision領域3D Vision的大牛（著名的五點法就是他的作品），他的CVPR 2004 Visual Odometry paper是從3D Vision的視角出發設計的一個SLAM方案，整個方案設計的比較簡單，只是著重連續較少幀的相對camera pose的估計，並沒有完整的地圖維護機制，因此容易造成drift。不過其3D Vision的演算法和思想是後續很多的SLAM演算法中都使用到的，這裡先從Nister的這篇VO paper開始介紹。

Nister VO (2004) [1]

下面是Nister這篇VO paper的演算法流程：

Nister的這篇paper重點介紹了vision相關的演算法及實現細節（包括如何高效實現角點提取和match等），但在系統流程上有些介紹的並不詳細，比如上面InnerLoop和OuterLoop的次數和條件等。演算法中的很多思想在後續的Visual SLAM演算法中都有沿用，比如2D to 2D用五點法計算初始pose、3D to 2D用P3P計算pose、2D correspondence -> essential matrix -> pose -> triangulation -> 3D to 2D correspondence -> P3P -> optimization的流程及子模組的迭代方式等（並不是說所有這些3D Vision演算法都是Nister首創，重要的是整個流程在Visual SLAM中的應用）。

• 演算法優點：沒有對motion model的假設，主要靠三點法P3P來計算相機pose；在計算過程中引入多次bundle adjustment提高精度
• 演算法缺點：演算法流程具體細節描述的不太清晰；沒有比較fancy的map points維護機制；插入firewall阻止誤差累積的方式比較原始，無法消除drift；方法比較簡單，沒有跟蹤丟失後重定位、迴環檢測等現代SLAM方法中比較常見的機制

MonoSLAM (2003-2007) [2] (code available)

Andrew Davison的MonoSLAM是將傳統機器人領域中基於laser range-finder的EKF-SLAM應用到了single camera的SLAM中，演算法相對於Nister VO更加完整，其中關鍵的提高在於通過guided feature matching取代invariant feature matching提高了計算速度。這裡有個介紹MonoSLAM的不錯的slides，在Github上有MonoSLAM的程式碼。另外，更入門的材料，關於Kalman Filter和Particle Filter的介紹，最容易intuitively理解的是udacity上Sebastian Thrun的視訊課程。另外，這裡有一篇非常不錯的對Kalman Filter介紹的blog。

具體點講，在MonoSLAM中，每一個state由一個當前狀態的最佳估計state vector x^，以及一個表示該狀態uncertainty的協方差矩陣 P 表示。其state vector包括相機的pose、velocity、angular velocity、以及場景中所有3D map points的座標。該模型是一個對於state vector中引數的single modal multivariate Gaussian modeling，借用一個上述slides的圖表示： 

假設場景中有 N 個3D map points，那麼每次更新上述協方差矩陣的計算複雜度就是 O(N2)，為了維持在realtime的performance，通常需要控制3D map points的個數 N<100。雖然這表面看起來不是很高效，paper中介紹了原因如下：之所以選擇一個full covariance matrix而不選擇使用covariance submatrix減小計算，是因為MonoSLAM的目標是在一間room大小的場景中進行repeatable localization，這種情況下相機的view會較多的與彼此重疊，會有頻繁的loop closure發生，這種情況下用一個full covariance matrix會更加精確，而且100個左右的map points足夠表徵一間room大小的場景。

MonoSLAM中對於特徵的檢測和匹配如下：特徵點檢測使用的是Shi-Tomasi corner detector (類似於Harris detector，這裡有其區別)，特徵匹配是靠warped image patch進行NCC匹配（每個patch template儲存其所在view的surface orientation，匹配時根據後續幀的pose將該patch template投影過去），paper中特意提到該patch template一旦初始化將不進行更新（為防止drift）。更進一步，patch template的surface orientation也是通過一個單獨的Kalman filter進行estimate的(Section 3.8)。

基於Kalman filter的演算法在每個時刻的計算一般分為兩步：第一步，predict step；第二步，update step。在第一步中一般是根據運動模型或者控制模型預測state vector，是uncertainty propagation的過程，其中用到的運動模型或者控制模型需要根據實際場景設定其uncertainty的引數，直接影響covariance matrix的計算。在第二步中是根據觀測到的結果來估計最佳state vector並減小uncertainty。借用wikipedia的公式，Kalman filter的計算過程如下： 

作為對比，EKF是將上述predict step中的變換矩陣 F 更換成非線性的函式 f(−)，將update step中的觀測矩陣 H 更換成非線性的函式 h(−)，EKF的計算流程如下： 

看到這麼多符號，我一般就跪了。幸運的是，MonoSLAM paper裡Kalman filter的符號(Section 3.4-3.5)以及上述wikipedia的符號都和這篇牛逼的深入淺出的blog文章裡的符號是一致的，對照著該blog文章可以大概領悟到MonoSLAM paper裡是在幹啥(進一步整明白還要再看看程式碼)。值得注意的是，在計算Kalman filter update step時的innovation covariance matrix S 可以用來選擇保留每一幀中資訊量較大的feature。

最後，在MonoSLAM中比較有趣的是其3D map point的triangulation（paper中叫feature initialization, Section 3.6），由於可以儲存3D點的uncertainty，當parallax不夠大時，3D點的depth不確定性很大，可以儲存其所在的3D line的資訊，隨著觀測的增多及parallax足夠大，最終再轉化成一個3D點的Gaussian distribution。估計後面的SVO的depth filter的思想也是從這裡借鑑的，只是在filter-based SLAM中由於uncertainty是顯式表達出來的，所以比較直觀，而在keyframe-based SLAM中就沒那麼直觀了。提到這兩類SLAM的比較，filter-based SLAM and keyframe-based SLAM, 比較深入的分析可以參見Hauke Strasdat的Why Filter IVC 2012 paper [7]。注意其中keyframe-based SLAM有時也被叫做BA-based SLAM或optimization-based SLAM，Strasdat的paper中用的典型例子是Ethan Eade and Tom Drummond的ICCV 2007 paper [3]。值得注意的是，在那篇paper中，雖然使用的是BA-based的方法，其實對於3d map point也是有uncertainty modeling的（使用的是information matrix，類似於covariance matrix，其區別可以參見Strasdat的Why Filter IVC 2012 paper [7]）。順便提一句，Ethan Eade的個人主頁上有不錯的Lie Group and Exponential Mapping的總結文件 :-)

整個MonoSLAM的流程圖如下：

Davison的這篇MonoSLAM是他之前幾篇會議paper的綜合，描述了一個相對比較完整的Filter-based Visual SLAM系統，包括用motion model預測的camera pose來進行guided feature matching（paper裡稱為active search）、patch matching時考慮surface normal、3D map points的維護等。在paper中也提到了可以較容易的integrate gyro資料，因為其state vector裡有angular velocity的引數，可以直接在Kalman filter中增加一步measurement update將gyro資料加入即可。整個系統可以在1.6GHz的Pentium M處理器上處理320x240的影象接近60fps，速度快的讓人吃驚（速度快得益於其處理的feature個數比較少，平均每幀影象大約只處理12個feature）。

• 演算法優點：速度快；可以顯式建模map points的uncertainty；較容易融合gyro資料
• 演算法缺點：Filter-based方法的缺點是隻維護一個當前時刻的state，不能利用history資料進行optimization；每幀跟蹤的map points太少，比較容易丟失；當需要精度較高時，需要更多的map points，這是計算複雜度會增大很快(參見[7])；在camera不太動時，會有jitter(參見PTAM paper[4]的對比分析)；系統初始化方法比較原始，屬於半自動形式

PTAM (2007-2008) [4,5,6] (code available)

PTAM是keyframe-based SLAM派系裡最出名的一個演算法，作者是Georg Klein和David Murray。David Murray是Oxford的教授，是Andrew Davison讀博士期間的導師。Georg Klein和Ethan Eade都是師從當年在Cambridge的Tom Drummond，兩位好像都在Microsoft的HoloLens工作過。有點扯遠了。PTAM名聲大噪的原因是，它開創了多執行緒SLAM的時代，後來的多數keyframe-based SLAM都是基於這個框架。PTAM受到了廣泛採用bundle adjustment (BA)的Nister演算法的啟發，將tracking和mapping分成兩個單獨的執行緒，這樣既可以不影響tracking的實時體驗，又可以在mapping執行緒中放心使用BA來提高精度（另外BA也沒有必要對所有frame做，只對一些keyframe進行BA即可）。這樣以來，由於BA的引入，PTAM的精度得到了大幅提高，連Davison自己都承認MonoSLAM被PTAM clearly beaten了（參見這個slides）。

PTAM的資料結構主要包括keyframe和3D map point。Keyframe儲存的是camera pose及一個4-level的image pyramid (從640x480到80x60)。3D map point儲存的是3D座標、patch normal、以及來自哪個keyframe的哪一層。系統執行時通常有大約100個keyframes和幾千個3D map points。

PTAM的mapping執行緒流程如下：

PTAM的tracking執行緒流程如下：

除了上述在paper [4]中描述的tracking和mapping，這裡的code還實現了paper [5]中的relocalization模組。大致來講，在track丟失後，用small blurry image （40x30的經過gaussian blur及mean-subtracted的小圖）進行SSD匹配來檢測是否與之前的keyframe匹配度較高，如果檢測到匹配的keyframe，通過先恢復出2D的SE(2)引數再進而用類似於unscented transform恢復出SO(3)引數得到camera pose。

PTAM在paper [4]中report的速度是在Intel Core2 Duo E6700 2.66 GHz處理器上處理640x480的frame能達到30fps，tracking中主要時間花費在3D到2D投影后的搜尋correspondence上。另外mapping執行緒中的bundle adjustment一般還是比較慢，為了跟上map expanding，global BA會經常被打斷。注意這裡的CPU其實比MonoSLAM用的CPU要強不少，甚至比現在主流智慧手機的CPU都要強。PTAM的作者後來將該演算法做了一些裁剪和定製應用到了一個iPhone 3G上（處理器412MHz，比E6700慢15-30x速度），參見paper [6]。

• 演算法優點：精度比MonoSLAM高，分兩個獨立執行緒，在不影響tracking體驗的情況下mapping引入BA
• 演算法缺點：tracking依賴於map points (3D to 2D tracking)，這樣如果如果fast motion時map expansion太慢會導致tracking丟失；沒有model 3D map points的uncertainty

ORB-SLAM (2014-2015) [8] (code available)

最近比較火的ORB-SLAM演算法，是keyframe-based SLAM派系的一個集大成者。ORB-SLAM演算法基本沿用了PTAM的框架，將近幾年來被驗證有效的module都加了進來，做出一個穩定性和精度很高、可以用於室內/室外和小規模/大規模等各種場景的全能系統，刷爆各種benchmark，並開源了質量很高的程式碼，還摘得了IEEE Transactions on Robotics的年度best paper award，可謂牛逼哄哄。關於這個演算法，網上能找到很多學習資料甚至程式碼解讀。

ORB-SLAM演算法與PTAM的主要不同之處在於以下幾個方面：

• 提出一個自動做map initialization的演算法：通過correspondence同時計算homography matrix和fundamental matrix，再通過一個heuristic的準則判斷屬於哪種情況，並用相應情況下的演算法計算初始pose。個人覺得這一點是ORB-SLAM這篇paper最大的貢獻（當然其程式碼開源的貢獻也非常大，這裡只是說paper的原創點）。

• 在tracking和mapping兩個執行緒之外加入第三個執行緒做loop closing，並在做loop closing時加入scale作為優化引數修正scale drift。

• 設計了更加高效和更適用於large scale場景的keyframe管理機制（比如covisibility graph、essential graph等），設計了更細緻的keyframe/map points的建立、篩選準則等。

• Loop detection和relocalization使用了更先進的place recognition方法（bag of words），取代了PTAM中的small blurry image方法。

• Feature matching用ORB feature matching取代了PTAM原始的patch matching。

ORB-SLAM的演算法流程跟PTAM有些細微的不同，這裡大致列一下。也先從mapping執行緒開始（為了與PTAM流程圖保持一致，下圖裡的步驟名稱沿用上面PTAM流程圖的）：

上圖可以看出ORB-SLAM的mapping中與PTAM的最大的不同之處是，ORB-SLAM的TriangulateMapPoints建立更多的map points時使用了更多的keyframes（即covisibility graph中的所有keyframes）。另外，global BA從mapping執行緒移到了loop closing時另起一個執行緒做（沒錯，其實系統在做global BA時同時會有四個執行緒在執行）。

ORB-SLAM的tracking執行緒如下（為了簡化，這裡省去了tracking lost時的relocalization的分支）：

上圖可以看出，ORB-SLAM的tracking中與PTAM的最大的不同之處是，當track當前幀時，ORB-SLAM只用了上一幀能觀測到的map points來投影匹配，而PTAM直接用所有地圖點來匹配，這樣在地圖點較多時顯然會效率較低。所以ORB-SLAM是做出了更適合large scale SLAM的設計。

ORB-SLAM的loop closing流程比較簡單，基本就是loop detection -> estimate Sim(3) transformation -> loop fusion -> optimize essential graph。這裡有很多細節需要注意，所幸的是ORB-SLAM公佈的程式碼註釋很豐富，很容易follow，這裡細節不再多說。

從paper中report的速度上來看，ORB-SLAM能在Intel Core i7-4700MQ (4 cores 2.40GHz)上track 512x382的視訊流達到30fps。鑑於這個CPU比PTAM的CPU要強不少，這個演算法應該比PTAM慢不少。

• 演算法優點：具有自動初始化的功能；精度和穩定性都較高；keyframe和map points的管理機制也比較完善；系統比較完整，可以用在large scale SLAM中
• 演算法缺點：tracking仍然依賴於map points (3D to 2D tracking)，這樣如果fast motion時map expansion太慢會導致tracking丟失；依然沒有model 3D map points的uncertainty，有些情況下可能會由於初始化的不好造成錯誤累積

LSD-SLAM (2013-2014) [10, 12] (code available)

自從2011年的ICCV上，Andrew Davison的學生Richard Newcombe帶著DTAM [9]用牛逼哄哄的效果完爆了PTAM之後，眾人意識到更dense的tracking和mapping會大幅度提高SLAM的精度和魯棒性。直覺上想想，如果整幅影象的資訊都可以拿來用，的確資訊量會比提一些feature多很多。另外，Computer Vision界研究了多年的傳統的image alignment及optical flow的很多subpixel精度的經驗都可以借鑑，也會幫助提高精度。自此，dense及direct這兩個概念開始在SLAM中火了起來（其實這兩個概念在SfM中可以追溯到很久之前），dense指的是儘量多的利用image資訊，direct是相對應於feature matching/projection的direct image alignment方法（通常feature-based methods是最小化reprojection error，而direct image alignment methods中是最小化photometric error）。由於DTAM的計算量有點大，在GTX 480 GPU上處理640x480視訊流勉強可以到30fps，也誕生了一些做了trade-off能達到更快的計算速度的方法，比如semi-dense VO [10]，semi-direct VO (SVO) [11]等。

Jakob Engel在ICCV 2013提出的semi-dense VO [10]方法其實就是最近很火的LSD-SLAM的前身。這個演算法基本是將他們組之前基於RGBD camera的DVO思想用到了monocular camera中。演算法思想如下：演算法仍然分為兩個獨立的執行緒分別做tracking和mapping，假設已知前兩幀影象對應的depth map（這個初始的depth map可以通過傳統的correspondence方法計算或者初始化為隨機值），tracking執行緒通過最小化photometric error來求解相機姿態（對於已有depth map的影象I1中每個有depth d的點x，通過未知姿態引數P，可以求出其在新一幀影象I2中相應的位置x~，然後該點的photometric error即為||I1(x)−I2(x~)||），mapping則通過類似於Kalman filter的機制來predict-update每個點的depth及其uncertainty（其中predict step是通過estimated pose來propagate到下一幀對應點的depth，update step是基於每個點選擇不同的reference frame匹配並計算depth，注意其中uncertainty的propagation和update比較有趣，包括observe時對於geometric uncertainty和photometric uncertainty的引入）。該VO演算法還被作者移植到手機上做AR，在一篇ISMAR paper中能找到，不過從演示視訊中看來感覺有點卡頓和抖動。

LSD-SLAM [12]就是在上面這個VO演算法基礎上，加入keyframe的管理及優化、loop closure等機制使得上面的VO演算法真正成為一個完整的SLAM系統。在LSD-SLAM中：

• 地圖是由一系列keyframe（儲存semi-dense的depth map和depth variance map）以及keyframes之間的Sim(3)變換組成
• 在對新來的一幀影象做tracking時，用上面的VO演算法從最近的keyframe估計其SE(3) pose
• 當需要建立新keyframe時，用上面的VO演算法估計其depth map及depth variance map
• 當把keyframe加入地圖時，估計該keyframe與其他keyframe之間的Sim(3)變換
• 用OpenFABMAP做loop detection

值得注意的幾個細節是：

• 在tracking的photometric error中，用intensity variance及depth variance來做normalize
• 每個keyframe儲存depth map時將其歸一化（mean inverse depth為1），在keyframe之間的edge中儲存scale的關係（即Sim(3)變換）
• 在estimate keyframe to keyframe的Sim(3)變換時，需要在cost function中引入depth error來對scale做constraint

關於LSD-SLAM的演算法解析和程式碼詳解，網上可以找到很多文章，這裡就不再囉嗦。

• 演算法優點：對map uncertainty的model比較好；對於low texutre場景應該比較魯棒；可以建相對比較dense的地圖
• 演算法缺點：演算法目前計算量還是有點大，用i7的CPU勉強可以達到實時；從AR的效果看似乎抖動比較明顯，似乎精度仍不如ORB-SLAM

SVO (2014) [11] (code available)

SVO (semi-direct VO) [11]是另一個由direct method催生的VO演算法，注意跟上述的semi-dense VO [10]名字很像，但其實是個很不一樣的演算法。SVO是一種hybrid的方法，結合了direct method和feature-based method的成分，演算法雖然依賴於feature correpondence，但是避免了顯式計算feature matching和outlier handling，因此也節約了不少無謂的計算量，所以演算法的速度可以做到非常快，在嵌入式的平臺ARM Cortex A9 1.6GHz CPU上處理752x480的frame能達到55fps（在laptop的Intel i7 2.8GHz CPU上可以高達300fps）。演算法依然是分為兩個獨立的執行緒分別做tracking和mapping，下面分別介紹。

SVO的mapping也是採用了probabilistic model，跟LSD-SLAM不同的是，SVO的depth filter是用的Gaussian distribution + Uniform distribution的mixture model，而LSD-SLAM是基於Gaussian distribution的model（類似於Kalman filter）。SVO的depth filter演算法在paper [19]中可以找到詳細推導過程。SVO的mapping執行緒流程比較簡單，主要演算法在updateSeeds裡：

SVO的tracking比較有趣，流程如下：

由上可以看到SVO的tracking中沒有用到motion model，在第一步的alignment中用到了direct method中的方法求pose，第二步中用了經典的optical flow演算法將匹配計算到subpixel精度，第三步後面又用了傳統SLAM演算法的最小化reprojection error的方法去優化。整個過程去掉了顯式的feature matching和RANSAC類的outlier handling演算法，很高效。

SVO paper中有幾個有趣的討論值得注意一下：

• Tracking為何不只使用第一步的演算法：因為第二步其實是用地圖中更多的map points來做tracking，會減少drift，類似於ORB-SLAM中的TrackLocalMap的步驟
• Tracking為何不直接從其第二步開始，跳過第一步：雖然第二步的LK演算法可以單獨作為tracking的演算法，但是第一步的sparse direct image alignment的作用是可以減少LK演算法的搜尋範圍並大量減少匹配的outlier，從而提高了搜尋速度和省去了RANSAC的計算過程
• 由於depth filter的使用，tracking時使用的map points是已經converge的比較可靠的點，所以每幀可以使用比PTAM更少的點來做tracking，從而速度更快
• 另外，mapping執行緒基本只做depth filter seed update，在seed較少時速度可以很快，甚至比tracking速度都快，所以可以每一幀都送到mapping執行緒進行處理

SVO的程式碼在Github上可以找到，程式碼不多比較容易讀，入口可以在Frame_handler_mono.cpp中找到，tracking的主要程式碼在processFrame函式中，mapping的主要程式碼在Depth_filter.cpp中的updateSeeds。

• 演算法優點：速度快；對map uncertainty的model比較好；對運動模型沒有假設
• 演算法缺點：由於track的feature比較少，有些情況下會丟失

DSO (2016) [13] (code available)

DSO [13]是LSD-SLAM的作者Jakob Engel最近放出的另一個大殺器，從其展示的實驗結果看，無論是robustness，或是accuracy，或是計算速度，都完爆LSD-SLAM和ORB-SLAM，上個月又放出了code。從其paper上看，DSO比LSD-SLAM提高的主要原因有以下幾個方面：

• 受OKVIS [16]的啟發，將LSD-SLAM中的depth map estimation從Kalman filter替換成bounded window optimization（仍然主要是photometric error minimization），這樣使得depth的估計更加精確，個人認為這個是精度提高的主要原因
• 在上述的optimization中，加入了更多的引數一起優化，除了camera pose和point depth value之外，還包括camera intrinsics以及考慮曝光時間等因素的affine brightness transfer parameters （當optimization的keyframes之間距離比較遠時，曝光引數的不同可能會影響photometric error）
• 設計了一套比較fancy的point selection的機制，相對於LSD-SLAM使用了更少的點，取樣更sparse，這樣使得計算量減少很多，另外也加入了顯式的outlier detection機制
• 採用了類似於ORB-SLAM的更先進的keyframe管理機制，另外也採用了類似OKVIS的marginalization機制
• 在track新來的一幀時，雖然跟LSD-SLAM一樣也是從最近的一個keyframe來estimate pose，但是在此之前將所有該keyframe可以看到的active points都project過來，這樣增多了可以用來track新frame的線索
• 考慮了camera的photometric calibration因素，比如lens vignetting以及nonlinear response function等，由於這些因素會直接對photometric error產生影響

需要注意的是，上面的optimization只是針對keyframes做的，並不是每一個新來的frame都會做。另外，在paper中對於direct methods和feature-based/indirect methods之間的對比noise實驗比較有趣：direct methods對geometric noise比較敏感，比如rolling shutter等；而indirect methods對photometric noise比較敏感，比如blur等。這點其實說明，在普通的智慧手機上（一般採用rolling shutter camera），基於PTAM類的演算法可能效果更好；而在基於global shutter的機器人相機中，direct methods可能越來越受歡迎。

演算法的計算速度上來看，正常設定可以在Intel i7-4910MQ CPU上處理640x480影象達到realtime的速度，引數低配時可以處理424x320影象達到5倍realtime的速度（不知道具體多少，150fps?）。

• 演算法優點：精度高，魯棒性好，drift小，適用於大規模SLAM
• 演算法缺點：計算量大；對rolling shutter比較敏感；由於optimization不是針對每個frame都做，而只是針對keyframes做，估計用在AR中會出現類似於LSD-SLAM的卡頓，因為非關鍵幀的pose估計不如關鍵幀的準確（TODO： 這點沒有實測，只是猜測，有待考證）

名字縮寫太多，我有點凌亂了，做個區分： 
DVO: TUM的基於RGBD camera的VO方法 
SVO: Gatech的基於semi-direct的hybrid VO方法 
DSO: TUM的direct sparse VO方法

三、Visual-Inertial Odometry演算法筆記

使用monocular camera + IMU的方案來做SLAM/Odometry，一般被稱作Visual-Inertial Odometry (VIO)或者Visual-Inertial Navigation System (VINS)。這一類paper大多出自Robotics社群，主要focus在如何更好的在Visual SLAM中融合IMU資料。IMU資料不單可以幫助resolve單目的scale ambiguity，一般情況下還可以提高SLAM的精度和魯棒性。需要注意的是，想要從IMU資料獲得準確的姿態沒那麼容易，一般需要做sensor fusion，從經典的complementary filter做gyroscope、accelerometer、magnetometer的融合，再到Mahony filter等更復雜的融合演算法，有很多可以選擇的演算法，其精度和複雜度也各不相同。現在的Android系統裡一般可以直接獲得手機姿態，至於其中用了哪種融合演算法本人還沒有仔細研究過，精度也有待考察。在Robotics社群的VIO paper中，一般是直接用原始的IMU資料或者經過簡單濾波的資料，一般需要對IMU的bias進行建模（尤其在MEMS IMU中，所謂的零飄和溫飄對精度影響很大，有些要求比較高的情況下甚至需要將其置於恆溫狀態工作）。

MSCKF (2007-2013) [14,15]

基於Kalman filter的MSCKF跟EKF-based SLAM一樣也是出自Robotics社群，從MSCKF 1.0 [14]到MSCKF 2.0 [15]，精度得到了不錯的提高，據說Google Project Tango中的SLAM演算法就是用的MSCKF演算法。

傳統的EKF-based SLAM做IMU融合時，跟前面介紹的MonoSLAM類似，一般是每個時刻的state vector儲存當前的pose、velocity、以及3D map points座標等（IMU融合時一般還會加入IMU的bias），然後用IMU做predict step，再用image frame中觀測3D map points的觀測誤差做update step。MSCKF的motivation是，EKF的每次update step是基於3D map points在單幀frame裡觀測的，如果能基於其在多幀中的觀測效果應該會好（有點類似於local bundle adjustment的思想）。所以MSCKF的改進如下：predict step跟EKF一樣，但是將update step推遲到某一個3D map point在多個frame中觀測之後進行計算，在update之前每接收到一個frame，只是將state vector擴充並加入當前frame的pose estimate。這個思想基本類似於local bundle adjustment（或者sliding window smoothing），在update step時，相當於基於多次觀測同時優化pose和3D map point。具體細節可以參考paper [15]。

OKVIS (2013-2014) [16] (code available)

相對應於MSCKF的filter-based SLAM派系，OKVIS是keyframe-based SLAM派系做visual-inertial sensor fusion的代表。從MSCKF的思想基本可以猜出，OKVIS是將image觀測和imu觀測顯式formulate成優化問題，一起去優化求解pose和3D map point。的確如此，OKVIS的優化目標函式包括一個reprojection error term和一個imu integration error term，其中已知的觀測資料是每兩幀之間的feature matching以及這兩幀之間的所有imu取樣資料的積分（注意imu取樣頻率一般高於視訊frame rate），待求的是camera pose和3D map point，優化針對的是一個bounded window內的frames（包括最近的幾個frames和幾個keyframes）。

需要注意的是，在這個optimization problem中，對uncertainty的建模還是蠻複雜的。首先是對imu的gyro和accelerometer的bias都需要建模，並在積分的過程中將uncertainty也積分，所以推導兩幀之間的imu integration error時，需要用類似於Kalman filter中predict step裡的uncertainty propagation方式去計算covariance。另外，imu的kinematics微分方程也是挺多數學公式，這又涉及到捷聯慣性導航(strapdown inertial navigation)中相關的很多知識，推導起來不是很容易。這可以另起一個topic去學習了。

OKVIS使用keyframe的motivation是，由於optimization演算法速度的限制，優化不能針對太多frames一起，所以儘量把一些資訊量少的frames給marginalization掉，只留下一些keyframes之間的constraints。關於marginalization的機制也挺有趣，具體參見paper [16]。

ETH Zurich的ASL組另外有一篇基於EKF的VIO paper，叫ROVIO [17]，也有code，具體還沒細看，聽說魯棒性不錯。

IMU Preintegration (2015-2016) [18] (code available in GTSAM 4.0)

從OKVIS的演算法思想中可以看出，在優化的目標函式中，兩個視訊幀之間的多個imu取樣資料被積分成一個constraint，這樣可以減少求解optimization的次數。然而OKVIS中的imu積分是基於前一個視訊幀的estimated pose，這樣在進行optimization迭代求解時，當這個estimated pose發生變化時，需要重新進行imu積分。為了加速計算，這自然而然可以想到imu preintegraion的方案，也就是將imu積分得到一個不依賴於前一個視訊幀estimated pose的constraint。當然與之而來的還有如何將uncertainty也做類似的propagation（考慮imu的bias建模），以及如何計算在optimization過程中需要的Jacobians。相關的推導和理論在paper [18]中有詳細的過程。在OKVIS的程式碼ImuError.cpp和GTSAM 4.0的程式碼ManifoldPreintegration.cpp中可以分別看到對應的程式碼。

[1]. David Nister, Oleg Naroditsky, and James Bergen. Visual Odometry. CVPR 2004.

[2]. Andrew Davison, Ian Reid, Nicholas Molton, and Olivier Stasse. MonoSLAM: Real-time single camera SLAM. TPAMI 2007.

[3]. Ethan Eade and Tom Drummond. Monocular SLAM as a Graph of Coalesced Observations. ICCV 2007.

[4]. Georg Klein and David Murray. Parallel Tracking and Mapping for Small AR Workspaces. ISMAR 2007.

[5]. Georg Klein and David Murray. Improving the Agility of Keyframe-based SLAM. ECCV 2008.

[6]. Georg Klein and David Murray. Parallel Tracking and Mapping on a Camera Phone. ISMAR 2009.

[7]. Hauke Strasdat, J.M.M. Montiel, and Andrew Davison. Visual SLAM: Why Filter?. Image and Vision Computing 2012.

[8]. Raul Mur-Artal, J. M. M. Montiel, and Juan D. Tardos. ORB-SLAM: A Versatile and Accurate Monocular SLAM System. IEEE Transactions on Robotics 2015.

[9]. Richard Newcombe, Steven Lovegrove, and Andrew Davison. DTAM: Dense Tracking and Mapping in Real-Time. ICCV 2011.

[10]. Jakob Engel, Jurgen Sturm, and Daniel Cremers. Semi-Dense Visual Odometry for a Monocular Camera. ICCV 2013.

[11]. Christian Forster, Matia Pizzoli, and Davide Scaramuzza. SVO: Fast Semi-Direct Monocular Visual Odometry. ICRA 2014.

[12]. Jakob Engel, Thomas Schops, and Daniel Cremers. LSD-SLAM: Large-Scale Direct Monocular SLAM. ECCV 2014.

[13]. Jakob Engel, Vladlen Koltun, and Daniel Cremers. Direct Sparse Odometry. In arXiv:1607.02565, 2016.

[14]. Anastasios Mourikis, Stergios Roumeliotis. A multi-state constraint Kalman filter for vision-aided inertial navigation. ICRA 2007.

[15]. Mingyang Li, Anastasios Mourikis. High-Precision, Consistent EKF-based Visual-Inertial Odometry. International Journal of Robotics Research 2013.

[16]. Stefan Leutenegger, Simon Lynen, Michael Bosse, Roland Siegwart, and Paul Timothy Furgale. Keyframe-based visual–inertial odometry using nonlinear optimization. The International Journal of Robotics Research 2014.

[17]. Michael Bloesch, Sammy Omari, Marco Hutter, and Roland Siegwart. Robust Visual Inertial Odometry Using a Direct EKF-Based Approach. IROS 2015.

[18]. Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza. On-Manifold Preintegration for Real-Time Visual-Inertial Odometry. IEEE Transactions on Robotics 2016.

[19]. George Vogiatzis, Carlos Hernandez. Video-based, Real-Time Multi View Stereo. Image and Vision Computing 2011. (Supplementary material)