orbslam2代码详解之tracking线程——局部地图跟踪

局部地图跟踪是tracking线程的第三部分，处理的数据来自位姿跟踪部分，具体知识可以先看这篇博客了解。
ORB-SLAM2-tracking线程

位姿跟踪三种方法都是基于帧间匹配，优化3D-2D重投影误差得到当前帧的初始位姿。这样得到的初始位姿不是可靠的，其只利用了两帧数据的信息，如果前一帧质量太差，得到的位姿可信度低。因此为了利用更多的信息，需要进一步将当前帧与局部地图进行匹配和优化，也就是TrackLocalMap() 。

局部地图跟踪

局部地图包括两部分：局部地图关键帧，局部地图地图点。

那么如何确定哪些关键帧应该作为局部地图的关键帧呢？ORB_SLAM2采用的是这张策略：

将所有能够观测到当前帧地图点的关键帧作为局部地图关键帧。
如果局部地图关键帧数量不够的话将这些关键帧的共视关键帧、子关键帧、父关键帧也加入到局部地图关键帧中，直到满足局部关键帧的数量要求。
将能看到当前帧最多地图点的关键帧设为参考关键帧,并更新当前帧的参考关键帧为该帧。
局部地图的地图点即为局部关键帧的所有地图点的集合。

在了解了局部地图的定义之后，再来看局部地图跟踪的目的。

局部地图跟踪的目的就是：在得到当前帧的初始位姿之后，根据局部地图中的地图点和当前帧进行匹配，然后根据匹配结果进一步优化当前帧的位姿和当前帧的地图点。

局部地图跟踪的主要思路为：

1.更新局部地图。将之前的局部地图数据清空，重新构建局部地图。构建局部地图的过程就是重新确定局部地图关键帧和局部地图地图点的过程。
2.局部地图中的地图点与当前帧的地图点进行匹配，然后利用匹配的地图点来优化当前帧的位姿。
3.根据优化后的位姿重新更新匹配点的内点和外点。
4.根据内点数量判定跟踪是否成功。

相机位姿跟踪与局部地图跟踪之间有什么区别和联系？

在相机位姿跟踪过程中，不论是运动模型跟踪还是参考关键帧模型，他们都是根据上一帧的位姿来初步确定当前帧的位姿，然后对位姿进行优化。在该过程中，确定相机位姿的时候只用到了当前帧之前的一帧或两帧图像。对之前数据的利用程度不高。
在局部地图跟踪过程中，局部地图更新的前提是必须知道当前帧的姿态和地图点（尽管可能不准确）。然后局部地图地图点与当前帧地图点进行匹配，然后进行当前帧的位姿优化。在该过程中，利用到了当前帧之前的多帧关键帧。这是与相机位姿跟踪过程最大的区别。可以说，局部地图跟踪也是在相机位姿跟踪基础之上做的操作，此外局部地图跟踪比相机位姿跟踪利用的信息更多。

TrackLocalMap（）

对相机位姿有个基本估计和初始特征匹配，将局部地图投影到当前帧寻找更多对应的匹配地图点MapPoints，优化相机位姿。

随着相机运动，我们向局部地图添加新的关键帧和3D地图点来维护局部地图，这样，即使跟踪过程中某帧出现问题，利用局部地图，我们仍然可以求出之后那些帧的正确位姿。

整体思路和TrackWithMotionModel类似，只不过TrackWithMotionModel匹配点对是通过帧间匹配得到的，而TrackLocalMap是剔除TrackWithMotionModel中的匹配点，从局部地图中找新的更多的匹配点，从而实现对位姿的进一步优化。

