[人工智能](01)ORB-SLAM2源码无死角解析-(34) 跟踪线程→恒速模型跟踪当前普通帧TrackWithMotionModel()

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析-接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

有兴趣的朋友可以加微信 17575010159 相互讨论技术 - 文末公众号也可关注

?

一、前言

在上一篇博客中，讲解了如何通过关键帧来估算当前帧的位姿，但是其最重要的核心函数 Optimizer::PoseOptimization(&mCurrentFrame) 没有进行讲解，这个知识点暂且搁置一下，先来看看恒速模型跟踪普通帧是如何实现的，恒速模型跟踪也叫做视觉里程计(不是很明白的朋友可以百度一下)。使用这种方式追踪呢，其假设相机移动的速度是均匀的，进一步利用微分的实现，就是上一帧变换的位姿与当前帧变换位姿是相同(或者说接近相同)。

如果两帧之间的速度(位姿变化)大致相同，另外两帧之间的时间间隔是比较小，一般为几十毫秒， 这里假设连续的三帧分别为 $f1，f2，f3$。基于前面的推论，$f1$ 到 $f2$，与 $f2$ 到 $f3$ 的位姿变换大致相同即 $R_{21}\approx R_{32}$, $t_{21}\approx t_{32}$(注意,这里使用的是约等于)。如果已知 $R_{21}$，先令 $R_{21}=R_{32}$，$t_{21}=t_{32}$。那么 $f_2$ 的一个特征点 $p_2$ 根据 $R_{32},t_{32}$ 就能映射到 $f3$ 中为 $p_3^{\prime }$。这里就需要注意了，通常情况 $p_2$ 与 $p_3^{\prime }$ 不是匹配的特征点对，但是与 $p_2$ 匹配的特征代 $p_3$ 肯定就在 $p_3^{\prime }$ 的附近。根据这里原理，对 $p_3^{\prime }$ 周边的像素进行搜索，找到与 $p_2$ 匹配的特征点 $p_3$。

或者这样说大家还不是很明确,那么请看图示如下：

首先根据上上帧到上一帧的位姿，把上一帧的特征点像素坐标映射到当前帧，然后在映射后结果坐标周围进行搜索，找到与之匹配的特征点。

?

二、代码流程

通过 一、前言 对 恒速模型跟踪当前普通帧 进行了简单的原理介绍，代码为src/Tracking.cc中的Tracking::UpdateLastFrame() 函数，下面是其实现流程:

$(01):$ 调用 UpdateLastFrame() 函数更新上一帧的位姿，源码运行的过程中保存关键帧于mpReferenceKF变量中，但是并没有把所有视频帧都保存于缓存中。通过追踪函数 TrackReferenceKeyFrame(), TrackWithMotionModel() 或者 Relocalization() 计算出位姿之后，其并没有马上赋值，而运行到下一帧才对当前帧的位姿进行赋值。另外 TrackReferenceKeyFrame() 函数并没有调用 UpdateLastFrame(), 因为只有在刚完成初始化，或者恒速跟踪失败，才进行参考帧跟踪。
①如果刚初始化完成，则上一帧为关键帧，关键帧已经存储位姿，所以不需要更新。
②如果参考关键帧跟踪，则说明恒速跟踪失败，也就是上一帧已经没有了意义，又源码中不存储非关键帧，故不必对上一帧位姿进行更新。

$(02):$ 根据之前估算的速度(或者说位姿变换)，用恒速模型得到当前帧的初始位姿。代表速度的变量为 mVelocity。该变量在 Tracking::Track() 函数中被赋值。如果当前帧追踪成功，那么速度被赋值为 mVelocity = mCurrentFrame.mTcw*LastTwc。其中mCurrentFrame.mTcw相机坐标系变换到当前帧坐标系(弈可以理解为当前帧相机在世界坐标系下的位姿)，LastTwc表示上一帧世界相机坐标系到世界坐标的变换矩阵。所以mCurrentFrame.mTcw*LastTwc 表示上一帧相机位姿到当前帧位姿的变换矩阵。

$(03):$ 设置特征匹配的搜索半径(以像素为单位)，因为单目相机存在尺度漂移，所以搜索范围会大一些。然后利用重投影进行匹配，如果没有匹配点不够则扩大搜索路径重来一次。通过重投影进行特征点匹配的函数为SearchByProjection(),稍等会进行详细讲解。

$(04):$ 利用3D-2D投影关系，优化当前帧位姿。其核心为代码为 Optimizer::PoseOptimization(&mCurrentFrame);该函数比较重要，在上一章节已经提及到，该篇博客不进行讲解，后面再为大家进行分析。

