orbslam2代码详解之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<KeyFrame*,int> 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.