SLAM——ORB-SLAM3代码分析（三）MapPoint（3）

2021SC@SDUSC

MapPoint分析（3）

• MapPoint.h的分析
• MapPoint.cc的分析（一）
  上篇博客已经分析完了MapPoint.h, 对MapPoint的工作有了大体上的了解，那么现在开始对MapPoint.cc的分析，在MapPoint.h中较为详细分析的部分这里就会简单带过。
  还记得头文件中有三个MapPoint构造函数，但参数不一样，从代码可以看出，前两个构造函数主要涉及的是变量的赋值等问题，比如下面的第一个构造函数：

MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap):
    mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0),
    mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
    mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false),
    mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap),
    mnOriginMapId(pMap->GetId())
{
    Pos.copyTo(mWorldPos);
    mNormalVector = cv::Mat::zeros(3,1,CV_32F);

    mbTrackInViewR = false;
    mbTrackInView = false;

    // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
    unique_lock<mutex> lock(mpMap->mMutexPointCreation);
    mnId=nNextId++;
}

而第三个构造函数与前两个存在略微不同，变量还涉及了一些简单的计算，例如最靠前的mNormalVector计算的是单位法矢；mfMaxDistance是由深度值乘尺度，得到的最大深度值，mfMinDistance则是深度值除以最大尺度，得到的最小深度值；而最后一行则是关于获取这个地图点的描述子。

    mNormalVector = mWorldPos - Ow;
    mNormalVector = mNormalVector/cv::norm(mNormalVector);

    cv::Mat PC = Pos - Ow;
    const float dist = cv::norm(PC);
    const int level = (pFrame -> Nleft == -1) ? pFrame->mvKeysUn[idxF].octave
                                              : (idxF < pFrame -> Nleft) ? pFrame->mvKeys[idxF].octave
                                                                         : pFrame -> mvKeysRight[idxF].octave;
    const float levelScaleFactor =  pFrame->mvScaleFactors[level];
    const int nLevels = pFrame->mnScaleLevels
    mfMaxDistance = dist*levelScaleFactor;
    mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1];

    pFrame->mDescriptors.row(idxF).copyTo(mDescriptor);

构造方法结束之后，SetWorldPos和GetWorldPos分别设置和获取了地图点在世界坐标系下的坐标，其中前者在设置时还使用了两个线程锁。

void MapPoint::SetWorldPos(const cv::Mat &Pos)
{
    unique_lock<mutex> lock2(mGlobalMutex);
    unique_lock<mutex> lock(mMutexPos);
    Pos.copyTo(mWorldPos);
}
cv::Mat MapPoint::GetWorldPos()
{
    unique_lock<mutex> lock(mMutexPos);
    return mWorldPos.clone();
}

GetNormal是获取归一化法矢，也就是世界坐标系下地图点被多个相机观测的平均观测方向；
GetReferenceKeyFrame可以获取地图点的参考关键帧。可以注意到这两个方法的时候都使用了一个线程互斥锁，来解决多线程对共享资源的竞争问题。

cv::Mat MapPoint::GetNormal()
{
    unique_lock<mutex> lock(mMutexPos);
    return mNormalVector.clone();
}

KeyFrame* MapPoint::GetReferenceKeyFrame()
{
    unique_lock<mutex> lock(mMutexFeatures);
    return mpRefKF;
}

AddObservation方法会添加观测，记录关键帧、特征点索引，这个函数是建立关键帧共视关系的核心函数，能共同观测到某些地图点的关键帧是共视关键帧。其中左、右目特征点分别添加；同时mObservations会观测到该MapPoint的关键帧KF和该MapPoint在KF中的索引，如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引；对相机观测数目的增加也是如同MapPoint.h中单目、双目和RGB-D分别+1、+2、+2。

void MapPoint::AddObservation(KeyFrame* pKF, int idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    // 左目特征点索引，右目特征点索引
    tuple<int,int> indexes;

    if(mObservations.count(pKF)){
        indexes = mObservations[pKF];
    }
    else{
        indexes = tuple<int,int>(-1,-1);
    }

    if(pKF -> NLeft != -1 && idx >= pKF -> NLeft){
        get<1>(indexes) = idx;
    }
    else{
        get<0>(indexes) = idx;
    }

    mObservations[pKF]=indexes;

    if(!pKF->mpCamera2 && pKF->mvuRight[idx]>=0)
        nObs+=2;
    else
        nObs++;
}

与添加相对的删除观测方法EraseObservation中，定义了一系列删除时的规则：当删除一个观测关键帧时，查找这个要删除的观测,根据单目和双目类型的不同从其中删除当前地图点的被观测次数。如果该keyFrame是参考帧，那么该Frame被删除后重新指定RefFrame；而当观测到该点的相机数目少于2时，丢弃该点。