$(05):$ 该步骤十分简单，主要就是根据判断地图点是否为外点，如果为外点，则清除他的所有关系。普配数目也同时更新。如果追踪匹配地图点超过10个则认为匹配成功。另外，纯定位模式下，如果追踪地图点非常少，那么 mbVO 标志就会定位。

其上 UpdateLastFrame() 与 SearchByProjection() 两个函数都是比较复杂。下面首先针对这两个函数进行讲解，然后再对TrackWithMotionModel() 进行整体的讲解(后面有代码注解)。

?

三、UpdateLastFrame()

该函数的实现位于 src/Tracking.cc 文件中，实现逻辑如下：
$(01):$ Tracking::Track() 函数中:①如果当前帧的位姿不为空(也就是说明当前帧被跟踪到)，那么就计算当前帧的参考帧到当前帧位姿变换，对应的代码为:

cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();

计算相对姿态 $Tcr=Tcw?Twr,Twr=Tr{w}^{?1}$。 $Tcr$ 表示参考帧到当前帧的变换矩阵，然后赋值给成员变量 mlRelativeFramePoses。②如果当前帧的位姿为空(也就是说明当前帧没有跟踪到)，那么 mlRelativeFramePoses 使用上一帧的值。
这样也就是说，知道了每一帧的关键帧到该帧的变换矩阵，那么在 UpdateLastFrame() 函数中的 cv::Mat Tlr 变量表示参考帧到上一阵的变换矩阵。结合关键帧在世界坐标系下的位姿，就能求解出上一帧在世界坐标系下的位姿。对应代码为 mLastFrame.SetPose(Tlr*pRef->GetPose())。这样就对上一帧的位姿进行了更新。

$(02):$ 如果上一阵为关键帧，或者是单目的情况，则退出该函数。因为针对双目或rgbd相机，还会为上一帧生成临时地图点(普通帧生成的地图点使用过后会被删除掉)。生成过程如下：获得到上一帧的所有特征点(不一定是地图点)，循环判断该点的深度是否有效，对有效特征点依照深度进行排序。然后再次进入循环，如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点，地图点被创建后就没有被观测，认为不靠谱，也需要重新创建。创建新的地图点，主要使用反射影函数UnprojectStereo()。完成之后，加入到上一帧的地图点之中。

其具体代码实现如下:

/**
 * @brief 更新上一帧位姿，在上一帧中生成临时地图点
 * 单目情况：只计算了上一帧的世界坐标系位姿
 * 双目和rgbd情况：选取有有深度值的并且没有被选为地图点的点生成新的临时地图点，提高跟踪鲁棒性
 */
void Tracking::UpdateLastFrame()
{
    // Update pose according to reference keyframe
    // Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿
    // 上一普通帧的参考关键帧，注意这里用的是参考关键帧（位姿准）而不是上上一帧的普通帧
    KeyFrame* pRef = mLastFrame.mpReferenceKF;  
    // ref_keyframe 到 lastframe的位姿变换
    cv::Mat Tlr = mlRelativeFramePoses.back();

    // 将上一帧的世界坐标系下的位姿计算出来
    // l:last, r:reference, w:world
    // Tlw = Tlr*Trw 
    mLastFrame.SetPose(Tlr*pRef->GetPose()); 

    // 如果上一帧为关键帧，或者单目的情况，则退出
    if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
        return;

    // Step 2：对于双目或rgbd相机，为上一帧生成新的临时地图点
    // 注意这些地图点只是用来跟踪，不加入到地图中，跟踪完后会删除

    // Create "visual odometry" MapPoints
    // We sort points according to their measured depth by the stereo/RGB-D sensor
    // Step 2.1：得到上一帧中具有有效深度值的特征点（不一定是地图点）
    vector<pair<float,int> > vDepthIdx;
    vDepthIdx.reserve(mLastFrame.N);

    for(int i=0; i<mLastFrame.N;i++)
    {
        float z = mLastFrame.mvDepth[i];
        if(z>0)
        {
            // vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id
            vDepthIdx.push_back(make_pair(z,i));
        }
    }

    // 如果上一帧中没有有效深度的点,那么就直接退出
    if(vDepthIdx.empty())
        return;

    // 按照深度从小到大排序
    sort(vDepthIdx.begin(),vDepthIdx.end());

    // We insert all close points (depth<mThDepth)
    // If less than 100 close points, we insert the 100 closest ones.
    // Step 2.2：从中找出不是地图点的部分  
    int nPoints = 0;
    for(size_t j=0; j<vDepthIdx.size();j++)
    {
        int i = vDepthIdx[j].second;

        bool bCreateNew = false;

        // 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点
        MapPoint* pMP = mLastFrame.mvpMapPoints[i];
        if(!pMP)
            bCreateNew = true;
        else if(pMP->Observations()<1)      
        {
            // 地图点被创建后就没有被观测，认为不靠谱，也需要重新创建
            bCreateNew = true;
        }

        if(bCreateNew)
        {
            // Step 2.3：需要创建的点，包装为地图点。只是为了提高双目和RGBD的跟踪成功率，并没有添加复杂属性，因为后面会扔掉
            // 反投影到世界坐标系中
            cv::Mat x3D = mLastFrame.UnprojectStereo(i);
            MapPoint* pNewMP = new MapPoint(
                x3D,            // 世界坐标系坐标
                mpMap,          // 跟踪的全局地图
                &mLastFrame,    // 存在这个特征点的帧(上一帧)
                i);             // 特征点id

            // 加入上一帧的地图点中
            mLastFrame.mvpMapPoints[i]=pNewMP; 

            // 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除
            mlpTemporalPoints.push_back(pNewMP);
            nPoints++;
        }
        else
        {
            // 因为从近到远排序，记录其中不需要创建地图点的个数
            nPoints++;
        }

        // Step 2.4：如果地图点质量不好，停止创建地图点
        // 停止新增临时地图点必须同时满足以下条件：
        // 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）
        // 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出
        if(vDepthIdx[j].first>mThDepth && nPoints>100)
            break;
    }
}

