(瞎忙了一陣,碼沒擼太多忙着挖雷,深感慚愧....)
進入正題,上次說完了orb-slam設計的自動地圖初始化,現在從前臺線程tracking繼續講。當orb-slam每接收到一幀新的圖像,都會喚起這個線程。它主要負責每一幀的定位,並且通過motion-only BA對剛剛得到的初始camera pose進行第一次優化。在此基礎之上,我們會確定一個當前幀的local map,這裏是通過當前幀與周圍幀的公視關係確定的。然後,該線程將當前的特徵點投回local map中的其他幀中,通過計算重投影誤差再一次優化camera pose。最後,該線程會通過一系列標準來確定當前幀是否爲關鍵幀,如果是關鍵幀,則喚醒local mapping線程並將當前幀作爲KF傳給它。
有了成功的初始化之後,slam系統每接收新的圖像幀,就可以開始圖中的tracking流程了。ORB1和ORB2在做pose prediction之前都會提取orb特徵,最初版本只支持單目,後面開放了雙目相機以及RGBD相機的接口,加入了相對應的處理機制,這裏不過多討論。在成功提取當前幀特徵的基礎上,ORB2提供了三種模式用來對pose做initial prediction,選擇不同模式在Tracking::Track()開始部分的代碼,注意mbOnlysTracking和mbVO兩個變量,前者爲true代表slam只做定位不建圖,後者爲true代表當前幀是找不到與地圖的match的,只能追蹤temporal visual odometry points。
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
else
{
// System is initialized. Track Frame.
bool bOK; //tracking result record
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
if(!mbOnlyTracking)
{
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
if(mState==OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
CheckReplacedInLastFrame();
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
bOK = TrackReferenceKeyFrame();
}
else
{
bOK = TrackWithMotionModel();
if(!bOK)
bOK = TrackReferenceKeyFrame();
}
}
else
{
bOK = Relocalization();
}
}
else //VO,localmapping not working
{
// Localization Mode: Local Mapping is deactivated
if(mState==LOST) //tracking lost
{
bOK = Relocalization();
}
else
{
if(!mbVO)
// In case of performing only localization, mbVO is true when there are no matches to
// points in the map. Still tracking will continue if there are enough matches with temporal points.
// In that case we are doing visual odometry. The system will try to do relocalization to recover
// "zero-drift" localization to the map.
{
// In last frame we tracked enough MapPoints in the map
if(!mVelocity.empty())
{
bOK = TrackWithMotionModel();
}
else
{
bOK = TrackReferenceKeyFrame();
}
}
else
{
// In last frame we tracked mainly "visual odometry" points.
// We compute two camera poses, one from motion model and one doing relocalization.
// If relocalization is sucessfull we choose that solution, otherwise we retain
// the "visual odometry" solution.
bool bOKMM = false;
bool bOKReloc = false;
vector<MapPoint*> vpMPsMM;
vector<bool> vbOutMM;
cv::Mat TcwMM;
if(!mVelocity.empty())
{
bOKMM = TrackWithMotionModel();
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.mTcw.clone();
}
bOKReloc = Relocalization();
if(bOKMM && !bOKReloc)
{
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
if(mbVO)
{
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
{
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
}
}
}
}
else if(bOKReloc)
{
mbVO = false;
}
bOK = bOKReloc || bOKMM;
}
}
}
1. TrackWithMotionModel():使用恆速模型的假設是攝像頭運動是勻速運動的,利用勻速運動之後的pose來初始化當前幀。若函數返回false證明該方法跟蹤失敗,然後採用trackreferencekeyframe(),同時注意如果一上來獲取不到速度或者是剛剛做了重定位不久,就直接採用跟蹤referenceKF的方法。
2. TrackReferenceKeyFrame():如果恆速模型失效,就開始嘗試和相鄰的關鍵幀來進行匹配,並且可以使用BoW來加速匹配,首先計算當前幀的BoW,並設定初始位姿爲上一幀的位姿,然後根據位姿和BoW詞典來尋找特徵匹配,進一步優化位姿。
3. Relocalization():到全局的地圖中查找匹配幀,計算位姿(solvePnP實現)。
基於前面的工作已經獲得了當前幀的位姿估計值以及一部分特徵點與地圖點的對應。在TrackLocalMap()中,我們用該幀追蹤一個局部地圖以獲取更多的2D-3D點的匹配,這裏用局部地圖而不是之前所有的關鍵幀也是爲了bound complexity。這個局部地圖包含了關鍵幀集合К1和К2,其中К1包含了與當前幀有共視關係的所有關鍵幀, К2包含了К1的每一幀在共視圖中的臨近幀。之後在局部地圖中再次優化當前幀的位姿。ps:在前端線程依靠跟蹤鄰近幀來進行局部優化從而保證快速而準確的追蹤的工作,個人覺得這對後面的關鍵幀選取以及建圖都是有很大的好處的,idea很棒,後面準備單獨寫一章捋一捋這部分的代碼,先挖個坑。
至此在Tracking線程中,當前幀已經歷了至少兩次pose優化。
最後我們要判斷該幀是不是我們想要的關鍵幀,如果是的話需同時滿足如下條件:
- 該幀距離上一次重定位已經過了至少二十幀。
- 距離上一次關鍵幀的插入(喚醒localmapping線程)至少過了二十幀。(同時如果當前幀條件都滿足但發現上一次喚醒的建圖線程還沒停,那麼先把他強制停了。保證關鍵幀的快速插入。)
- 至少跟蹤了五十個特徵點。
- 爲了保證一定的視覺變化,該幀追蹤到的點應少於局部地圖中與該幀共視關係最強幀追蹤到的點數的90%。(雖然到了建圖線程也有刪除KFs的機制,但個人覺得這一條的目的就是防止過早地出現冗餘關鍵幀,讓新的幀包含一定比重的新的信息。這4條組合的策略下插幀已經很快了,所以冗餘的在這個節點該刪就刪唄。)
確定關鍵幀後,我們將其送到建圖線程,維護我們的地圖,其中包括地圖點的添加刪除,以及冗餘關鍵幀的刪除等,這些我們後面繼續說。
(以上全部爲個人學習心得,如有錯誤歡迎評論指正,如有幸被轉發,請標明轉載出處)