ORB-SLAM2的关键是提取每一帧中的ORB特征点，这是系统运行的基础（我们真正利用的信息仅仅是特征点，因此系统的建图也是稀疏的）。每个Frame类会把其提取出的关键点的数量记为N（1~N同时也作为了这些关键点的索引）。每个Frame类还有一个成员mvpMapPoints，其容量也是N。如果对应索引（例：第 i 个）的关键点有匹配的地图点，则那个地图点的指针会被存到对应关键点索引的位置上（例：mvpMapPoints[ i ] ）；否则 mvpMapPoints[ i ] 就会是空的。Frame类还有一个成员mvbOutlier，它每一位表示对应索引的关键点的匹配到的地图点是否是外点（没有对应地图点则为false）。在位姿跟踪中，检查当前帧的每个关键点对应的地图点：如果存在该对应地图点且没有关键帧观察到它（地图点的成员nObs<1），则这个对应地图点从mvpMapPoints中删去（对应位置为空的MapPoint*），mvbOutlier对应位置为false。

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
    // 步骤1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
    UpdateLocalMap();

    // 步骤2：在局部地图中查找与当前帧匹配的MapPoints
    SearchLocalPoints();

    // Optimize Pose，位姿优化(还是只优化位姿不优化地图点)
    // 在这个函数之前，在Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel中都有位姿优化，
    // 步骤3：更新局部所有MapPoints后对位姿再次优化
    //mcurrentFrame是当前帧
    Optimizer::PoseOptimization(&mCurrentFrame);


    // Update MapPoints Statistics
    // 步骤3：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
    //遍历当前帧跟踪到的地图点 mvpMapPoints，如果是外点则抛弃；如果不是外点则该地图点的 mnFound +1。
    //另外，对于不是外点的地图点：如果只跟踪模式，则匹配内点数（ Tracking.mnMatchesInliers ）+1；否则如果该点被关键帧观察到，则匹配内点数+1。
    mnMatchesInliers = 0;//表示当前帧的mappoint中能被其他关键帧观测到的个数
    for(int i=0; i<mCurrentFrame.N; i++)//遍历当前帧所有特征点
    {
        if(mCurrentFrame.mvpMapPoints[i])//这个特征点有相应的mappoint匹配
        //MapPoint型指针，储存着与该关键帧上特征点关联的地图点
        {
            if(!mCurrentFrame.mvbOutlier[i])//mvbOutlier是指优化位姿后被排除在视野外的匹配对
            //在位姿跟踪中优化位姿后，会将outlier的地图点剔除掉，剔除后改为false
            //mCurrentFrame.mvbOutlier[i]=false
            {
                // 由于当前帧的MapPoints可以被当前帧观测到，其被观测统计量加1
                mCurrentFrame.mvpMapPoints[i]->IncreaseFound();//这个加的是被普通帧观测到的次数
                if(!mbOnlyTracking)//用户选择正常模式
                {
                    // 该MapPoint被其它关键帧观测到过
                    if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)//这个Observations表示的是这个mappoint被关键帧观测到的次数
                        mnMatchesInliers++;
                }
                else//用户选择仅跟踪定位模模式，不建图，不添加关键帧
                    // 记录当前帧跟踪到的MapPoints，用于统计跟踪效果
                    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
    // 步骤4：决定是否跟踪成功
    //在刚进行重定位后，对跟踪的要求比较严格，要求当前帧的MapPoints被其他关键帧观测到50个以上
    if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
        return false;
    //正常状态要求当前帧的MapPoints被其他关键帧观测到30个以上
    if(mnMatchesInliers<30)
        return false;
    else
        return true;
}

UpdateLocalMap（）

更新局部地图关键帧UpdateLocalKeyFrames()+局部地图点UpdateLocalPoints()

更新局部关键帧mvpLocakKeyFrames步骤如下：

1.搜索具有共视关系的关键帧。由之前的帧间匹配，我们得到当前帧的地图点（MapPoint），通过遍历这些地图点，得到也能观测到这些地图点的关键帧（地图点上记录了共视信息）。同时记录关键帧的共视次数。关键帧和共视次数存储在map keyframeCounter;
2.将步骤1得到的关键帧全部插入到mvpLocakKeyFrames；
3.遍历步骤1keyframeCounter中每个关键帧，得到与之共视程度最高的10个关键帧，也全部插入到mvpLocakKeyFrames；
4.遍历步骤1keyframeCounter中每个关键帧，将每个关键帧的子关键帧（>=1）和父关键帧(=1，共视程度最高的之前的关键帧)都插入到mvpLocakKeyFrames ；
5.更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧。

更新局部地图点mvpLocalMapPoints步骤如下：

1.清空mvpLocalMapPoints；mvpLocalMapPoints.clear();
2.遍历局部关键帧mvpLocakKeyFrames，将局部关键帧的地图点插入到mvpLocalMapPoints。

/**
 * @brief 更新LocalMap
 *
 * 局部地图包括： \n
 * - K1个关键帧、K2个临近关键帧和参考关键帧
 * - 由这些关键帧观测到的MapPoints
 */
void Tracking::UpdateLocalMap()
{
    // This is for visualization
    // 设置参考地图点
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
    // Update
    // 更新局部关键帧和局部MapPoints
    UpdateLocalKeyFrames();
    UpdateLocalPoints();
}

UpdateLocalKeyFrames()

/**
 * @brief 更新局部关键帧，called by UpdateLocalMap()
 *
 * 遍历当前帧的MapPoints，将观测到这些MapPoints的关键帧和相邻的关键帧取出，更新mvpLocalKeyFrames
 */
void Tracking::UpdateLocalKeyFrames()
{
    // Each map point vote for the keyframes in which it has been observed
    // 步骤1：遍历当前帧的MapPoints，记录所有能观测到当前帧MapPoints的关键帧
    //用于储存关键帧和共视次数
    map<KeyFrame*,int> keyframeCounter;
    for(int i=0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
            //判断地图点是否存在
            if(!pMP->isBad())
            {
                // 能观测到当前帧MapPoints的关键帧 GetObservations()
                const map<KeyFrame*,size_t> observations = pMP->GetObservations();
                for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
                //遍历当前帧的所有地图点，通过地图点索引出 能够看到当前帧地图的所有关键帧 看到加一
                //得到所有能看到地图点的帧，以及能看到的地图点对应的数目
                    keyframeCounter[it->first]++;
            }
            else
            {
                mCurrentFrame.mvpMapPoints[i]=NULL;
            }
        }
    }
    //没有能看到的帧
    if(keyframeCounter.empty())
        return;

	//两个变量用于找出能看到的点最多的帧
    int max=0;
    KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);
    // 步骤2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有三个策略
    // 先清空局部关键帧
    mvpLocalKeyFrames.clear();
    mvpLocalKeyFrames.reserve(3*keyframeCounter.size());
    // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
    // V-D K1: shares the map points with current frame
    // 策略1：能观测到当前帧MapPoints的关键帧作为局部关键帧
    for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
    {
        //遍历得到共视次数最多的帧
        KeyFrame* pKF = it->first;
        if(pKF->isBad())
            continue;
        if(it->second>max)
        {
            max=it->second;
            pKFmax=pKF;
        }
        //同时将这些帧全部存入mvpLocalKeyFrames中，变为局部关键帧
        mvpLocalKeyFrames.push_back(it->first);
        // mnTrackReferenceForFrame防止重复添加局部关键帧
        //将该帧的track参数更改为当前参考帧
        pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
    }
    // Include also some not-already-included keyframes that are neighbors to already-included keyframes
    // V-D K2: neighbors to K1 in the covisibility graph
    // 策略2：与策略1得到的局部关键帧共视程度很高的关键帧作为局部关键帧
    //遍历策略1得到的所有局部关键帧
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        // Limit the number of keyframes
        if(mvpLocalKeyFrames.size()>80)
            break;
        KeyFrame* pKF = *itKF;
        // 策略2.1:最佳共视的10帧
        const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
        //将共视关系最好的十帧也都加入到mvpLocalKeyFrames中
        for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
        {
            KeyFrame* pNeighKF = *itNeighKF;
            if(!pNeighKF->isBad())
            {
                // mnTrackReferenceForFrame防止重复添加局部关键帧
                if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pNeighKF);
                    pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    break;
                }
            }
        }
        // 策略2.2:自己的子关键帧
        const set<KeyFrame*> spChilds = pKF->GetChilds();
        for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
        {
            KeyFrame* pChildKF = *sit;
            if(!pChildKF->isBad())
            {
                if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pChildKF);
                    pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    break;
                }
            }
        }
        // 策略2.3:自己的父关键帧
        KeyFrame* pParent = pKF->GetParent();
        if(pParent)
        {
           // mnTrackReferenceForFrame防止重复添加局部关键帧
           if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
           {
                mvpLocalKeyFrames.push_back(pParent);
                pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                break;
            }
        }
    }
    //子关键帧和父关键帧同样也加入进去
    // V-D Kref： shares the most map points with current frame
    // 步骤3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧
    if(pKFmax)
    {
        mpReferenceKF = pKFmax;
       mCurrentFrame.mpReferenceKF = mpReferenceKF;
    }
}