?

四、SearchByProjection()

下面再来分析 SearchByProjection() 函数，该函数位于 ORBmatcher.cc 文件中，该函数实现的就是 一、前言 中的图示过程。具体如下：

$(01):$ 建立旋转差直方图，用于检测旋转一致性，最后会剔除不一致的匹配。

$(02):$ 首先获当前帧相机位姿 $R_{cw},t_{cw}$，以及上一帧的位姿 $R_{lw},t_{lw}$，然后令变换矩阵如下：
$$\begin{array}{r}{\mathbf{T}}_{cw}=\left[\begin{array}{cc}{R}_{cw}& t_{cw}\\ 0^T& 1\end{array}\right]{\mathbf{T}}_{lw}=\left[\begin{array}{cc}{R}_{lw}& t_{lw}\\ 0^T& 1\end{array}\right]\end{array}$$$${\mathbf{T}}_{wc}=\left[\begin{array}{cc}{R}_{cw}& t_{cw}\\ 0^T& 1\end{array}\right]={\mathbf{T}}_{cw}^{?1}=\left[\begin{array}{cc}{R}_{cw}^T& ?R_{cw}^T{t}_{cw}\\ 0^T& 1\end{array}\right]$$ 其上${\mathbf{T}}_{wc}$ 是根据 ${\mathbf{T}}_{wc}{\mathbf{T}}_{cw}=\mathbf{E}$ 求解而来。很明显可知 $t_{wc}=?R_{cw}^T{t}_{cw}$, 继续推导: $$\begin{array}{rl}{\mathbf{T}}_{lc}& ={\mathbf{T}}_{lw}{\mathbf{T}}_{cw}^{?1}\\ & =\left[\begin{array}{cc}{R}_{lw}& t_{kw}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{R}_{cw}^T& ?R_{cw}^T{t}_{cw}\\ 0^T& 1\end{array}\right]\\ & =\left[\begin{array}{cc}{R}_{lw}{R}_{cw}^T& ?R_{kw}{R}_{cw}^T{t}_{cw}+t_{lw}\\ 0^T& 1\end{array}\right]\end{array}$$所以最终可以推导出 $t_{lc}=?R_{lw}{R}_{cw}^T{t}_{cw}+t_{lw}=R_{lw}{t}_{wc}+t_{lw}$, 这样就能计算出当前帧相对于上一帧相机的平移向量，根据 $t_{lc}$ 最后一个深度变换值，可以知道相对于上一帧，相机是前进还是后退了。另外，非单目情况，如果Z大于基线，则表示相机明显前进；非单目情况，如果-Z小于基线，则表示相机明显后退。

$(03):$ 对上一帧的每一个地图点，通过相机投影模型，得到得到投影到当前帧的像素坐标。获取上一帧中地图点对应二维特征点所在的金字塔层级，根据相机的前后前进方向来判断搜索尺度范围。
①当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来。
②当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来。
该步骤主要获得当前帧半径范围内的所有特征点，核心函数为GetFeaturesInArea()，前面已经进行讲解过。

