2. 程式人生 > >Visual SLAM演算法筆記

Visual SLAM演算法筆記

阿新 • • 發佈：2019-01-04
  

一、概述

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的組織和牛人：

二、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的演算法流程：

TrackFeatures[使用Harris提取角點，用patch correlation做match]EstimateInitCameraPose[使用五點法從2D點匹配估計相機相對pose，然後使用三幀影象做BA]TriangulateMapPoints[用點的track軌跡來三角化出點的3D位置，並估計相機pose相對之前軌跡的scale]TrackFromMapPoints[根據已經三角化的3D點，使用三點法P3P求後面幀的pose，並用Local BA迭代refine]RefineMapPoints[根據新的track結果，進一步refine 3D points的位置]End of InnerLoop?[重複TrackFromMap和RefinePoints數次]End of OuterLoop?[重複從EstimateInitPose到RefinePoints數次]InsertFirewallAndReset[插入Firewall阻止誤差累積，重新從第一步開始VO]yesnoyesno

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的圖表示： 
Single modal multivariate Gaussian modeling

假設場景中有 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的計算過程如下： 
Kalman filter

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

看到這麼多符號，我一般就跪了。幸運的是，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的流程圖如下：

SystemInit[使用已知target上的幾個已知corner的位置來初始化]KalmanFilterPredict[使用motion model計算state vector及covariance matrix]MakeMeasurement[根據prediction的state vector進行guided feature matching]KalmanFilterUpdate[根據match的結果update state vector及covariance matrix]ManageMapPoints[根據一些準則刪除不太好的3D map points]TriangulateMapPoints[Bayesian計算uncertainty並triangulate map points]##### End of image stream? #####Endyesno

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執行緒流程如下：

MapInit [其實在tracking執行緒裡，程式碼裡叫TrackForInitialMap][使用者指定兩幀，檢測FAST corner並match correspondences，然後使用Nister五點法計算pose並triangulate map points，最後檢測dominant plane。注意，程式碼裡不是用五點法初始化，而是用homography初始化的。]HasNewKeyframe?[根據幾個準則判斷是否加入新keyframe]ProcessNewKeyFrame [程式碼在AddKeyFrameFromTopOfQueue函式內][將在tracking執行緒中與該keyframe匹配成功的map points做關聯，這一般在傳統的SLAM中叫做data association]ReFindInSingleKeyFrame[將更多的map points投影到keyframe並匹配，加入成功的observation，即data association]ThinCornerCandidates[由於tracking執行緒計算量限制，只能用較快的FAST corner detector，所以在mapping執行緒這裡可以從容的使用Shi-Tomasi或Harris score對corner進行篩選，減少一些質量不高的corner，為下一步做準備]TriangulateMapPoints [程式碼裡叫AddPointEpipolar][在當前keyframe與上一個keyframe之間匹配correspondence：從一幀的corner point出發，在另一幀的epipolar line上用SSD搜尋匹配點，然後進行triangulation計算得到3D map point]HandleBadPoints[檢查所有3D map points的質量，處理掉一些不好的點]SleepForAFewMilliseconds[執行緒讓出CPU幾個ms並準備進入下一輪]BundleAdjustRecent[小範圍的Local BA，這裡的待優化引數是剛加入的keyframe和與其緊密相鄰的幾個keyframes、以及在這些keyframe中可以觀測到的3D map points，作為constraint的是所有可以觀測到這些待優化3D map points的keyframes]ReFindNewlyMade[這一步也是data association，即將剛剛建立的3D map points與之前的keyframes關聯起來，因為剛剛建立的3D map points只是從相鄰的兩個keyframes triangulate出來的，只與這兩個keyframes有關聯]BundleAdjustAll[如果Local BA做完了並且還沒有新keyframe過來，就放個大招]yesno

PTAM的tracking執行緒流程如下：