UpdateLocalPoints()

void Tracking::UpdateLocalPoints()
{
    // 步骤1：清空局部MapPoints
    mvpLocalMapPoints.clear();
    // 步骤2：遍历局部关键帧mvpLocalKeyFrames
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        KeyFrame* pKF = *itKF;
        const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
        // 步骤2：将局部关键帧的MapPoints添加到mvpLocalMapPoints
        for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
        {
           MapPoint* pMP = *itMP;
            if(!pMP)
                continue;
            // mnTrackReferenceForFrame防止重复添加局部MapPoint
            if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)
                continue;
             //得到所有局部关键帧地图点的集合
            if(!pMP->isBad())
            {
                mvpLocalMapPoints.push_back(pMP);
                pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
            }
        }
    }
}

SearchLocalPoints（）

匹配局部地图视野范围内的点和当前帧特征点

步骤如下：

1.遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索，因为当前的MapPoints一定在当前帧的视野内。这些MapPoints是在帧间匹配过程中用到的。
2.将局部地图点mvpLocalMapPoints投影到当前帧，投影矩阵是通过帧间匹配得到。判断局部地图点是否在当前帧的视野内isInFrustum（视野内，mbTrackInView 为true），只有视野内的地图点才进行投影匹配。判断要求包括：投影点是否在图像内；地图点到相机中心距离是否在尺寸变换范围内；相机中心到地图点向量与地图点平均视角夹角是否小于60°。
3.对视野内的地图点通过投影进行特征匹配

