ORB_SLAM2中的疑難雜症

用ORB_SLAM也有一段時間了,基於該project也做了不少的開發,期間遇到了一些bug,在這裏總結一下,在github上的issue中也有,只是issue數量太大,所以總結出一個關於代碼錯誤的幾個方面(主要是在遇到的時候也不敢相信,畢竟是大牛的作品):

1. Reset() 的時候會遇到段錯誤,很偶爾遇到的一個問題,主要原因是雙目的初始化函數StereoInitialization() 中關於mLastFrame的賦值順序出錯,具體分析如下:

Track() 部分在mState=OK時的具體的track方法如下:

if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
     // 將上一幀的位姿作爲當前幀的初始位姿
     // 通過BoW的方式在參考幀中找當前幀特徵點的匹配點
     // 優化每個特徵點都對應3D點重投影誤差即可得到位姿
      bOK = TrackReferenceKeyFrame();
 }
 else
 {
  // 根據恆速模型設定當前幀的初始位姿
  // 通過投影的方式在參考幀中找當前幀特徵點的匹配點
  // 優化每個特徵點所對應3D點的投影誤差即可得到位姿
  bOK = TrackWithMotionModel();
  if(!bOK)
       bOK = TrackReferenceKeyFrame();
 }

TrackReferenceKeyFrame() 應該只在兩種情況下發生:

一種是整個系統剛剛初始化成功,mVelocity.empty();

第二種情況是 剛剛重定位完成  mCurrentFrame.mnId<mnLastRelocFrameId+2, 也就是前兩幀用ReferenceKF進行計算,第三幀開始用motionmodel;

但是這裏有一個問題是Reset() (該問題僅僅適用於雙目), 如果整個系統system運行到一半,這時mVelocity就不爲空,在這個過程中突然將系統reset() , 由於reset() 時並未將mVelocity清空,因此會使用motionmodel進行track, 

而TrackwithMotionModel() 中需要先進行update,也就是函數UpdateLastFrame();部分摘抄如下:

void Tracking::UpdateLastFrame()
{
    // Update pose according to reference keyframe
    KeyFrame* pRef = mLastFrame.mpReferenceKF; //主要是這裏的參考幀
    cv::Mat Tlr = mlRelativeFramePoses.back();
    mLastFrame.SetPose(Tlr*pRef->GetPose());
}

發生段錯誤的地方就是這裏,因爲需要用到mpReferenceKF, 但實際mpReferenceKF卻爲空,這就導致訪問了不存在的內容,也就導致了段錯誤;

具體的修改方法如下:

1. 在Reset函數中添加如下:

mnLastRelocFrameId = 0;
mVelocity = cv::Mat();

也就是reset時同時也將重定位的ID和運動模型清空,這個不難理解,因爲我既然整個系統都已經重新跑了,這兩個對應的參數都應該是初始狀態;這樣保證每次reset() 初始化後可以使用referenceKF進行track;

2. 第二種方法修改StereoInitialization()在中的變量賦值順,改成如下:

 mpLocalMapper->InsertKeyFrame(pKFini);
 //original line:
 mvpLocalKeyFrames.push_back(pKFini);
 mvpLocalMapPoints=mpMap->GetAllMapPoints();
 mpReferenceKF = pKFini;
 mCurrentFrame.mpReferenceKF = pKFini;  // 第5行
        
 mLastFrame = Frame(mCurrentFrame);   //第6行
 mnLastKeyFrameId=mCurrentFrame.mnId;
 mpLastKeyFrame = pKFini;
        
 mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

其實主要是第五行和第六行的順序,改成先確定mCurrentFrame的參考關鍵幀,再將該frame包裝作爲mLastFrame,這樣就保證在UpdateLastFrame() 時 mLastFrame的參考幀不爲空,不會導致段錯誤;

我的代碼中將這兩部分都做了修改,因爲對比雙目和單目的初始化就可以發現,單目初始化過程就是如上的順序;當然,這種順序如果會遇到bug,還請指正;

對應的該問題在github上的issue如下: https://github.com/raulmur/ORB_SLAM2/issues/123

 

 

 

 

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