ORB-SLAM2初步（Tracking.cpp）

　　今天主要是分析一下Tracking.cpp这个文件，它是实现跟踪过程的主要文件，这里主要针对单目，并且只是截取了部分代码片段。

一、跟踪过程分析

1. 首先构造函数中使用初始化列表对跟踪状态mState(NO_IMAGES_YET), 传感器类型mSensor(sensor), 是否只进行定位mbOnlyTracking(false)等变量进行了初始化（注意：一些const关键字或者指针类的变量只能使用初始化列表进行初始化），同时在构造函数的函数体内通过OpenCV的FileStorage从文件中读取了Camera标定参数，ORB特征提取的相关参数等，并用其对相关变量进行了初始化。（由于这一段比较简单，很容易看懂，就不再赘述。）
2. Tracking.cpp文件的调用接口函数是：

   cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
   

   　　它完成了对一帧的初始化，并转入Track过程：

   if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
           mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
       else
           mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
   
       Track();

3. Track（）实现了跟踪的主要逻辑过程：（1）第一步首先就是判断是否进行了初始化，关于初始化先暂且不表，只知道它通过调用一个初始化函数进行了初始化：

   if(mState==NOT_INITIALIZED)
       {
           //根据双目或RGBD或单目分别进行初始化，调用不同的函数；
           if(mSensor==System::STEREO || mSensor==System::RGBD)
               StereoInitialization();
           else
               MonocularInitialization();
   
           mpFrameDrawer->Update(this);
   
           //如何初始化没有完成，需要返回重新进入线程进行初始化，成功了继续执行；
           if(mState!=OK)
               return;
       }

   （2）接下来也是判断，这里是同时跟踪和建图的跟踪过程，其实只有跟踪的时候也很有意思：

   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();
   
                   //判定速度是否为空，是则根据参考帧进行跟踪，否则根据运动模型进行跟踪，或者距离上一次重定位过去少于2帧；
                   if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+)
                   {
                       bOK = TrackReferenceKeyFrame();
                   }
                   else
                   {
                       bOK = TrackWithMotionModel();
                       //如果运动模型跟踪失败，使用参考帧模型进行跟踪；
                       if(!bOK)
                           bOK = TrackReferenceKeyFrame();
                   }
               }
               //如果跟踪状态为丢失，则使用重定位找回当前相机的位姿；
               else
               {
                   bOK = Relocalization();
               }
           }

   （3）到这里三种跟踪模型都已经出现了，它们分别是：运动模型、参考帧模型、重定位，这里先暂时跳过，后面单独分析这三种模型

   TrackWithMotionModel();
   TrackReferenceKeyFrame();
   Relocalization()

   从上面的过程可以看出，如果初始化成功或上一帧的状态为成功(mState==OK)，同时速度矩阵非空（mVelocity.empty()），则优先使用运动模型进行跟踪（从后面的分析可以看出这种跟踪方式速度最快），否则使用参考帧模型进行跟踪，如果上一帧跟踪状态为失败，就需要直接进行重定位找回相机位姿。

4. 假设不管使用哪种方法，跟踪状态显示成功了，同时返回了一个初始的相机位姿，下面就是要进行局部地图的跟踪过程，可以理解为三种模型获取一个初始相机位姿，然后使用跟踪局部地图的方式对位姿进行优化：

   bOK = TrackLocalMap();

   我认为只要前面三种模型跟踪成功了，对局部地图的跟踪就会成功，所以这里bOK的状态不会改变（没有考虑其它特殊情况），其结果相当于获得了一个相对精确的相机位姿。

5. 下面就是一些扫尾工作，如果不知道运动模型是什么一种跟踪模型，那对前面的速度矩阵肯定很感兴趣了：

   if(!mLastFrame.mTcw.empty())
               {
                   cv::Mat LastTwc = cv::Mat::eye(,,CV_32F);
                   mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(,).colRange(,));
                   mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(,).col());
                   //这里的速度矩阵存储的具体内容是当前帧的位姿乘以上一帧的位姿；
                   mVelocity = mCurrentFrame.mTcw*LastTwc;
               }
               else
                   //把速度矩阵设置为空。
                   mVelocity = cv::Mat();

   结合后面的运动模型可以得知，它假设的是相机运动速度不变（就是假设下一帧的位姿矩阵和这一帧一样，但是相机的位置是不一样的）：

   //更新当前帧的位姿：速度乘以上一帧的位姿；
       mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

   其它的工作包括判断是否插入关键帧，删除一些外点，把当前帧置位上一帧等，如果为跟丢，且关键帧总数小于5（初始化不久就丢了），则需要进行重置。

二、子函数分析

