ORB-SLAM笔记——(6)LocalMapping

到上一篇为止算是把Tracking的主体部分介绍得差不多了，那么这一次开始说LocalMapping线程。

当下主流的Keyframe-based SLAM或VO方法，都是用当前最新的观测和已经储存的地图信息进行匹配，从而实现定位功能的。那么问题来了：我们存什么在地图里？什么时候应该向地图里存东西？内存是有限的我们肯定不能无限地扩大地图，那又该怎么删以维持有限的地图大小呢？

以上问题总结起来就是我们如何去维护地图，既能保证定位精度，同时又不会无限占用内存。那么在orb-slam中，这些问题全部交由LocalMapping线程来处理，全部工作包括：处理新关键帧，地图点检查剔除，生成新地图点，Local BA，关键帧剔除。

ProcessNewKeyFrame()

当Tracking线程判断当前帧是一帧关键帧时，会首先调用Tracking::CreateNewKeyFrame()去更新这一帧的相关信息。然后，函数内部会再通过mpLocalMapper->InsertKeyFrame(pKF)，将这一帧加入到建图线程中的新关键帧队列（mlNewKeyFrames）中。当前线程会定时通过CheckNewKeyFrames()检查Tracking线程是否送来了新关键帧。若有，取最早送进队列的关键帧进行处理，具体流程如下：

1. 计算当前关键帧的Bag of Words：mpCurrentKeyFrame->ComputeBoW()

2. 在TrackLocalMap阶段，我们已经在局部地图中找出了一些与当前帧能匹配上的地图点。用当前帧对应特征去更新地图点的法向向量（normal vector）和深度，并选出一个最佳描述子。

UpdateNormalAndDepth()

void MapPoint::UpdateNormalAndDepth()
{
    map<KeyFrame*,size_t> observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        if(mbBad)
            return;
        observations=mObservations;
        pRefKF=mpRefKF;
        Pos = mWorldPos.clone();
    }

    if(observations.empty())
        return;

    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali/cv::norm(normali);
        n++;
    }

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();
    const float dist = cv::norm(PC);
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];
    const int nLevels = pRefKF->mnScaleLevels;

    {
        unique_lock<mutex> lock3(mMutexPos);
        mfMaxDistance = dist*levelScaleFactor;
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];
        mNormalVector = normal/n;
    }
}

更新该点的平均法向量（点到相机的方向向量），以及点的尺度不变的深度范围。

ComputeDistinctiveDescriptors()

void MapPoint::ComputeDistinctiveDescriptors()
{
    // Retrieve all observed descriptors
    vector<cv::Mat> vDescriptors;

    map<KeyFrame*,size_t> observations;

    {
        unique_lock<mutex> lock1(mMutexFeatures);
        if(mbBad)
            return;
        observations=mObservations;
    }

    if(observations.empty())
        return;

    vDescriptors.reserve(observations.size());

    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;

        if(!pKF->isBad())
            vDescriptors.push_back(pKF->mDescriptors.row(mit->second));
    }

    if(vDescriptors.empty())
        return;

    // Compute distances between them
    const size_t N = vDescriptors.size();

    float Distances[N][N];
    for(size_t i=0;i<N;i++)
    {
        Distances[i][i]=0;
        for(size_t j=i+1;j<N;j++)
        {
            int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
            Distances[i][j]=distij;
            Distances[j][i]=distij;
        }
    }

    // Take the descriptor with least median distance to the rest
    int BestMedian = INT_MAX;
    int BestIdx = 0;
    for(size_t i=0;i<N;i++)
    {
        vector<int> vDists(Distances[i],Distances[i]+N);
        sort(vDists.begin(),vDists.end());
        int median = vDists[0.5*(N-1)];

        if(median<BestMedian)
        {
            BestMedian = median;
            BestIdx = i;
        }
    }

    {
        unique_lock<mutex> lock(mMutexFeatures);
        mDescriptor = vDescriptors[BestIdx].clone();
    }
}

一个MapPoint对应n个观测会有n个描述子，显然我们需要选出一个足够有代表性的作为该地图点的描述子，不然当我们去匹配一个地图点时，和多个描述子进行对比可还了得，失去了“描述”二字的意义。在orb-slam中，具体做法是计算两两描述子的距离，然后找出那个，和剩余描述子距离的中值最小的那个描述子，即为我们想要的。这其实相当于找了一个有平均最小距离的描述子吧。