void Tracking::SearchLocalPoints()
{
    // Do not search map points already matched
    // 步骤1：遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索
    // 因为当前的mvpMapPoints一定在当前帧的视野中
    for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;
        if(pMP)
        {
            if(pMP->isBad())
            {
                *vit = static_cast<MapPoint*>(NULL);
            }
            else
            {
                // 更新能观测到该点的帧数加1
                pMP->IncreaseVisible();
                // 标记该点之后不再包含在该tracking线程内，最后一次是出现在该帧中
                //也用于剔除外点
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                // 标记该点将来不被投影，因为已经匹配过
                pMP->mbTrackInView = false;
            }
        }
    }

    int nToMatch=0;//局部地图中除了当前帧的mvpMapPoints之外的点，在当前视野内的数目

    // Project points in frame and check its visibility
    // 步骤2：将所有局部MapPoints投影到当前帧，判断是否在视野范围内，然后进行投影匹配
    for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;
        //遍历Tracking线程跟踪的局部地图点 mvpLocalMapPoints ，跳过那些坏点（pMP->isBad()）以及第一步被标记的已经匹配的点
        //将在当前帧视锥范围内（ mCurrentFrame.isInFrustum(pMP,0.5) ）的地图点统计作为想要匹配的局部地图点。如果这些可以匹配的局部地图点数量大于零则进行后续步骤，否则本函数结束。
        if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)
            continue;
        if(pMP->isBad())
            continue;
        
        // Project (this fills MapPoint variables for matching)
        // 步骤2.1：判断LocalMapPoints中的点是否在在视野内
        if(mCurrentFrame.isInFrustum(pMP,0.5))//0.5表示当前帧对该mappoint的观测视角与该mappoint被观测的平均视角差的余弦值不能超过0.5，也就是60度
        //通过计算<该MapPoint与当前帧的相机中心连线>与该MapPoint的平均观测法向量mNormalVector夹角的余弦值，来判断当前帧是否能看到该MapPoint，阈值选择为0.5，即表示小于cos(60)，夹角大于60度就pass掉MapPoint
        {
        	// 观测到该点的帧数加1，该MapPoint在某些帧的视野范围内
            pMP->IncreaseVisible();
            // 只有在视野范围内的MapPoints才参与之后的投影匹配
            nToMatch++;
        }
    }

    if(nToMatch>0)
    {
        ORBmatcher matcher(0.8);
        //最优匹配和次优匹配之间的距离，最优要小于次优的0.8倍才认为合格
        int th = 1;
        //投影点的搜索范围系数
        if(mSensor==System::RGBD)
            th=3;
        //RGBD相机搜索范围偏大

        // If the camera has been relocalised recently, perform a coarser search
        // 如果不久前进行过重定位，那么进行一个更加宽泛的搜索，阈值需要增大
        if(mCurrentFrame.mnId<mnLastRelocFrameId+2)
            th=5;

        // 步骤2.2：对视野范围内的MapPoints通过投影进行特征点匹配
        matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
        //得到一组3D局部地图点-2D特征点匹配对
    }
}

