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。

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值