ORB_SLAM2中Tracking线程的三种追踪方式

1、参考关键帧追踪模式

bool Tracking::TrackReferenceKeyFrame()

  对参考关键帧中的路标点进行跟踪。在Tracking线程中，每传入一帧，都会进行位姿优化。
  以上一帧的位姿为当前位姿进行优化。
（1）计算当前帧的词袋

mCurrentFrame.ComputeBoW();

（2）通过词袋加速算法，获得当前帧的特征点与参考帧的路标点间的联系。当匹配数小于15时，退出。

    // Step 2：通过特征点的BoW加快当前帧与参考帧之间的特征点匹配
    // 特征点的匹配关系由MapPoints进行维护
    int nmatches = matcher.SearchByBoW(
        mpReferenceKF,          //参考关键帧
        mCurrentFrame,          //当前帧
        vpMapPointMatches);     //存储匹配关系

    //这里的匹配数超过15才继续
    if(nmatches<15)
        return false;

（3）以上一帧的位姿为当前位姿，通过重投影误差进行优化

mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些
// Step 4:通过优化3D-2D的重投影误差来获得位姿
 Optimizer::PoseOptimization(&mCurrentFrame);

（4）删除外点
  根据BA优化的结果，判断那些点是外点。如果是外点的话，就删除其痕迹：路标点的指针置为NULL等。如果是内点的话，计数器加一。

	int nmatchesMap = 0;
    for(int i =0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            //如果对应到的某个特征点是外点
            if(mCurrentFrame.mvbOutlier[i])
            {
                //清除它在当前帧中存在过的痕迹
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];

                mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                mCurrentFrame.mvbOutlier[i]=false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                //匹配的内点计数++
                nmatchesMap++;
        }
    }

（4）如果内点的数目大于10的话，返回追踪成功。此时当前帧的位姿已经被优化。

2、恒速运动模型

bool Tracking::TrackWithMotionModel

 &emps;根据前两帧的运动估计当前帧的位姿，作为优化初值。

（1）更新上一帧位姿。如果是双目、RGB-D相机的话，还会将未创建为地图点的三维点（双目相机可获得深度信息）作为临时地图点加入到地图中。

UpdateLastFrame();

（2）根据恒速运动模型获得当前帧的位姿。mVelocity为估计的上一帧到当前帧的位姿变换Tcl。Tcl*Tlw=Tcw

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

（3）根据上一帧特征点对应地图点进行投影匹配，返回匹配上的个数。
（4）如果匹配个数不够的话，扩大搜索半径。如果匹配个数（特征点-特征点的联系、特征点-路标点的联系含义是一样的）还不够，直接返回。
（4）仅优化位姿。
（5）去除外点，根据剩余的匹配数量，返回是否匹配成功。追踪模式的要求更加严格（个人认为是追踪模式只有位姿约束，因此需要更多的位姿进行约束。）。
 
 

重定位（最后的拯救措施）

  以PNP估计结果作为优化初值。

（1）使用词袋模型找到当前帧的相似关键帧集合

    mCurrentFrame.ComputeBoW();

    // Relocalization is performed when tracking is lost
    // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
    // Step 2：找到与当前帧相似的候选关键帧组
    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
    
    // 如果没有候选关键帧，则退出
    if(vpCandidateKFs.empty())
        return false;

（2）使用ORB特征点检测候选关键帧，对合适的关键帧进行优化
vvpMapPointMatches：存放当前帧对每个关键帧成功匹配上的路标点的数量。
vbDiscarded：某个候选关键帧是否要放弃。
1）使用词袋快速匹配，匹配成功数量小于15的话，直接放弃

            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
            // 如果和当前帧的匹配数小于15,那么只能放弃这个关键帧
            if(nmatches<15)
            {
                vbDiscarded[i] = true;
                continue;
            }

2）对匹配的路标点数目大于15的每个候选关键帧，设置pnp求解参数

				// 参数为当前帧、当前帧的特征点索引---->路标点
                PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                pSolver->SetRansacParameters(
                    0.99,   //用于计算RANSAC迭代次数理论值的概率
                    10,     //最小内点数, 但是要注意在程序中实际上是min(给定最小内点数,最小集,内点数理论值),不一定使用这个
                    300,    //最大迭代次数
                    4,      //最小集(求解这个问题在一次采样中所需要采样的最少的点的个数,对于Sim3是3,EPnP是4),参与到最小内点数的确定过程中
                    0.5,    //这个是表示(最小内点数/样本总数);实际上的RANSAC正常退出的时候所需要的最小内点数其实是根据这个量来计算得到的
                    5.991); // 自由度为2的卡方检验的阈值,程序中还会根据特征点所在的图层对这个阈值进行缩放
                vpPnPsolvers[i] = pSolver;
                nCandidates++;

（3）遍历候选关键帧，进行RANSAC+EPNP迭代估计相机位姿。

            // Step 4.1：通过EPnP算法估计姿态，迭代5次
            PnPsolver* pSolver = vpPnPsolvers[i];
            // 获得初步估计的相机位姿
            cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

（4）对每个候选关键帧的位姿进行优化

int nGood = Optimizer::PoseOptimization(&mCurrentFrame);

（5）如果匹配到的内点数目不够，则将参考关键帧中的地图点投影至当前帧中，形成新的匹配。如果够了的话，直接结束循环。

  之前是通过词袋加速算法获得了当前帧中的匹配到的路标点，会有遗漏。这里将参考关键帧中的所有路标点再次进行投影。
（6）再次进行优化

nGood = Optimizer::PoseOptimization(&mCurrentFrame);

（7）如果不行的话，再用严格的标准投影一遍

                        if(nGood>30 && nGood<50)
                        {
                            // 用更小窗口、更严格的描述子阈值，重新进行投影搜索匹配
                            sFound.clear();
                            for(int ip =0; ip<mCurrentFrame.N; ip++)
                                if(mCurrentFrame.mvpMapPoints[ip])
                                    sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
                            nadditional =matcher2.SearchByProjection(
                                mCurrentFrame,          //当前帧
                                vpCandidateKFs[i],      //候选的关键帧
                                sFound,                 //已经找到的地图点，不会用于PNP
                                3,                      //新的窗口阈值，会乘以金字塔尺度
                                64);                    //匹配的ORB描述子距离应该小于这个阈值

                            // Final optimization
                            // 如果成功挽救回来，匹配数目达到要求，最后BA优化一下
                            if(nGood+nadditional>=50)
                            {
                                nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                                //更新地图点
                                for(int io =0; io<mCurrentFrame.N; io++)
                                    if(mCurrentFrame.mvbOutlier[io])
                                        mCurrentFrame.mvpMapPoints[io]=NULL;
                            }
                            //如果还是不能够满足就放弃了
                        }

（8）如果还行，只能放弃