ORBSLAM2解析(2)——Tracking 線程

主要參考

雙目ORB-SLAM2代碼個人總結(一) -> 非常詳細描述了跟蹤線程
ORB SLAM2源碼解讀 -> 整個系列按各個類(Frame、MapPoint、KeyFrame…)來解讀

Tracking線程

Tracking線程作爲系統的主線程,主要實現輸入的預處理、地圖的初始化、相機位姿估計、局部地圖的跟蹤、關鍵幀的判斷與創建。(注意:文章以RGBD輸入下的定位且建圖模式進行分析
Tracking

輸入的預處理

在圖像每一幀的初始化中,ORBSLAM2中將雙目或者RGB-D相機輸入的數據都轉化成了Stereo Keypoint,即(uL,vL,uR)(u_L, v_L, u_R),這樣後續對於雙目或者RGB-D的處理是幾乎一樣的。
Pre-process input

地圖的初始化

地圖初始化是發生在SLAM系統運行的最開始時候,創建一個能提取足夠特徵點的關鍵幀作爲第一幀,並將第一幀固定,創建特徵點對應的地圖點。
Stereo Initialization

相機位姿的估計(包括重定位)

相機位姿估計應該是整個跟蹤線程最重要的部分了,這裏重點講述下個人的理解。這部分主要分爲三種情況:跟蹤運動模型、跟蹤關鍵幀、運動丟失後重定位。(每部分思維圖就不展開了,不然太長了)

《視覺SLAM十四講》中對於PnP問題的求解是這麼下結論的:在SLAM中,通常做法是先使用P3P/EPnP等方法估計相機位姿,再構建最小二乘優化問題對估計值進行調整(即進行BA)。Tracking線程中對於相機的估計主要分兩步:先對相機的位姿做一個初步的預測,然後再利用BA對相機的位姿進行調整。

跟蹤運動模型、跟蹤關鍵幀、運動丟失後重定位三種情況下對於相機位姿的初步預測是不一樣的,但有了預測的相機位姿後都是進行一次BA對相機位姿進行調整。具體情況如下:

  1. 對於跟蹤運動模型,要求相機的運動速度不爲0,且假設相機是勻速運動的,即每兩幀之間位姿的變化速度是相同的,因此可以通過 當前幀位姿 = 上一幀速度 × 上一幀位姿 來進行預測,具體的那行代碼是mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw) 。然後再將上一幀的地圖點重新投影至當前幀並與當前幀關鍵點匹配,matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR)
  2. 當相機的速度爲0,跟蹤模型的運動假設顯然是不成立了,此時直接假設 當前幀位姿 = 上一幀位姿。此時,如果仍將上一幀的地圖點重新投影至當前幀並與當前幀關鍵點匹配,顯然是不合適。(可以認爲將上一幀位姿直接作爲當前幀位姿這個預測可能存在比較大的誤差,投影匹配的方式只是在一個小範圍的窗口進行搜索匹配,這樣可以節省計算資源,但是需要在預測已經比較準確的情況下。)因此,可以選擇跟蹤上一幀的參考關鍵幀(如果還是匹配上一幀,那麼後續BA優化將不起作用),並使用暴力匹配關鍵幀地圖點與當前幀特徵點的ORB,這過程可以通過詞袋模型的k-d樹結構進行加速,SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches)。此外,對於剛完成重定位的情況,個人認爲需要有比較準確的地圖點與特徵點之間的匹配,因此也使用暴力匹配的方式。
  3. 當相機運動丟失後,不存在上一幀位姿去估計當前幀位姿的可能了,因此使用PnPsolver(EPnP、ICP、RANSAC)求解相機的位姿作爲優化前的預測值。在PnPsolver求解前,對於候選關鍵幀的地圖點與當前幀特徵點匹配使用SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);在首次優化後,如果nGood小於50大於10,還會使用重投影的方式SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100)進行地圖點與特徵點的匹配,然後再次進行優化,直到nGood夠50,即有足夠的匹配數量。

Pose Estimation

局部地圖的跟蹤

在位姿估計的時候,也進行了對上一幀或者參考關鍵幀的地圖點的跟蹤,但是這種情況下當前幀特徵點沒有與其它的地圖點進行匹配。因此,跟蹤局部地圖的目的就是爲了使當前幀的特徵點能與更多的地圖點進行匹配。
Tracking LocalMapping

關鍵幀的判斷與創建

這一步創建的關鍵幀主要是用於後續的局部建圖和迴環檢測。
KeyFrame

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章