1. 首先是运动模型：

   //设置匹配器；
   ORBmatcher matcher(0.9,true);
   //更新上一帧信息，对单目只更新了相机位姿；
   UpdateLastFrame();
   //更新当前帧的位姿：速度乘以上一帧的位姿；
   mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
   fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
   
   // Project points seen in previous frame
   int th;
   if(mSensor!=System::STEREO)
       th=;
   else
       th=;
   int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
   
   //如果匹配数小于20，则扩大搜索范围；
   if(nmatches<)
   {
       fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
       nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,*th,mSensor==System::MONOCULAR);
   }
   
   //如果还是匹配数小于20，则判定运动模型跟踪失败；
   if(nmatches<)
       return false;
   
   //如果匹配数大于20了，就优化相机位姿；
   // Optimize frame pose with all matches
   Optimizer::PoseOptimization(&mCurrentFrame);

   后面就是根据不同情况对跟踪结果进行返回，还有当前帧特征中的地图点的判定等。

2. 参考帧模型：

   //计算当前帧的Bow向量
   mCurrentFrame.ComputeBoW();
   //设定匹配器；
   ORBmatcher matcher(0.7,true);
   vector<MapPoint*> vpMapPointMatches;
   
   //统计当前帧和参考关键帧之间匹配点数，使用BoW加速匹配过程；
   int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
   //这里将上一帧的位姿赋给了当前帧
   mCurrentFrame.SetPose(mLastFrame.mTcw);

   //优化当前位姿；
     Optimizer::PoseOptimization(&mCurrentFrame);

   后面的过程跟运动模型类似。但是这里直接将上一帧位姿作为初值进行优化，并没有使用PnP求解，保留疑问。

3. 重定位：

   //计算BoW向量；
       mCurrentFrame.ComputeBoW();
       //在关键帧数据库中搜索当前帧的候选关键帧；
       vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
       if(vpCandidateKFs.empty())
           return false;
       const int nKFs = vpCandidateKFs.size();
       //设置匹配器；
       ORBmatcher matcher(0.75,true);
   
       vector<PnPsolver*> vpPnPsolvers;
       vpPnPsolvers.resize(nKFs);
   
       vector<vector<MapPoint*> > vvpMapPointMatches;
       vvpMapPointMatches.resize(nKFs);
   
       vector<bool> vbDiscarded;
       vbDiscarded.resize(nKFs);
   
       int nCandidates=;
   
       //对每一个关键帧进行PnP求解；
       //首先进行BoW匹配，匹配达到15个点的就进行进一步求解，并作为候选关键帧；
       for(int i=; i<nKFs; i++)
       {
           KeyFrame* pKF = vpCandidateKFs[i];
           if(pKF->isBad())
               vbDiscarded[i] = true;
           else
           {
               int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
               if(nmatches<)
               {
                   vbDiscarded[i] = true;
                   continue;
               }
               else
               {
                   PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                   pSolver->SetRansacParameters(0.99,,,,0.5,5.991);
                   vpPnPsolvers[i] = pSolver;
                   nCandidates++;
               }
           }
       }
       //再后面一个大循环是上面的匹配结果进行优化求精，进行RANSAC迭代，如果有足够多的内点则跳出循环并返回

   重定位的过程至少在逻辑上是很好理解的，就是在已经生成的关键帧数据库中搜索看看当前帧和谁最相近，而方法就是先用BoW匹配，然后进行PnP求解，最后使用RANSAC迭代。

4. 还有一个重要的函数，就是局部地图跟踪：

   //更新局部地图，包括关键帧和地图点的更新；
       //它的主要作用就是通过关键帧和地图点的共视关系更新这两个变量：mvpLocalKeyFrames，mvpLocalMapPoints，它们中存储着局部地图中的关键帧和地图点；
       UpdateLocalMap();
       //这个函数就是搜索一下mvpLocalMapPoints中的点是否符合跟踪要求，并匹配当前帧和局部地图点；
       SearchLocalPoints();
       //然后就是优化位姿，需要注意的是，无论是匹配过程还是优化过程都会对地图点做一些修改
       Optimizer::PoseOptimization(&mCurrentFrame);
       //后面就是根据匹配和优化结果更新地图点的状态，并判断匹配的内点数量，最后返回

   实际上局部地图匹配的过程要比上面几行代码复杂的多，基本思想就是通过共视关系找出局部地图的关键帧和局部地图点，并用当前帧与之进行匹配优化。

5. 还有一些子函数，如判断是否插入关键帧，跟踪的重置，初始化过程，以及只进行跟踪不建图的跟踪过程等，以后有机会再说吧。

三、总结

　　通过梳理跟踪过程的代码，对ORB-SLAM的跟踪过程的算法和代码都有了更深入的理解，也体会到了写一个类似工程的知识量和工作量。

从前面的分析知道跟踪过程的时间消耗主要在局部地图的跟踪过程，提高效率的方法之一应该就是相应的减小局部地图的大小，具体来说就是mvpLocalKeyFrames，mvpLocalMapPoints这两个变量的大小，现在还没遇到这个问题，所以没有做相应实验。