$(04):$ 达到缩小搜索特征匹配范围目的之后，则进行两帧之间的特征点配对， 如果最佳匹配距离要小于设定阈值。则认为匹配成功，最后再计算匹配点旋转角度差所在的直方图，进行旋转一致检测，剔除不一致的匹配。

其具体代码实现如下：

/**
 * @brief 将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。用于跟踪前一帧
 * 步骤
 * Step 1 建立旋转直方图，用于检测旋转一致性
 * Step 2 计算当前帧和前一帧的平移向量
 * Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
 * Step 4 根据相机的前后前进方向来判断搜索尺度范围
 * Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点 
 * Step 6 计算匹配点旋转角度差所在的直方图
 * Step 7 进行旋转一致检测，剔除不一致的匹配
 * @param[in] CurrentFrame          当前帧
 * @param[in] LastFrame             上一帧
 * @param[in] th                    搜索范围阈值，默认单目为7，双目15
 * @param[in] bMono                 是否为单目
 * @return int                      成功匹配的数量
 */
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
{
    int nmatches = 0;

    // Rotation Histogram (to check rotation consistency)
    // Step 1 建立旋转直方图，用于检测旋转一致性
    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);

    //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码
    const float factor = HISTO_LENGTH/360.0f;

    // Step 2 计算当前帧和前一帧的平移向量
    //当前帧的相机位姿
    const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3);
    const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);

    //当前相机坐标系到世界坐标系的平移向量
    const cv::Mat twc = -Rcw.t()*tcw; 

    //上一帧的相机位姿
    const cv::Mat Rlw = LastFrame.mTcw.rowRange(0,3).colRange(0,3);
    const cv::Mat tlw = LastFrame.mTcw.rowRange(0,3).col(3); // tlw(l)

    // vector from LastFrame to CurrentFrame expressed in LastFrame
    // 当前帧相对于上一帧相机的平移向量
    const cv::Mat tlc = Rlw*twc+tlw; 

    // 判断前进还是后退
    const bool bForward = tlc.at<float>(2) > CurrentFrame.mb && !bMono;     // 非单目情况，如果Z大于基线，则表示相机明显前进
    const bool bBackward = -tlc.at<float>(2) > CurrentFrame.mb && !bMono;   // 非单目情况，如果-Z小于基线，则表示相机明显后退

    //  Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
    for(int i=0; i<LastFrame.N; i++)
    {
        MapPoint* pMP = LastFrame.mvpMapPoints[i];

        if(pMP)
        {
            if(!LastFrame.mvbOutlier[i])
            {
                // 对上一帧有效的MapPoints投影到当前帧坐标系
                cv::Mat x3Dw = pMP->GetWorldPos();
                cv::Mat x3Dc = Rcw*x3Dw+tcw;

                const float xc = x3Dc.at<float>(0);
                const float yc = x3Dc.at<float>(1);
                const float invzc = 1.0/x3Dc.at<float>(2);

                if(invzc<0)
                    continue;

                // 投影到当前帧中
                float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
                float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;

                if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)
                    continue;
                if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
                    continue;

                // 上一帧中地图点对应二维特征点所在的金字塔层级
                int nLastOctave = LastFrame.mvKeys[i].octave;

                // Search in a window. Size depends on scale
                // 单目：th = 7，双目：th = 15
                float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索范围越大

                // 记录候选匹配点的id
                vector<size_t> vIndices2;         

                // Step 4 根据相机的前后前进方向来判断搜索尺度范围。
                // 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点
                // 当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来
                // 当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来
                if(bForward) // 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
                else if(bBackward) // 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
                else // 在[nLastOctave-1, nLastOctave+1]中搜索
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);

                if(vIndices2.empty())
                    continue;

                const cv::Mat dMP = pMP->GetDescriptor();

                int bestDist = 256;
                int bestIdx2 = -1;

                // Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点 
                for(vector<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)
                {
                    const size_t i2 = *vit;

                    // 如果该特征点已经有对应的MapPoint了,则退出该次循环
                    if(CurrentFrame.mvpMapPoints[i2])
                        if(CurrentFrame.mvpMapPoints[i2]->Observations()>0)
                            continue;

                    if(CurrentFrame.mvuRight[i2]>0)
                    {
                        // 双目和rgbd的情况，需要保证右图的点也在搜索半径以内
                        const float ur = u - CurrentFrame.mbf*invzc;
                        const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
                        if(er>radius)
                            continue;
                    }

                    const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);

                    const int dist = DescriptorDistance(dMP,d);

                    if(dist<bestDist)
                    {
                        bestDist=dist;
                        bestIdx2=i2;
                    }
                }

                // 最佳匹配距离要小于设定阈值
                if(bestDist<=TH_HIGH)
                {
                    CurrentFrame.mvpMapPoints[bestIdx2]=pMP; 
                    nmatches++;

                    // Step 6 计算匹配点旋转角度差所在的直方图
                    if(mbCheckOrientation)
                    {
                        float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
                        if(rot<0.0)
                            rot+=360.0f;
                        int bin = round(rot*factor);
                        if(bin==HISTO_LENGTH)
                            bin=0;
                        assert(bin>=0 && bin<HISTO_LENGTH);
                        rotHist[bin].push_back(bestIdx2);
                    }
                }
            }
        }
    }

    //Apply rotation consistency
    //  Step 7 进行旋转一致检测，剔除不一致的匹配
    if(mbCheckOrientation)
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i<HISTO_LENGTH; i++)
        {
            // 对于数量不是前3个的点对，剔除
            if(i!=ind1 && i!=ind2 && i!=ind3)
            {
                for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
                {
                    CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                    nmatches--;
                }
            }
        }
    }

    return nmatches;
}