NeedNewKeyFrame()

判断是否需要生成新的关键帧，确定关键帧的标准
当前跟踪效果是否理想、当前帧的跟踪质量好坏、当前局部建图线程是否有空
如果Tracking处于只跟踪模式，则本函数直接返回false，不需要新关键帧；如果局部建图线程因为检测到了闭环而被终止或正在终止，则本函数直接返回false；如果距离上一次重定位不久，直接返回false。

1 仅线程追踪模式不需要添加新的关键帧
2 局部地图模式被暂停或者被发起暂停请求(回环检测冻结局部地图) 则不添加关键帧
3 上次重定位后没有足够的帧通过 则不添加关键帧
4 追踪线程是比较弱的
5 此帧距离上次插入关键帧已经超过了最大的帧数(很久没插入关键帧了)
6 局部地图处于空闲状态并且没有超过最大关键帧数
7 局内点数量比较少 跟踪地图中地图点比较少
8 当前帧的跟踪点数小于90%的参考关键帧跟踪点数，并且当前帧跟踪点数大于15

bool Tracking::NeedNewKeyFrame()
{
    // 步骤1：如果用户在界面上选择重定位，那么将不插入关键帧
    // 由于插入关键帧过程中会生成MapPoint，因此用户选择重定位后地图上的点云和关键帧都不会再增加
    if(mbOnlyTracking)//如果仅跟踪，不选关键帧
        return false;
    //If Local Mapping is freezed by a Loop Closure do not insert keyframes
    // 如果局部地图被闭环检测使用，则不插入关键帧
    if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        return false;
    const int nKFs = mpMap->KeyFramesInMap();//关键帧数
 
    // Do not insert keyframes if not enough frames have passed from last relocalisation
    // 步骤2：判断是否距离上一次插入关键帧的时间太短
    // mCurrentFrame.mnId是当前帧的ID
    // mnLastRelocFrameId是最近一次重定位帧的ID
    // mMaxFrames等于图像输入的帧率
    // 如果关键帧比较少，则考虑插入关键帧
    // 或距离上一次重定位超过1s，则考虑插入关键帧
    if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
        return false;
 
    // Tracked MapPoints in the reference keyframe
    // 步骤3：得到参考关键帧跟踪到的MapPoints数量
    // 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
 
    int nMinObs = 3;
    if(nKFs<=2)
        nMinObs=2;
    //参考关键帧中所有地图点被观察到的次数大于2或3次的地图点数量
    int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
    //获取参考关键帧跟踪到的MapPoints数量
 
 
 
 
    // Local Mapping accept keyframes?
    // 步骤4：查询局部地图管理器是否繁忙
    bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
 
 
 
    // Stereo & RGB-D: Ratio of close "matches to map"/"total matches"
    //双目和RGBD：比率接近地图匹配数/总匹配数
    // "total matches = matches to map + visual odometry matches"
    //总匹配数=地图匹配数+视觉里程计匹配数
    // Visual odometry matches will become MapPoints if we insert a keyframe.
    // This ratio measures how many MapPoints we could create if we insert a keyframe.
    //这个比率测量如果我们插入一个关键帧，我们可以创建多少个MapPoints
    // 步骤5：对于双目或RGBD摄像头，统计总的可以添加的MapPoints数量和跟踪到地图中的MapPoints数量
 
    int nMap = 0;//地图匹配数
    int nTotal= 0;//总匹配数
    if(mSensor!=System::MONOCULAR)// 双目或rgbd
    {
       for(int i =0; i<mCurrentFrame.N; i++)//遍历当前帧所有匹配点
        {
            if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)//map点的速度在合理范围内
            {
                nTotal++;// 总的可以添加mappoints数
                if(mCurrentFrame.mvpMapPoints[i])
                    if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)//mappoint能被观测
                        nMap++;// 被关键帧观测到的mappoints数，即观测到地图中的MapPoints数量
            }
        }
    }
    else
    {
        // There are no visual odometry matches in the monocular case
        nMap=1;
        nTotal=1;
    }
    //计算比例
    const float ratioMap = (float)nMap/(float)(std::max(1,nTotal));
    // 步骤6：决策是否需要插入关键帧
    // Thresholds
    // 设定inlier阈值，和之前帧特征点匹配的inlier比例
    //参考比率 表示当前帧内点数量/参考关键帧中所有被观察到的次数大于2或3次的地图点数量
    float thRefRatio = 0.75f;
    if(nKFs<2)
        thRefRatio = 0.4f;// 关键帧只有一帧，那么插入关键帧的阈值设置很低
    if(mSensor==System::MONOCULAR)
        thRefRatio = 0.9f;
 
    // MapPoints中和地图关联的比例阈值
    float thMapRatio = 0.35f;
    if(mnMatchesInliers>300)
    //匹配到的内点数值
        thMapRatio = 0.20f;
 
    // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
    // 很长时间没有插入关键帧
    const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
    // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
    // localMapper处于空闲状态且距离上次插入关键帧已经经过了最小帧数
    const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
 
    // Condition 1c: tracking is weak
    // 跟踪要跪的节奏，0.25和0.3是一个比较低的阈值
    //如果传感器不是单目   并且(匹配到的内点小于好地图点的0.25倍或者匹配的地图点小于0.3)   追踪线程是比较弱的
    const bool c1c =  mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || ratioMap<0.3f) ;
    // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
    // 阈值比c1c要高，与之前参考帧（最近的一个关键帧）重复度不是太高
    //内点数量大于15  并且内点匹配小于好的地图点的thRefRatio倍
    const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio || ratioMap<thMapRatio) && mnMatchesInliers>15);
 
    if((c1a||c1b||c1c)&&c2)
    {
        // If the mapping accepts keyframes, insert keyframe.
        // Otherwise send a signal to interrupt BA
        //局部建图线程空闲：直接返回true，决定创建新关键帧
        //如果mapping接受关键帧，则插入关键帧，否则发送信号到中断BA
        if(bLocalMappingIdle)
        {
           return true;
        }
 
        else
        {
            //首先让局部建图线程去打断集束优化操作（mpLocalMapper->InterruptBA()）。
            //最后，如果局部建图线程的关键帧队列中关键帧数量小于3则返回true，否则返回false。
            mpLocalMapper->InterruptBA();//中断BA
            if(mSensor!=System::MONOCULAR)
            {
                // 队列里不能阻塞太多关键帧
                // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
                // 然后localmapper再逐个pop出来插入到mspKeyFrames
                if(mpLocalMapper->KeyframesInQueue()<3)//队列中关键帧小于3
                    return true;
                else
                    return false;
            }
            else
                return false;
        }
    }
    else
        return false;
}

