用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