初始化完成后,對於相機獲取當前圖像mCurrentFrame,通過跟蹤匹配上一幀mLastFrame特征點的方式,可以獲取一個相機位姿的初始值;為了兼顧計算量和跟蹤魯棒性,處理了三種模型:
1. TrackWithMotionModel
2. TrackReferenceKeyFrame
3. Relocalization
這三種跟蹤模型都是為了獲取相機位姿一個粗略的初值,后面會通過跟蹤局部地圖TrackLocalMap對位姿進行BundleAdjustment(捆集調整),進一步優化位姿。
優先選擇通過恆速運動模型,從LastFrame直接預測出(乘以一個固定的位姿變換矩陣)當前幀的姿態;如果是靜止狀態或者運動模型匹配失效(運用恆速模型后反投影發現LastFrame的地圖點和CurrentFrame的特征點匹配很少),通過增大參考幀的地圖點反投影匹配范圍,獲取較多匹配后,計算當前位姿;若這兩者均失敗,即代表tracking失敗,mState!=OK,則在KeyFrameDataBase中用Bow搜索CurrentFrame的特征點匹配,進行全局重定位GlobalRelocalization,在RANSAC框架下使用EPnP求解當前位姿。這三種跟蹤模式僅僅跟蹤了一幀圖像中的特征,沒有全局的信息,因此獲取的位姿誤差較大。
一旦我們通過上面三種模型獲取了初始的相機位姿和初始的特征匹配,就可以將完整的地圖投影到當前幀中去搜索更多的匹配。但是投影完整的地圖,在large scale的場景中是很耗計算而且也沒有必要的,因此,這里使用了局部地圖LocalMap來進行投影匹配。
LocalMap包含:與當前幀相連的關鍵幀K1,以及與K1相連的關鍵幀K2(一級二級相連關鍵幀);K1、K2對應的地圖點;參考關鍵幀Kf。
匹配過程如下:
1. 拋棄投影范圍超出相機畫面的;
2. 拋棄觀測視角和地圖點平均觀測方向相差60o以上的;
3. 拋棄特征點的尺度和地圖點的尺度(通過高斯金字塔層數表示)不匹配的;
4. 計算當前幀中特征點的尺度;
5. 將地圖點的描述子和當前幀ORB特征的描述子匹配,需要根據地圖點尺度在初始位姿獲取的粗略x投影位置附近搜索;
6. 根據所有匹配點進行PoseOptimization優化。
在處理完mCurrentFrame的跟蹤定位后,需要判定當前幀是否可以加入關鍵幀list:
以下條件必須同時滿足,才可以加入關鍵幀,但是其實ORB-SLAM中關鍵幀的加入是比較密集的,這樣確保了定位的精度,同時在LocalMapping線程最后會進行關鍵幀的剔除,又確保了關鍵幀的數量不會無限增加,不會對large scale的場景造成計算負擔。但是,ORB的作者又提到了,Tracking中除了提取特征點,TrackLocalMap也挺耗時,可以通過減少關鍵幀的數量來降低Local Map的規模,提高Tracking速度(但是精確度可能降低)。
1. 距離上一次重定位距離至少20幀;
2. 局部地圖線程空閑,或者距離上一次加入關鍵幀過去了20幀;如果需要關鍵幀插入(過了20幀)而LocalMapping線程忙,則發送信號給LocalMapping線程,停止局部地圖優化,使得新的關鍵幀可以被及時處理(20幀,大概過去了不到1s)。
3. 當前幀跟蹤至少50個點;確保了跟蹤定位的精確度
4. 當前幀跟蹤到LocalMap中參考幀的地圖點數量少於90%;確保關鍵幀之間有明顯的視覺變化。
注意這里只是判斷當前幀是否需要加入關鍵幀,並沒有真的加入地圖,因為Tracking線程的主要功能是局部定位,而處理地圖中的關鍵幀,地圖點,包括如何加入,如何刪除的工作是在LocalMapping線程完成的,這里也可以看出作者的思路是比較清楚的,Tracking負責localization,LocalMapping負責Mapping,就構建了粗略的完整SLAM框架,然后加入初始化和閉環檢測以及一些可視化模塊,形成完整的SLAM。
PTAM中關鍵幀的插入僅僅依靠關鍵幀之間的距離,明顯遜於ORB-SLAM的適者生存方法(survival of the fittest)。
對於RGBD和雙目,非關鍵幀均可以恢復出圖像點的深度,,可以創建一些臨時地圖點,用於普通幀之間跟蹤,主要用於恆速運動模型,這些臨時地圖點僅僅用於MotionModel跟蹤,不會加入地圖或者關鍵幀中。運動過程中不斷會跟丟地圖點,因此需要不斷補充。
void Tracking::UpdateLastFrame();
然后根據恆速模型估計當前幀位姿
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
使用勻速模型估計的位姿,將LastFrame中臨時地圖點投影到當前姿態,在投影點附近根據描述子距離進行匹配(需要>20對匹配,否則勻速模型跟蹤失敗,運動變化太大時會出現這種情況),然后以運動模型預測的位姿為初值,優化當前位姿,優化完成后再剔除外點,若剩余的匹配依然>=10對,則跟蹤成功,否則跟蹤失敗,需要Relocalization:

bool Tracking::TrackWithMotionModel() { ORBmatcher matcher(0.9,true); // Update last frame pose according to its reference keyframe // Create "visual odometry" points if in Localization Mode UpdateLastFrame(); mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw); fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL)); // Project points seen in previous frame int th; if(mSensor!=System::STEREO) th=15; else th=7; int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR); // If few matches, uses a wider window search if(nmatches<20) { fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL)); nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); } if(nmatches<20) return false; // Optimize frame pose with all matches Optimizer::PoseOptimization(&mCurrentFrame); // Discard outliers int nmatchesMap = 0; for(int i =0; i<mCurrentFrame.N; i++) { if(mCurrentFrame.mvpMapPoints[i]) { if(mCurrentFrame.mvbOutlier[i]) { MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL); mCurrentFrame.mvbOutlier[i]=false; pMP->mbTrackInView = false; pMP->mnLastFrameSeen = mCurrentFrame.mnId; nmatches--; } else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) nmatchesMap++; } } if(mbOnlyTracking) { mbVO = nmatchesMap<10; return nmatches>20; } return nmatchesMap>=10; }
以上兩種僅僅完成了視覺里程計中的幀間跟蹤,還需要進行局部地圖的跟蹤,提高精度:(這其實是Local Mapping線程中干的事情)
bool Tracking::TrackLocalMap()
其中會完成局部地圖的更新,以及局部地圖點的更新:
UpdateLocalMap();
SearchLocalPoints();
BA優化后,統計符合局部先驗信息的mnMatchesInliers內點數,判斷是否成功,如果最近剛剛做過Relocalization,則要求的內點數多一些。
重定位Relocalization的過程大概是這樣的:
1. 計算當前幀的BoW映射;
2. 在關鍵幀數據庫中找到相似的候選關鍵幀;
3. 通過BoW匹配當前幀和每一個候選關鍵幀,如果匹配數足夠,進行EPnP求解;
4. 對求解結果使用BA優化,如果內點較少,則反投影當前幀的地圖點到候選關鍵幀獲取額外的匹配點;若這樣依然不夠,放棄該候選關鍵幀,若足夠,則將通過反投影獲取的額外地圖點加入,再進行優化。
5. 如果內點滿足要求(>50)則成功重定位,將最新重定位的id更新:mnLastRelocFrameId = mCurrentFrame.mnId; 否則返回false。
這三種模式可以一定程度上保證短期的VO不會跟丟;下面還需要對局部地圖進行跟蹤,以及選擇插入關鍵幀過程。
局部地圖跟蹤TrackLocalMap()中需要首先對局部地圖進行更新(UpdateLocalMap),並且搜索局部地圖點(SearchLocalPoint)。局部地圖的更新又分為局部地圖點(UpdateLocalPoints)和局部關鍵幀(UpdateLocalKeyFrames)的更新:

bool Tracking::TrackLocalMap() { // We have an estimation of the camera pose and some map points tracked in the frame. // We retrieve the local map and try to find matches to points in the local map. UpdateLocalMap(); SearchLocalPoints(); // Optimize Pose Optimizer::PoseOptimization(&mCurrentFrame); mnMatchesInliers = 0; // Update MapPoints Statistics for(int i=0; i<mCurrentFrame.N; i++) { if(mCurrentFrame.mvpMapPoints[i]) { if(!mCurrentFrame.mvbOutlier[i]) { mCurrentFrame.mvpMapPoints[i]->IncreaseFound(); if(!mbOnlyTracking) { if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) mnMatchesInliers++; } else mnMatchesInliers++; } else if(mSensor==System::STEREO) mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL); } } // Decide if the tracking was succesful // More restrictive if there was a relocalization recently if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50) return false; if(mnMatchesInliers<30) return false; else return true; }
局部地圖點的更新比較容易,完全根據局部關鍵幀來,所有局部關鍵幀的地圖點就構成局部地圖點;因此,關鍵在於如何去選擇當前幀對應的局部關鍵幀。
第一部分是所有能觀察到當前幀對應地圖點的的關鍵幀;
當關鍵幀數量較少時(<=80),考慮加入第二部分關鍵幀,是與第一部分關鍵幀聯系緊密的關鍵幀,並且始終限制關鍵數量不超過80。聯系緊密體現在三類:1. 共視化程度高的關鍵幀;2. 子關鍵幀;3. 父關鍵幀。
還有一個關鍵的問題是:如何判斷該幀是否關鍵幀,以及如何將該幀轉換成關鍵幀?調用NeedNewKeyFrame()和CreateNewKeyFrame()兩個函數來完成。