CreateNewKeyFrame()

1：用当前帧构造成关键帧
2：将当前关键帧设置为当前帧的参考关键帧
3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints,并且将关键帧传递给localmapping线程。

void Tracking::CreateNewKeyFrame()
{
    // 停止局部建图，如果不成功则本函数直接返回；否则继续后面的步骤
    if(!mpLocalMapper->SetNotStop(true))
        return;

    // 步骤1：将当前帧构造成关键帧
    KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

    // 步骤2：将当前关键帧设置为当前帧的参考关键帧
    // 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
    mpReferenceKF = pKF;
    //更新参考关键帧为当前新添加的关键帧
    mCurrentFrame.mpReferenceKF = pKF;
    //当前帧的参考关键帧设为该帧

    // 这段代码和UpdateLastFrame中的那一部分代码功能相同
    // 步骤3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints
    if(mSensor!=System::MONOCULAR)
    {
        // 根据Tcw计算mRcw、mtcw和mRwc、mOw
        //更新一下当前帧的旋转矩阵、位移向量、相机光心的世界坐标
        mCurrentFrame.UpdatePoseMatrices();

        // We sort points by the measured depth by the stereo/RGBD sensor.
        // We create all those MapPoints whose depth < mThDepth.
        // If there are less than 100 close points we create the 100 closest.
        // 步骤3.1：得到当前帧深度小于阈值的特征点
        // 通过深度对点进行排序   将100个近点插入到地图中
        // 创建新的MapPoint, depth < mThDepth
        //首先检查是否需要创建新的地图点（按照关键点深度从小到大）
        //对于深度大于0的当前帧关键点，如果其没有对应地图点或者地图点没有被观察到（Observation<1），则创建新的地图点
        //创建方法:Frame::UnprojectStereo + MapPoint构造函数并更新信息。

        //以上补充创建地图点的过程在满足下述条件时停止：关键点深度大于mThDepth且当前帧有100个以上地图点。
        vector<pair<float,int> > vDepthIdx;
        vDepthIdx.reserve(mCurrentFrame.N);
        for(int i=0; i<mCurrentFrame.N; i++)
        {
            float z = mCurrentFrame.mvDepth[i];
            if(z>0)
            {
                vDepthIdx.push_back(make_pair(z,i));
            }
        }

        if(!vDepthIdx.empty())
        {
            // 步骤3.2：按照深度从小到大排序
            sort(vDepthIdx.begin(),vDepthIdx.end());

            // 步骤3.3：将距离比较近的点包装成MapPoints
            int nPoints = 0;
            for(size_t j=0; j<vDepthIdx.size();j++)
            {
                int i = vDepthIdx[j].second;

                bool bCreateNew = false;

                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                if(!pMP)
                    bCreateNew = true;
                else if(pMP->Observations()<1)
                {
                    bCreateNew = true;
                    mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
                }

                if(bCreateNew)
                {
                    cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                    MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
                    // 这些添加属性的操作是每次创建MapPoint后都要做的
                    pNewMP->AddObservation(pKF,i);
                    pKF->AddMapPoint(pNewMP,i);
                    pNewMP->ComputeDistinctiveDescriptors();
                    pNewMP->UpdateNormalAndDepth();
                    mpMap->AddMapPoint(pNewMP);

                    mCurrentFrame.mvpMapPoints[i]=pNewMP;
                    nPoints++;
                }
                else
                {
                    nPoints++;
                }

                // 这里决定了双目和rgbd摄像头时地图点云的稠密程度
                // 但是仅仅为了让地图稠密直接改这些不太好，
                // 因为这些MapPoints会参与之后整个slam过程
                // 如果地图点的深度大于阈值(远点)并且当前添加的地图点数量已经大于100 则不再继续添加地图点了
                if(vDepthIdx[j].first>mThDepth && nPoints>100)
                    break;
            }
        }
    }
    /**
    * 往LocalMapping线程的mlNewKeyFrames队列中插入新生成的keyframe
    * LocalMapping线程中检测到有队列中有keyframe插入后线程会run起来
    */
    mpLocalMapper->InsertKeyFrame(pKF);

    mpLocalMapper->SetNotStop(false);

    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKF;
}

map容器及迭代器

Map就是用来存储“键(key)-值(value) 对”的。 Map类中存储的“键值对”通过键来标识，所以“键对象”不能重复。

C++ STL 标准库为 map 容器配备的是双向迭代器（bidirectional iterator）。这意味着，map 容器迭代器只能进行 ++p、p++、–p、p–、*p 操作，并且迭代器之间只能使用 == 或者 != 运算符进行比较。

#include 
#include       // pair
#include        // string
using namespace std;
int main() {
    //创建并初始化 map 容器
    std::map<std::string, std::string>myMap{ {"STL教程","http://c.biancheng.net/stl/"},{"C语言教程","http://c.biancheng.net/c/"} };
    //调用 begin()/end() 组合，遍历 map 容器
    for (auto iter = myMap.begin(); iter != myMap.end(); ++iter) {
        cout << iter->first << " " << iter->second << endl;
    }
    return 0;
}

C++ STL map容器迭代器用法详解