?

五、TrackWithMotionModel()

了解 UpdateLastFrame() 与 SearchByProjection() 函数之后，最后再来看看，TrackWithMotionModel() 函数，其实现于 src/Tracking.cc 文件中:

/**
 * @brief 用局部地图进行跟踪，进一步优化位姿
 * 
 * 1. 更新局部地图，包括局部关键帧和关键点
 * 2. 对局部MapPoints进行投影匹配
 * 3. 根据匹配对估计当前帧的姿态
 * 4. 根据姿态剔除误匹配
 * @return true if success
 * 
 * Step 1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints 
 * Step 2：在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪
 * Step 3：更新局部所有MapPoints后对位姿再次优化
 * Step 4：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
 * Step 5：决定是否跟踪成功
 */
bool Tracking::TrackLocalMap()
{
    // We have an estimation of the camera pose and some map points tracked in the frame.
    // We retrieve the local map and try to find matches to points in the local map.

    // Update Local KeyFrames and Local Points
    // Step 1：更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints
    UpdateLocalMap();

    // Step 2：筛选局部地图中新增的在视野范围内的地图点，投影到当前帧搜索匹配，得到更多的匹配关系
    SearchLocalPoints();

    // Optimize Pose
    // 在这个函数之前，在 Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel 中都有位姿优化，
    // Step 3：前面新增了更多的匹配关系，BA优化得到更准确的位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
    mnMatchesInliers = 0;

    // Update MapPoints Statistics
    // Step 4：更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目
    for(int i=0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            // 由于当前帧的地图点可以被当前帧观测到，其被观测统计量加1
            if(!mCurrentFrame.mvbOutlier[i])
            {
                // 找到该点的帧数mnFound 加 1
                mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                //查看当前是否是在纯定位过程
                if(!mbOnlyTracking)
                {
                    // 如果该地图点被相机观测数目nObs大于0，匹配内点计数+1
                    // nObs： 被观测到的相机数目，单目+1，双目或RGB-D则+2
                    if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                        mnMatchesInliers++;
                }
                else
                    // 记录当前帧跟踪到的地图点数目，用于统计跟踪效果
                    mnMatchesInliers++;
            }
            // 如果这个地图点是外点,并且当前相机输入还是双目的时候,就删除这个点
            // ?单目就不管吗
            else if(mSensor==System::STEREO)  
                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);

        }
    }

    // Decide if the tracking was succesful
    // More restrictive if there was a relocalization recently
    // Step 5：根据跟踪匹配数目及重定位情况决定是否跟踪成功
    // 如果最近刚刚发生了重定位,那么至少成功匹配50个点才认为是成功跟踪
    if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
        return false;

    //如果是正常的状态话只要跟踪的地图点大于30个就认为成功了
    if(mnMatchesInliers<30)
        return false;
    else
        return true;
}

?

六、源码注释

通过该篇博客，已经非常详细的分析了 TrackWithMotionModel() 。同时对 UpdateLastFrame() 与 SearchByProjection() 进行了详细的讲解。另外还有一个要点，就是如何计算出当前帧相对于上一帧相机的平移向量，并且根据该向量去判断相机处于前进还是后退状态。

开发者涨薪指南

48位大咖的思考法则、工作方式、逻辑体系