3. 在做完点的更新后，需要更新该帧在covisibility graph和essential graph中，和其他关键帧的连接关系。

MapPointCulling()

上一步结束之后，我们得到了新关键帧和之前老关键帧的种种联系。基于这些信息，在orb-slam中，便可以对地图点进行管理。因为每一个新的关键帧都会对应产生一些新的地图点，那么是不是这些地图点都可以直接随着关键帧的生成一股脑地插入到map中呢？答案显然是不能的。其最直接的理由在于，如果该点只是被当前关键帧发现，但在后面的连续追踪过程中几乎不能被观测或匹配到，并且基于该点，当前关键帧可以找出的共视关键帧非常少。那么这样看来，即使当时在创建关键帧该点被找出来了，也不能成为进入地图的理由，应当即使删除掉。那么现在我们看一下orb-slam中是如何具体操作的：

首先，在localmapping线程中，orb维护了一个叫做mlpRecentAddedMapPoints的list，这里面存放了地图中比较新的那些地图点的索引。每当有新的mappoint生成时，都会将对应指针push到list中。每次调用mappointculling()，都会遍历该list，并且根据一系列条件判断该点是否应该被删去。先在这里放上代码：

void LocalMapping::MapPointCulling()
{
    // Check Recent Added MapPoints
    list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;

    int nThObs;
    if(mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;

    while(lit!=mlpRecentAddedMapPoints.end())
    {
        MapPoint* pMP = *lit;
        if(pMP->isBad())
        {
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(pMP->GetFoundRatio()<0.25f )
        {
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
        {
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
            lit++;
    }
}

这一部分的代码还是非常清楚的，步骤是：

1. 已经是坏点的MapPoints直接从检查链表中删除;

2. 通过GetFoundRatio()返回的(mnFound)/mnVisible的比值，也就是实际能够发生匹配的帧数与可以发生匹配的帧数的比值。如果该值<0.25，将该点置为坏点并从list中删去该点；

3. 如果连续该点从生成以后经历了两个以上的关键帧，但观测数却<=cnThObs，则将该点置为坏点并从list中删去该点；

4. 如果该点从生成以后经历了三个以上的关键帧，并且没有被当作坏点，则停止对它进行更多的判断，认为它是一个mappoint，并从list中删除索引。

CreateNewMapPoints()

遍历covisibilitygraph中共视程度最高的10或20（单目模式下）个关键帧。针对每一帧，一开始会先计算两帧之间基线的长度，如果基线太短，认为不利用三角化，直接放弃对其进行后续操作。若基线满足条件，通过ComputeF12()计算两帧之间的基础矩阵F并利用matcher.SearchForTriangulation()以及F，采用极限约束的方法找到当前帧在共视帧的匹配。对这些匹配进行三角化，并对scale的一致性进行查验，以下是这部分的代码：

//Check scale consistency
cv::Mat normal1 = x3D-Ow1;
float dist1 = cv::norm(normal1);

cv::Mat normal2 = x3D-Ow2;
float dist2 = cv::norm(normal2);

if(dist1==0 || dist2==0)
    continue;

const float ratioDist = dist2/dist1;
const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];

/*if(fabs(ratioDist-ratioOctave)>ratioFactor)
    continue;*/
if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
    continue;

代码中最后的if判断若满足，则表明尺度一致性不满足。

以上满足的情况下认为三角化成功，生成mappoint并计算描述子，normal vector和depth。最后将其加到map以及上一部分提到的mlpRecentAddedMapPoints这个list中。

BA+KeyFrameCulling()

在做完以上步骤后，会先check一下有没有新的关键帧进来并且有没有stoprequest，若没有则可以进行接下的操作。

首先进行的是一个局部优化，这里不同于我们上一篇博客讨论tracklocalmap()中的BA。这里优化的对象有当前关键帧Ki、共视图中的相邻关键帧Kc和这些关键帧观测到的地图点，另外可以观测到这些地图点但是不与当前帧相邻的关键帧仅作为约束条件而不作为优化的对象。

在进行过BA之后，我们在localmapping这个线程的最后，需要对map中的关键帧进行维护，防止关键帧个数无限地增长。这里的策略非常简单：如果某一关键帧，它所观测到的90%的地图点都能被至少其他三个关键帧所观测到，那么则认为这一帧是冗余的应该删去。

 

那么这些就是localmapping中比较核心的思想以及实现方式了！

最后，以上全部为个人学习心得，如有理解错误或是出入较大的地方欢迎评论区指正，如需转发，请标明转载出处。