ORB_SLAM2中从特征点到地图点完整PipeLine介绍

0 文章导读

在 ORB_SLAM2 中，特征点与地图点是整个 SLAM 系统的关键，所以整个 Pipeline 贯穿了 Tracking 和 LocalMapping 两大线程，本文将从特征点产生开始， 探讨它是怎么一步步经过删除和融合最终变成 MapPoint 地图点的，由于本文探讨的倾向于整个主要的流程，所以其余细节的部分，例如删除和融合策略等忽略不谈，（下文默认按照 RGBD 模式并且跟踪未丢失的情况进行探讨，如果文章有任何的错误或者遗漏请您指正，十分感谢！）。

文章结构概览

1. 流程（PipeLine）
   • 初始化生成地图点
   • 跟踪匹配，得到“旧地图点”
   • 创建关键帧，生成新地图点
   • 处理新增加关键帧，分别处理新旧地图点
   • 检验新增地图点
   • 检查并融合当前关键帧与相邻关键帧中重复的地图点

1 流程（PipeLine）

1.1 初始化生成地图点

系统初始化时，由于全局地图内无地图点 MapPoint，会先通过 Tracking::StereoInitialization() 函数用符合要求的普通帧生成关键帧，并且将符合要求的特征点生成为地图点 MapPoint。
此时全局地图内有了一些与当前帧关联的地图点。

1.2 跟踪匹配，得到“旧地图点”

在 Tracking 未跟丢的情况下，特征点匹配的处理分为两个主要场景：参考关键帧跟踪模式和恒速跟踪模式。参考关键帧跟踪模式时，匹配是在 Tracking::TrackReferenceKeyFrame() 函数内通过 ORBmatcher::SearchByBoW 实现的。该方法利用词袋模型（BoW）中的描述符来寻找匹配点。恒速跟踪模式时，匹配则通过 Tracking::TrackWithMotionModel() 内的 ORBmatcher::SearchByProjection 进行。这种方法通过预测的投影范围来寻找匹配点，不同于基于 BoW 的搜索。

//ORBmatcher::SearchByProjection内部：
if(bestDist<=ORBdist)
{
    CurrentFrame.mvpMapPoints[bestIdx2]=pMP; //根据上一帧特征点与地图点的匹配关系直接得到当前帧特征点与地图点的匹配关系
    nmatches++;
     // Step 5 计算匹配点旋转角度差所在的直方图
    if(mbCheckOrientation)
    {
        float rot = pKF->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);
    }
}

当系统 Tracking 来到下一帧与前一帧通过上述两种方式做特征点匹配时，会出现以下两种情况

1. 特征点与上一帧特征点相匹配，根据上一帧特征点与地图点的匹配关系直接得到当前帧特征点与地图点 MapPoint 的匹配关系，下文称此类地图点为“旧地图点”（提示：在 ORB_SLAM2 中，两帧之间的特征点是靠与地图点 MapPoint 的关系而联系起来的），注意：通过此方式获得匹配关系时，地图点没有对此帧的观测，这个特性在后面的步骤中很重要。
2. 未找到匹配的特征点，下一步进行处理

1.3 创建关键帧，生成新地图点

在函数 Tracking::CreateNewKeyFrame() 中，当系统决定将当前帧变为关键帧的时候，系统会遍历所有特征点，将没有对应地图点的特征点包装为地图点（此时有对应地图点的特征点都是通过步骤 1.2，根据上一帧匹配而来）,代码如下：

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);
                }
                ...
            }

对于此时包装的新地图点，会进行如下操作

MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
pNewMP->AddObservation(pKF,i); //增加此新地图点对此关键帧的观测
pKF->AddMapPoint(pNewMP,i);//将此地图点添加到此关键帧储存地图点与特征点关系的容器中
pNewMP->ComputeDistinctiveDescriptors();
pNewMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pNew);//将此地图点添加进全局地图
mCurrentFrame.mvpMapPoints[i]=pNewMP;

1. 增加此新地图点对此关键帧的观测
2. 将此地图点添加到此关键帧储存地图点与特征点关系的容器中
   …
3. 将此地图点添加进全局地图

第一步十分重要，因为在之后的步骤中，就是通过地图点是否对当前关键帧有观测来分辨是否为新加入地图点。

1.4 处理新增加关键帧，分别处理新旧地图点

在 1.3 中新生成的关键帧会在函数 LocalMapping::ProcessNewKeyFrame() 中进行处理，在此函数中，首先会将新增加的关键帧中的所有地图点取出。

