TrackWithMotionModel函数

1、理论部分.

假设俩帧之间是匀速运动，根据这个条件来缩小匹配范围，加速匹配减少计算量。

2、代码部分

//带有运动模型的跟踪
bool Tracking::TrackWithMotionModel()
{
    //匹配点大于90%的总数
    ORBmatcher matcher(0.9,true);

    // Update last frame pose according to its reference keyframe
    //根据参考帧更新上一帧
    // Create "visual odometry" points if in Localization Mode
    //创建视觉里程计

    //更新上一帧
    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;
    //如果不是双目，设定阈值为15，是双目阈值为7
    if(mSensor!=System::STEREO)
        th=15;
    else
        th=7;
    //在当前帧与上一帧之间进行匹配，输出匹配的点的数目
    int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);

    // If few matches, uses a wider window search
    //如果匹配数目<20，则清空地图点，扩大阈值（搜索范围）在匹配
    if(nmatches<20)
    {
        fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
        nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);
    }
    //若还是数量 < 20 ,那么判定搜索失败。。
    if(nmatches<20)
        return false;

    // Optimize frame pose with all matches
    //优化当前帧姿
    Optimizer::PoseOptimization(&mCurrentFrame);

    // Discard outliers
    //剔除外点
    int nmatchesMap = 0;
    //遍历当前帧所有点
    for(int i =0; i<mCurrentFrame.N; i++)
    {
        //如果是地图点
        if(mCurrentFrame.mvpMapPoints[i])
        {
            //如果是外点
            if(mCurrentFrame.mvbOutlier[i])
            {
                //pmp相当于一个点到地图点的一个桥梁
                //把这个外点对应的地图点给pmp
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                //清空地图上外点，也就是把该外点设置为空 -1
                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++;
        }
    }    

    if(mbOnlyTracking)
    {
        //如果在纯定位过程中追踪的地图点非常少,那么这里的 mbVO 标志就会置位
        mbVO = nmatchesMap<10;
        return nmatches>20;
    }

    return nmatchesMap>=10;
}