ORB-SLAM2 ---- Tracking::TrackWithMotionModel函数

目录

1.函数作用

2.步骤 

3.code 

4.函数解释 

4.1 更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点

4.2 根据之前估计的速度，用恒速模型得到当前帧的初始位姿。

4.3 用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次 

4.4 利用3D-2D投影关系，优化当前帧位姿 

4.5 剔除地图点中外点

---

1.函数作用

        用最近的普通帧来跟踪当前的普通帧。根据恒速模型设定当前帧的初始位姿，通过投影的方式在参考帧中找当前帧特征点的匹配点，优化每个特征点所对应3D点的投影误差即可得到位姿。

2.步骤 

 * @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪
 * Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
 * Step 2：根据上一帧特征点对应地图点进行投影匹配
 * Step 3：优化当前帧位姿
 * Step 4：剔除地图点中外点
 * @return 如果匹配数大于10，认为跟踪成功，返回true

3.code 

bool Tracking::TrackWithMotionModel()
{
    // 最小距离 < 0.9*次小距离 匹配成功，检查旋转
    ORBmatcher matcher(0.9,true);
 
    // Update last frame pose according to its reference keyframe
    // Create "visual odometry" points
    // Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
    UpdateLastFrame();
 
    // Step 2：根据之前估计的速度，用恒速模型得到当前帧的初始位姿。
    mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
    
    // 清空当前帧的地图点
    fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL));
 
    // Project points seen in previous frame
    // 设置特征匹配过程中的搜索半径
    int th;
    if(mSensor!=System::STEREO)
        th=15;//单目
    else
        th=7;//双目
 
    // Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次
    int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
 
    // If few matches, uses a wider window search
    // 如果匹配点太少，则扩大搜索半径再来一次
    if(nmatches<20)
    {
        fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL));
        nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th
    }
 
    // 如果还是不能够获得足够的匹配点,那么就认为跟踪失败
    if(nmatches<20)
        return false;
 
    // Optimize frame pose with all matches
    // Step 4：利用3D-2D投影关系，优化当前帧位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
 
    // Discard outliers
    // Step 5：剔除地图点中外点
    int nmatchesMap = 0;
    for(int i =0; i
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            if(mCurrentFrame.mvbOutlier[i])
            {
                // 如果优化后判断某个地图点是外点，清除它的所有关系
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
 
                mCurrentFrame.mvpMapPoints[i]=static_cast(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;
    }
 
    // Step 6：匹配超过10个点就认为跟踪成功
    return nmatchesMap>=10;
}

4.函数解释 

4.1 更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点

ORB-SLAM2 ---- Tracking::UpdateLastFrame函数解析https://blog.csdn.net/qq_41694024/article/details/128192577        通过这一步，我们计算了LastFrame的位姿并且对于双目或RGB-D相机来说，我们新增了一些地图点。

4.2 根据之前估计的速度，用恒速模型得到当前帧的初始位姿。

         当前帧本身是没有位姿的，恒速模型设置当前帧的位姿为速度mVelocity * 上一帧的位姿得到当前帧的初始位姿。

        我们看速度mVelocity 的更新：

                // 更新恒速运动模型 TrackWithMotionModel 中的mVelocity
                cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
                mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
                mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
                // mVelocity = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc
                mVelocity = mCurrentFrame.mTcw*LastTwc; 
            }

        它其实是上一帧到当前帧的变换，它不断在更新，因此“上一帧到当前帧的变换”mVelocity （lastFrame的上一帧到lastFrame的变换） * 上一帧的位姿（lastFrame的位姿），即我们假设相同两帧的变化幅度不大，用LastFrame的LastFrame到LastFrame的变换来代替LastFrame到CurrentFrame的变化矩阵，但这仅仅是个初始值。

        随后清空当前帧CurrentFrame的地图点，根据相机类型选择匹配半径。

4.3 用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次 

        我们用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次。

        如果匹配点太少，则扩大搜索半径再来一次。

        如果还是不能够获得足够的匹配点，那么就认为跟踪失败。

4.4 利用3D-2D投影关系，优化当前帧位姿 

4.5 剔除地图点中外点

        如果我们经过优化后确定当前地图点是外点，则清除它的所有关系：

        ①将当前帧的地地图点mvpMapPoints[i]的内存清空

        ②设置当前地图点的外点的标记mvbOutlier为NULL。属于外点的特征点标记，在 Optimizer::PoseOptimization 使用了。

        ③将标记mbTrackInView设置为false

mbTrackInView==false的点有几种：
 a 已经和当前帧经过匹配（TrackReferenceKeyFrame，TrackWithMotionModel）但在优化过程中认为是外点
b 已经和当前帧经过匹配且为内点，这类点也不需要再进行投影 
c 不在当前相机视野中的点（即未通过isInFrustum判断）

        ④将标记mnLastFrameSeen设置为false

mnLastFrameSeen==mCurrentFrame.mnId的点有几种：
a 已经和当前帧经过匹配（TrackReferenceKeyFrame，TrackWithMotionModel）但在优化过程中认为是外点
b 已经和当前帧经过匹配且为内点，这类点也不需要再进行投影