然后通过地图点是否对此关键帧有观测判断此地图点是否为新增加地图点，还是通过与上一帧匹配的方式得到的。（ps： 新增加的地图点在 RGBD / 双目模式中是在 Tracking::CreateNewKeyFrame 中生成，对于单目模式，是在 LocalMapping::CreateNewMapPoints 中通过三角化产生）。

如果是新增加的地图点，那么将其放入 std::list<MapPoint*> mlpRecentAddedMapPoints 容器中，等待后续对其处理。

而对于通过上一帧匹配而来的旧地图点，则增加地图点对当前关键帧的观测。

这部分代码如下：

if(!pMP->IsInKeyFrame(mpCurrentKeyFrame)) //如果为通过上一帧匹配而来的地图点，其中函数IsInKeyFrame就是通过判断地图点是否对关键帧有观测实现
{
    pMP->AddObservation(mpCurrentKeyFrame, i);
    // 获得该点的平均观测方向和观测距离范围
    pMP->UpdateNormalAndDepth();
    // 更新地图点的最佳描述子
    pMP->ComputeDistinctiveDescriptors();
}
else //如果为新增的地图点
{
    // 这些地图点可能来自双目或RGBD在创建关键帧中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生
    // 将上述地图点放入mlpRecentAddedMapPoints，等待后续MapPointCulling函数的检验
    mlpRecentAddedMapPoints.push_back(pMP); 
}

1.5 检验新增地图点

在 1.4 中添加的新增地图点都会在 LocalMapping::MapPointCulling() 函数进行筛选，以决定这个地图点是否应该真正的加入 Map 当中，注意，走到这一步的地图点其实已经存在于地图中了，在 pangolin 的窗口其实可以看到 mlpRecentAddedMapPoints 内的地图点，但是当这里的逻辑判断不合格以后，会 set bad，然后从 Map 中删除。

判断一个地图点是否为不合格点有以下两个判断依据

1. 跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于 25%
2. 从生成该点的关键帧，到现在，如果不小于两关键帧了，并且观测到该点的相机数却不超过阈值 cnThObs（这里的 " 现在 " 指的是当前处理的关键帧）

当地图点满足以上其中一个条件时，系统会将此地图点 set bad（从 Map 中删除），并且移出 mlpRecentAddedMapPoints 。
而如果一个地图点经过了三个关键帧还没被清除出队列 mlpRecentAddedMapPoints,则认为这个点可以正式加入 Map 中，所以不 set bad，然后从队列中移除，不用再进行这里的检验了。

while(lit!=mlpRecentAddedMapPoints.end())
{
    MapPoint* pMP = *lit;
    if(pMP->isBad())
    {
        // Step 2.1：已经是坏点的地图点仅从队列中删除
        lit = mlpRecentAddedMapPoints.erase(lit);
    }
    else if(pMP->GetFoundRatio()<0.25f)
    {
        // Step 2.2：跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除
        // (mnFound/mnVisible） < 25%
        // mnFound ：地图点被多少帧（包括普通帧）看到，次数越多越好
        // mnVisible：地图点应该被看到的次数
        // (mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低
        pMP->SetBadFlag();
        lit = mlpRecentAddedMapPoints.erase(lit);
    }
    else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
    {
        // Step 2.3：从该点建立开始，到现在已经过了不小于2个关键帧
        // 但是观测到该点的相机数却不超过阈值cnThObs，从地图中删除
        pMP->SetBadFlag();
        lit = mlpRecentAddedMapPoints.erase(lit);
    }
    else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
        // Step 2.4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
        // 因此没有SetBadFlag()，仅从队列中删除
        lit = mlpRecentAddedMapPoints.erase(lit);
    else
        lit++;
}

1.6 检查并融合当前关键帧与相邻关键帧中重复的地图点

当 LocalMapping 已经处理完队列中的最后的一个关键帧以后，系统在函数 LocalMapping::SearchInNeighbors 中找出与当前关键帧一，二级相邻的关键帧，遍历这些相邻关键帧里面的所有地图点。在函数 ORBmatcher::Fuse 中通过位姿关系，将这些地图点投影到当前关键帧中，找出与当前关键帧中匹配的特征点，按照下面策略进行融合：

• 如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
• 如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点

这里就不放代码了，感兴趣的读者可以自行阅读。

至此，特征点到地图点的完整 PipeLine 就结束了，感谢各位的阅读，如果各位发现有任何错误或者遗漏的地方，请批评指出！