ORB-SLAM2学习3 MapPoint.h Map.h KeyFrame.h

Posted 2020-09-22

1.MapPoint.h

里面有个类MapPoint,其有两个构造函数。分别为，参考帧是关键帧和参入是普通帧的情况。

   1 MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap);
     MapPoint(const cv::Mat &Pos,  Map* pMap, Frame* pFrame, const int &idxF);

    [email protected] Pos,指的是该点的3D位置。     2 @pRefKF，参考关键帧。   3 @ pMap,地图点。    4 @ pFrame 普通帧 。
    5 @idxF 该地图点在普通帧里面的编号索引。

当参考帧为关键帧的时候，在构造函数里面只是简单的设置下这个地图点的3D坐标。设置这个地图的编号，初始化mNormalVector 。当传入为普通帧的时候，设置这个点的3D坐标，mNormalVector 初始化，并且设置这个地图点的描述子。

 

2    //设置该点在空间中的3维坐标
    void SetWorldPos(const cv::Mat &Pos);
    cv::Mat GetWorldPos();
   
    //得到相机中心到该点的向量的归一化值。
    cv::Mat GetNormal();
    KeyFrame* GetReferenceKeyFrame();

    //得到所有能看到这个点的关键帧，以及点在这个关键帧里面的序号
    std::map<KeyFrame*,size_t> GetObservations();
    
    //返回这个点被观测到的次数。
    int Observations();

这些函数都是返回共有变量。

 

 3  //输入的是看到这个点的关键帧，和这个点在关键帧里面的序号，函数增加Observation的值。
    void AddObservation(KeyFrame* pKF,size_t idx);
    //删除Observations里面，对应的pKF
    void EraseObservation(KeyFrame* pKF);

这两个函数都是改变mObservations， std::map<KeyFrame*,size_t> mObservations; first 表示在哪个关键帧被观测到，second表示该地图点在这个关键帧的编号是多少。

 4  //输入的是看到这个点的关键帧，和这个点在关键帧里面的序号，函数增加Observation的值。
    void AddObservation(KeyFrame* pKF,size_t idx);
    //删除Observations里面，对应的pKF
    void EraseObservation(KeyFrame* pKF);
  
    //得到该地图点在给的关键帧里面的编号
    int GetIndexInKeyFrame(KeyFrame* pKF);
    //该点是不是在这关键帧里面
    bool IsInKeyFrame(KeyFrame* pKF);
    //这个点不好的话，我们要删除，所有关键帧中的这个点。map中的这个点也要删除。
    void SetBadFlag();
    bool isBad();
        //替换关键帧里面的地图点为pMp;
    void Replace(MapPoint* pMP);    
    MapPoint* GetReplaced();

    void IncreaseVisible(int n=1);
    void IncreaseFound(int n=1);

 

这些函数实现都和简单，细看源代码，这里说下setBadFlag函数，就是讲坏点从这些能看到这个点的关键帧里面删除。void Replace(MapPoint* pMP)函数要将所有看到改点的帧里面的那点换成pMP点。

 

   5 void ComputeDistinctiveDescriptors();

计算最好的描述子，改变的是共有变量mDescriptor 。

具体实现：找出所有能看到这个关键点的关键帧里面这个关键点的描述子，放进一个容器 vDescriptors里面。然后计算vDescriptors不同行的描述子之间的距离，结果放入Distances里面。

然后讲Distances的每行进行从小向大排序，找出每一行的中间那个值median。所有的meadian最小的最为BestMedian,并且记录下该行索引作为最好索引BestIdx=i;然后就得到了我们要找的最好描述子。

6  void UpdateNormalAndDepth();

我们在这个函数里面可以得到，这个函数的最大，最小尺度不变距离，和一个很重要的概念mNormalVector:指的是，所有能看到这个点的参考帧的相机中心到地图点连线组成的所有向量之和的平均值。

 

7    float GetMinDistanceInvariance();
     float GetMaxDistanceInvariance();

返回全局量，没什么说的。

 8 
    int PredictScale(const float &currentDist, KeyFrame*pKF);
    int PredictScale(const float &currentDist, Frame* pF);

两个参数都是输入，该函数返回的是，该地图点在该帧(关键帧)金字塔里面出现的层数。

2 Map.h

里面有个Map类，主要提供一些函数，来维护这个地图，函数都十分简单，自行查看源码。

 

3.KeyFramDataBase.h

类KeyFrameDataBase 主要作用其实就是讲关键帧保存起来，进行闭环检测、重定位时有用。里面有个std::vector<list<KeyFrame*> > mvInvertedFile;所谓的反向索引，每一个叶子节点就是一个”单词“，每一个单词都有一个反向索引里面装的是这个dataBase里面，有这个”单词"的所有关键帧。

  1 KeyFrameDatabase(const ORBVocabulary &voc);

   void add(KeyFrame* pKF);

   void erase(KeyFrame* pKF);

   void clear();

构造函数，add,erase,clear,都比较简单。注意的是，我们加入删除关键帧的时候，都是对反向索引做的操作！！！mvInvertedFile。

 2  // Loop Detection
   std::vector<KeyFrame *> DetectLoopCandidates(KeyFrame* pKF, float minScore);

   // Relocalization
   std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F);

这里的两个函数实现都是差不多的，都是从这个database里面找到与给的帧（关键帧）最相似的。

