SLAM——ORB-SLAM3代码分析（四）MapPoint（4）

2021SC@SDUSC

MapPoint分析（4）

• MapPoint.h的分析
• MapPoint.cc的分析（二）
  这篇博客会接着进行分析MapPoint.cc并且进行MapPoint内容的收尾总结。
  IncreaseVisible函数的作用是增加观测数量，可以看到它使用了一个线程互斥锁和一个累加器。函数中的Visible指的是：（1）该MapPoint在某些帧的视野范围内，通过Frame::isInFrustum()函数判断；（2）该MapPoint被这些帧观测到，但并不一定能和这些帧的特征点匹配上，比如说有一个MapPoint在某一帧F的视野范围内，但并不表明该点M可以和F这一帧的某个特征点能匹配上。

void MapPoint::IncreaseVisible(int n)
{
    unique_lock<mutex> lock(mMutexFeatures);
    mnVisible+=n;
}

接着与IncreaseVisible相关的，是IncreaseFound函数，与visible不同的是，found是能找到该点的帧数，并表示与其匹配上了；而GetFoundRatio则是在计算被找到的比例，也就是用found除以visible。

void MapPoint::IncreaseFound(int n)
{
    unique_lock<mutex> lock(mMutexFeatures);
    mnFound+=n;
}

//found/visible
float MapPoint::GetFoundRatio()
{
    unique_lock<mutex> lock(mMutexFeatures);
    return static_cast<float>(mnFound)/mnVisible;
}

ComputeDistinctiveDescriptors函数展现的是如何计算地图点最具代表性的描述子，由于一个地图点会被许多相机观测到，因此在插入关键帧后，需要判断是否更新代表当前点的描述子，过程是先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值。
Step1先获取该地图点所有有效的观测关键帧信息；
Step2遍历观测特征点，将描述子加入向量vDescriptors里面，如果是双目，都加进来（mit->first取观测到该地图点的关键帧，mit->second取该地图点在关键帧中的索引）；
Step3计算这些描述子两两之间的距离， N表示描述子的总数；
Step4从N个描述子中取一个作为当前点的代表描述子，该描述子与其他描述子的距离中值，是最小的，可以理解为是这一堆描述子中心的位置。

void MapPoint::ComputeDistinctiveDescriptors()
{
    vector<cv::Mat> vDescriptors;

    map<KeyFrame*,tuple<int,int>> observations;
// Step 1
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        if(mbBad)
            return;
        observations=mObservations;
    }

    if(observations.empty())
        return;

    vDescriptors.reserve(observations.size());
// Step 2
    for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;

        if(!pKF->isBad()){
            tuple<int,int> indexes = mit -> second;
            int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

            if(leftIndex != -1){
                vDescriptors.push_back(pKF->mDescriptors.row(leftIndex));
            }
            if(rightIndex != -1){
                vDescriptors.push_back(pKF->mDescriptors.row(rightIndex));
            }
        }
    }

    if(vDescriptors.empty())
        return;

// Step 3
    const size_t N = vDescriptors.size();

    // 计算N个观测点对应描述子之间的距离
    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;
        }
    }
// Step 4
    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();
    }
}

GetIndexInKeyFrame函数获取当前地图点在某个关键帧的观测中，MapPoint对应的特征点的ID；
IsInKeyFrame的作用是检查该地图点是否在关键帧中（有对应的二维特征点）。

tuple<int,int> MapPoint::GetIndexInKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexFeatures);
    if(mObservations.count(pKF))
        return mObservations[pKF];
    else
        return tuple<int,int>(-1,-1);
}

//是否被该关键帧观测
bool MapPoint::IsInKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexFeatures);
    return (mObservations.count(pKF));
}

UpdateNormalAndDepth又是一个较为重要且略微复杂的函数，用来更新地图点的平均观测方向、观测距离范围。
Step1获得观测到该地图点的所有关键帧、坐标等信息；
Step2是计算该地图点的平均观测方向，能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向。初始值为0向量，累加为归一化向量，最后除以总数n；
Step3是找到在参考帧中的特征点的金字塔层级；
Step4会设置最大最小距离

void MapPoint::UpdateNormalAndDepth()
{
// Step 1
    map<KeyFrame*,tuple<int,int>> 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;
// Step 2
    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;

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

        if(leftIndex != -1){
            // 相机光心坐标
            cv::Mat Owi = pKF->GetCameraCenter();
            // 法矢
            cv::Mat normali = mWorldPos - Owi;
            // 法矢简单相加更新
            normal = normal + normali/cv::norm(normali);
            n++;
        }
        if(rightIndex != -1){
            cv::Mat Owi = pKF->GetRightCameraCenter();
            cv::Mat normali = mWorldPos - Owi;
            normal = normal + normali/cv::norm(normali);
            n++;
        }
    }
    cv::Mat PC = Pos - pRefKF->GetCameraCenter();
    const float dist = cv::norm(PC);

//Step3
    tuple<int ,int> indexes = observations[pRefKF];
    int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
    int level;
    if(pRefKF -> NLeft == -1){
        level = pRefKF->mvKeysUn[leftIndex].octave;
    }
    else if(leftIndex != -1){
        level = pRefKF -> mvKeys[leftIndex].octave;
    }
    else{
        level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave;
    }

//Step4    
    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;
    }
}

PredictScale根据最大、最小深度与层级的关系，预测地图点对应特征点所在的图像金字塔尺度层数，具体原理如下图所示。

int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF)
{
    float ratio;
    {
        unique_lock<mutex> lock(mMutexPos);
        ratio = mfMaxDistance/currentDist;
    }

    int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
    if(nScale<0)
        nScale = 0;
    else if(nScale>=pKF->mnScaleLevels)
        nScale = pKF->mnScaleLevels-1;

    return nScale;
}

在总结的时候放上这个图。可以看到MapPoint在其中的位置：
MapPoint是地图中的特征点，它自身的参数是三维坐标和描述子，在这个类中它的主要工作有以下方面：

（1）维护关键帧之间的共视关系

（2）通过计算描述向量之间的距离，在多个关键帧的特征点中找最匹配的特征点

（3）在闭环完成修正后，需要根据修正的主帧位姿修正特征点

（4）对于非关键帧，也产生MapPoint，只不过是给Tracking功能临时使用

从这几篇博客的分析，可以看到这些功能的对应，那么MapPoint的分析到此结束，接下来分析Map。