DetectCorner[將image做成image pyramid，並在每一層上detect FAST corners]ApplyMotionModel[根據運動模型預測camera pose]TrackMapCoarse[將3D map points投影到當前frame並在coarse level上進行SSD匹配，這裡匹配用的template也是經過affine warp的image patch]OptimizePose[進行Gauss-Newton迭代優化camera pose]TrackMapFine[同上TrackMapCoarse, 在fine level上用更多的點進行匹配]OptimizePose[進行Gauss-Newton迭代優化camera pose]UpdateMotionModel[根據camera pose結果, 更新運動模型]AssessTrackingQuality[評估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流程圖的）：

MapInit [其實在tracking執行緒裡，程式碼裡叫MonocularInitialization][分別使用八點法計算fundmental matrix和用DLT計算homography matrix，用heuristic判斷屬於哪種model適用，然後用相應的model估計出pose，並triangulate map points]HasNewKeyframe?[根據幾個準則判斷是否加入新keyframe]ProcessNewKeyFrame[將在tracking執行緒中與該keyframe匹配成功的map points做關聯，即data association]HandleBadPoints [程式碼裡叫MapPointCulling][檢查剛加入的新3D map points的質量，處理掉一些不好的點]TriangulateMapPoints [程式碼裡叫CreateNewMapPoints][在covisibility graph內的keyframes之間匹配correspondence：從一幀的corner point出發，在另一幀的epipolar line上搜索匹配點，然後進行triangulation計算得到3D map point]SleepForAFewMilliseconds[執行緒讓出CPU幾個ms並準備進入下一輪]SearchInNeighbors[這一步也是data association，即將剛剛建立的3D map points與相鄰兩級內的keyframes關聯起來，將重複的map points合併]BundleAdjustRecent[小範圍的Local BA，這裡的待優化引數是剛加入的keyframe和與其緊密相鄰的幾個keyframes、以及在這些keyframe中可以觀測到的3D map points，作為constraint的是所有可以觀測到這些待優化3D map points的keyframes]KeyFrameCulling[基於一些heuristic準則對之前所有的keyframes做個篩查]yesno

上圖可以看出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的分支）：

ExtractORB[將image做成image pyramid，並在每一層上extract ORB features，這裡有個小trick，將image劃分成grid，在每個grid裡單獨提feature，保證所有的grid都包含至少5個features]ApplyMotionModel[根據運動模型預測camera pose]TrackWithMotionModel[將上一個frame裡觀測到的3D map points投影到當前frame並進行匹配，注意這裡與PTAM的TrackMap的區別，PTAM裡是將所有的map points進行投影，這裡只用到了上一個frame的，所以後面另外加了一步TrackLocalMap][如果沒有運動模型或者TrackWithMotionModel失敗，則呼叫TrackReferenceKeyFrame，將上一個keyframe裡觀測到的map points投影並匹配]OptimizePose[進行Levenberg迭代優化camera pose]TrackLocalMap[收集更多的相鄰keyframes並將其能觀察到的所有map points投影到當前幀進行匹配]OptimizePose[進行Levenberg迭代優化camera pose]AssessTrackingQuality[評估tracking的質量，作為判斷是否跟蹤丟失或插入關鍵幀的依據]UpdateMotionModel[根據camera pose結果, 更新運動模型]

上圖可以看出，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)變換
  • 用

    相關推薦

    Visual SLAM演算法筆記

       一、概述 Simultaneous Localization and Mapping (SLAM)原本是Robotics領域用來做機器人定位的，最早的SLAM演算法其實是沒有用視覺camera的（Robotics領域一般用Laser Range Finder來做S

    SLAM學習筆記（2）SLAM演算法

    2、Tinyslam又稱CoreSLAM The advantage of DP-SLAM over CoreSLAM is thus the thoretical ability not to be lost in long corridors, and thisis the goal indeed of

    在Turtlebot2機器人平臺進行SLAM演算法實驗的技術筆記【一】

    一、關於演算法移植方面a.因實際專案需求，我需要把在turtlebot2實驗平臺上面的建圖、定位、路徑規劃、自主導航演算法移植到對應專案的另一個移動底座上，並且上位機也需要更換成另一臺筆記本或者工控機b.首先，我們需要把我們原先筆記本上的整個工作區拷貝到新的上位機上面，由於我

    SLAM學習筆記 - 世界坐標系到相機坐標系的變換

    尺度 sub 像素 png lam 三維 cnblogs 筆記 相對 參考自： http://blog.csdn.net/yangdashi888/article/details/51356385 http://blog.csdn.net/li_007/article/de

    SLAM學習筆記 - 視覺SLAM方法資源匯總

    paper icc 筆記 richard 倫敦 pap 資源 lob sla PTAM - ISMAR2007 英國牛津 Georg Klein 主頁及代碼 PTAM-GPL DTAM - ICCV2011 倫敦帝國理工學院 Richard paper O

    [摘抄] SFM 和 Visual SLAM

    要求 tro 全面 定性 主頁 套路 大牛 匹配 visualsfm SFM和vSLAM基本討論的是同一問題，不過SFM是vision方向的叫法，而vSLAM是robotics方向的叫法， vSLAM所謂的mapping，我們vision方向叫structure，vSLA

    《機器學習實戰》第二章——k-近鄰演算法——筆記

    在看這一章的書之前，在網上跟著博主Jack-Cui的部落格學習過，非常推薦。 部落格地址：http://blog.csdn.net/c406495762  《Python3《機器學習實戰》學習筆記（一）：k-近鄰演算法(史詩級乾貨長文)》 講述的非常細緻，文字幽默有趣，演算法細

    九章演算法筆記 5.深度優先搜尋 Depth First Search

    DFS cs3k.com 什麼時候用dfs? 短, 小, 最問題 而90%DFS的題, 要麼是排列, 要麼是組合 組合搜尋問題 Combination 問題模型:求出所有滿足條件的“組合” 判斷條件:組合中的元素是順序無關的 時間複雜度:與 2^n 相關 遞迴三要素 一般來說,如果面試官不特

    九章演算法筆記 4.寬度優先搜尋 Breadth First Search

    演算法與題型 cs3k.com DFS: 用於搜尋, 題目中有ALL字樣 二分法: 用於時間複雜度小於O(n)的情況 分治法: 二叉樹問題, 子問題和父問題有關係 BFS:- 二叉樹上的寬搜- 圖上的寬搜: 拓撲排序- 棋盤上的寬搜   什麼時候應該用BFS? 圖

    九章演算法筆記 3.二叉樹與分治演算法Binary Tree & Divide Conquer

    大綱 cs3k.com • 時間複雜度訓練 II • 二叉樹的遍歷演算法 Traverse in Binary Tree Preorder / Inorder / Postorder • 二叉樹的深度優先搜尋 DFS in Binary Tree 1.遍歷問題 Preorder

    九章演算法筆記 2.Binary Search

    大綱 cs3k.com 第一境界 二分法模板 • 時間複雜度小練習• 遞迴與非遞迴的權衡• 二分的三大痛點• 通用的二分法模板 第二境界 • 二分位置 之 圈圈叉叉 Binary Search on Index – OOXX• 找到滿足某個條件的第一個位置或者最後一個位置 第三境界 •二分位置 之

    九章演算法筆記 1.Introducing Algorithm Interview & Coding Style

    Implement strStr cs3k.com http://www.lintcode.com/problem/strstr/ Returns the position of the first occurrence of string target in string source, or -1

    九章演算法筆記 8.雜湊表與堆 Hash & Heap

    大綱 cs3k.com 資料結構概述 雜湊表 Hash： a.原理  b.應用 堆 Heap： a.原理    b.應用-優先佇列 Priority Queue  c.替代品-TreeMap   資料結構的兩類問題 cs3k

    九章演算法筆記 6.連結串列與陣列 Linked List & Array

    刷題注意事項 cs3k.com 每道題需要總結的 思路 演算法 核心程式碼 這個題得到的啟示!!!重點是bug free的能力   linked list理解 結果兩個都是 1 2 3 node是存在main函式裡的區域性變數, 還是全域性變數? 區

    九章演算法筆記 7.兩根指標 Two Pointers

    大綱 cs3k.com 同向雙指標 相向雙指標 Two Sum ：3.1 大小• = target• <= target • > target     3.2 去重• unique pairs    3.3 離得近• closest to t

    演算法筆記-快速冪

    快速冪就是快速的求底數的整數次方，比起樸素的方法O(n)的時間複雜度，其時間複雜度是O(log2n)。這是很不錯的一個效率提升。 通常要求一個數X的Y次方，記做X^Y，樸素的計算方法是把X乘Y次得到這個結果。 而快速冪計算的過程是，對於指數Y進行奇偶性的判斷。為了方便解釋，

    演算法筆記—日期類

      題目連結：http://codeup.cn/problem.php?cid=100000578&pid=3 題目描述 編寫一個日期類，要求按xxxx-xx-xx 的格式輸出日期，實現加一天的操作。 輸入 輸入第一行表示測試用例的個數m，接下來m

    演算法筆記 — 列印日期

    題目連結：http://codeup.cn/problem.php?cid=100000578&pid=2 題目描述 給出年分m和一年中的第n天，算出第n天是幾月幾號。 輸入 輸入包括兩個整數y(1<=y<=3000)，n(1<=n<=366)。

    《演算法筆記》總結一

          目錄 第二章 1. 定義變數時儘可能賦初值，避免在程式時出現未知訪問錯誤！！！（不能想當然的賴編譯器的預設初始化） 1.1C程式的記憶體佈局。 1.2記憶體管理的目的及建議 1.3 note that 2. ASCII碼的程式設計

    演算法筆記_快速冪

    快速冪演算法基於二分法思想 已下給出快速冪遞迴寫法 //求a^b%m,遞迴寫法 long long binaryPow(long long a, long long b, long long m) { if (b == 0) return 1;//如果b為0,那麼a^0=1 //b為