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