用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