void MapPoint::EraseObservation(KeyFrame* pKF)
{
    bool bBad=false;
    {
        unique_lock<mutex> lock(mMutexFeatures);
        if(mObservations.count(pKF))
        {
            tuple<int,int> indexes = mObservations[pKF];
            int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

            if(leftIndex != -1){
                if(!pKF->mpCamera2 && pKF->mvuRight[leftIndex]>=0)
                    nObs-=2;
                else
                    nObs--;
            }
            if(rightIndex != -1){
                nObs--;
            }
            mObservations.erase(pKF);

            if(mpRefKF==pKF)
                mpRefKF=mObservations.begin()->first;

            // If only 2 observations or less, discard point
            if(nObs<=2)
                bBad=true;
        }
    }
    if(bBad)
        SetBadFlag();
}

可以注意到，上面删除观测方法EraseObservation的末尾有一个SetBadFlag方法。在这个方法中，Step 1把mObservations转存到obs，obs和mObservations里存的是指针，赋值过程为浅拷贝；Step 2 则把mObservations指向的内存释放，obs作为局部变量之后自动删除。在for循环中，EraseMapPointMatch会告诉可以观测到该MapPoint的KeyFrame，该MapPoint被删除了。最后一步则是擦除该MapPoint申请的内存——一套完整的流程。

void MapPoint::SetBadFlag()
{
    map<KeyFrame*, tuple<int,int>> obs;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        mbBad=true;
        //Step 1
        obs = mObservations;
        //Step 2
        mObservations.clear();
    }
    for(map<KeyFrame*, tuple<int,int>>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        int leftIndex = get<0>(mit -> second), rightIndex = get<1>(mit -> second);
        if(leftIndex != -1){
            pKF->EraseMapPointMatch(leftIndex);
        }
        if(rightIndex != -1){
            pKF->EraseMapPointMatch(rightIndex);
        }
    }

    mpMap->EraseMapPoint(this);
}

Replace方法的作用是替换地图点，更新观测关系。这个方法同样有一套严谨而完整的逻辑：

• 首先如果目标替换地图点和原地图点是同一个地图点则跳过流程
• 接下来是清除当前地图点的信息，这里的操作与SetBadFlag函数相同：清除当前地图点的原有观测，然后暂存当前地图点的可视次数和被找到的次数，接下来指明当前地图点已经被指定的地图点替换了
• 将观测到当前地图的的关键帧的信息进行更新（需要注意的是所有能观测到原地图点的关键帧都要复制到替换的地图点上）
• 在更新过程中，若该关键帧中没有对"要替换本地图点的地图点"的观测：那么先让KeyFrame用pMP替换掉原来的MapPoint，再让MapPoint替换掉对应的KeyFrame；否则删除这个关键帧对当前帧的观测
• 接下来将当前地图点的观测数据等其他数据都"叠加"到新的地图点上
• 描述子更新
• 最后告知地图,删掉"this"MapPoint

void MapPoint::Replace(MapPoint* pMP)
{
    if(pMP->mnId==this->mnId)
        return;

    int nvisible, nfound;
    map<KeyFrame*,tuple<int,int>> obs;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        obs=mObservations;
        mObservations.clear();
        mbBad=true;
        nvisible = mnVisible;
        nfound = mnFound;
        mpReplaced = pMP;
    }

    // 遍历原始MP观测帧，如果pMP不在观测帧地图点集合中，用pMP代替MP；如果在，删除MP的匹配关系
    for(map<KeyFrame*,tuple<int,int>>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
    {
        // Replace measurement in keyframe
        KeyFrame* pKF = mit->first;

        tuple<int,int> indexes = mit -> second;
        int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

        if(!pMP->IsInKeyFrame(pKF))
        {
            if(leftIndex != -1){
                pKF->ReplaceMapPointMatch(leftIndex, pMP);
                pMP->AddObservation(pKF,leftIndex);
            }
            if(rightIndex != -1){
                pKF->ReplaceMapPointMatch(rightIndex, pMP);
                pMP->AddObservation(pKF,rightIndex);
            }
        }
        else
        {
            if(leftIndex != -1){
                pKF->EraseMapPointMatch(leftIndex);
            }
            if(rightIndex != -1){
                pKF->EraseMapPointMatch(rightIndex);
            }
        }
    }
    pMP->IncreaseFound(nfound);
    pMP->IncreaseVisible(nvisible);
    pMP->ComputeDistinctiveDescriptors();

    mpMap->EraseMapPoint(this);
}

剩余一半MapPoint.cc的内容将在下篇博客继续分析。