主要实现：

        第一步:找出与当前dataBase里面有相同word的所有关键帧，并且统计相同word的个数也就是没帧的得分。选出最大的得分，自定义个最小得分。保留超过最小的分的那些关键帧,将得分和关键帧匹配起来放在lScoreAndMatch.push_back(make_pair(si,pKFi));...............我自己觉得这里的原码有个BUG。。。。。。不知道哪位大神可以解释下。

       第二步：找出与 lScoreAndMatch中的每个关键帧有最多共视的10帧，组成一个group. 分别得到这个些groups里面的每一个group的累计得分和有最高得分的那一帧，放入lAccScoreAndMatch。 找出这些累计得分的最大值，自定义最小值。o.75*bestAccScore....只有lAccAcoreAndMactch里面的accScore超过了这个定义的最最小值，这个帧pBestKF才会被装入vpLoopCandidates中，作为候选的闭环帧。

 

 

问题求解释：

/这里来检查闭环的候选者。里面有很多思想，论文里面没有写出来的，这里我们可以读出来。


vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
    set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
    list<KeyFrame*> lKFsSharingWords;                //与该关键帧有共同word  的关键帧集合

    // Search all keyframes that share a word with current keyframes
    //闭环存在的地方是，与当前帧有共同word的帧们
    
    // Discard keyframes connected to the query keyframe
    //与当前帧有共视点的话，我们就不选择这个，太近了......闭环用处不是特别大。
    {
        unique_lock<mutex> lock(mMutex);

        for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
        {
            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];    //一个个单词，list 装的是有共同word的关键帧

            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
          //讲一个个关键帧取出来。
                KeyFrame* pKFi=*lit;                    
                
                if(pKFi->mnLoopQuery!=pKF->mnId)
                {
                    pKFi->mnLoopWords=0;     //与该关键帧，与当前关键帧共有的word个数。
                    //这里的判断有毛病吧???这样检测不到闭环吧，是闭环肯定有共视啊！！！！！！！
                    if(!spConnectedKeyFrames.count(pKFi))
                    {
                        pKFi->mnLoopQuery=pKF->mnId;
                        lKFsSharingWords.push_back(pKFi);
                    }
                }
                pKFi->mnLoopWords++;
            }
        }
    }

    if(lKFsSharingWords.empty())
        return vector<KeyFrame*>();

    list<pair<float,KeyFrame*> > lScoreAndMatch;

    // Only compare against those keyframes that share enough words
    int maxCommonWords=0;      
    
    //找出与当前帧，有最多相同word数目。
    
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnLoopWords>maxCommonWords)
            maxCommonWords=(*lit)->mnLoopWords;
    }

    int minCommonWords = maxCommonWords*0.8f;

    int nscores=0;

    // Compute similarity score. Retain the matches whose score is higher than minScore
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;

        if(pKFi->mnLoopWords>minCommonWords)
        {
            nscores++;

            float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);  //当前关键帧和 pKFi的得分

            pKFi->mLoopScore = si;    // 得分。
            
            //得分，和对应的关键帧放进这个里面。
            if(si>=minScore)
                lScoreAndMatch.push_back(make_pair(si,pKFi));      
        }
    }

    if(lScoreAndMatch.empty())
        return vector<KeyFrame*>();

    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
    float bestAccScore = minScore;

    // Lets now accumulate score by covisibility
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        KeyFrame* pKFi = it->second;
    
    //a group of keyframes  然后就算累积得分
    //得到跟这个关键帧有最多共视的10个关键帧。
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); 

        float bestScore = it->first;
        float accScore = it->first;    //累积得分/
        
        //最好的关键帧。
        KeyFrame* pBestKF = pKFi;              
        
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)
            {
                accScore+=pKF2->mLoopScore;     //累积得分。
                if(pKF2->mLoopScore>bestScore)
                {
                    pBestKF=pKF2;           //最好的关键帧换了,共视group（10)里面有最高得分的那个。
                    bestScore = pKF2->mLoopScore;
                }
            }
        }

        //得到的是，在共识group（10）里面得分最高的。（累积分数，最好的帧）
        
        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));   
        if(accScore>bestAccScore)
            bestAccScore=accScore;
    }

    // Return all those keyframes with a score higher than 0.75*bestScore
    float minScoreToRetain = 0.75f*bestAccScore;

    set<KeyFrame*> spAlreadyAddedKF;
    vector<KeyFrame*> vpLoopCandidates;
    vpLoopCandidates.reserve(lAccScoreAndMatch.size());

    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    {
        if(it->first>minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            if(!spAlreadyAddedKF.count(pKFi))
            {
          //有没有多余的感觉呢?
                vpLoopCandidates.push_back(pKFi);   
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

//这里思路理一下。 1，找出与当前帧有共同word的关键帧们，取一个伐值，必须要超过多少个
//共同word才能被选。2，找出与1中找出的所有关键帧中，有最多共视点的10个帧，组成一个group，
//然后计算累计得分，这里找出group中使得bestscore最大的关键帧，改关键帧可能是与有相同word的关键帧的
//有很多的共视的关键帧。  3，找出累积得分最大的 参考关键帧们。（可能是有相同的Word关键帧，或者是有很多共视的关键帧）
    return vpLoopCandidates;
}