ORB-SLAM2代码笔记（三）：KeyFrame

KeyFrame

由于KeyFrame中一部分数据会被多个线程访问修改，因此需要在这些成员中加线程锁，保证同一时间只有一个线程有访问权。涉及线程安全的有：

关键帧位姿的设置（lock(mMutexPose)）；

关键帧间连接关系的设置（lock(mMutexConnections)）；

关键帧对应地图点的操作（lock(mMutexFeatures)），

• void KeyFrame::SetPose(const cv::Mat &Tcw_)
  设置当前关键帧的位姿

• KeyFrame::UpdateConnections()更新图的连接

  • 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键与其它所有关键帧之间的共视程度，对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。

  for(vector::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)//遍历3D点
  {
      MapPoint* pMP = *vit;
      if(!pMP)
          continue;
      if(pMP->isBad())
          continue;
      // 对于每一个MapPoint点，observations记录了可以观测到该MapPoint的所有关键帧
      map observations = pMP->GetObservations();
      for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
      {
          // 除去自身，自己与自己不算共视
          if(mit->first->mnId==mnId)
              continue;
          KFcounter[mit->first]++;
      }
  }
  

  • 权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）

      // 对于一个和当前关键帧具有共视关系的关键帧
  for(map::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
  {
      if(mit->second>nmax)
      {
          nmax=mit->second;
          // 找到对应权重最大的关键帧（共视程度最高的关键帧）
          pKFmax=mit->first;
      }
      if(mit->second>=th)
      {
          // 对应权重需要大于阈值，对这些关键帧建立连接
          vPairs.push_back(make_pair(mit->second,mit->first));
          (mit->first)->AddConnection(this,mit->second);
      }
  }
  // 如果没有超过阈值的权重，则对权重最大的关键帧建立连接
  if(vPairs.empty())
  {
      mConnectedKeyFrameWeights
      // 这是对之前th这个阈值可能过高的一个补丁
      vPairs.push_back(make_pair(nmax,pKFmax));
      pKFmax->AddConnection(this,nmax);
  }
  

  • 对这些连接按照权重从大到小进行排序，以方便将来的处理更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树

• void KeyFrame::UpdateBestCovisibles()按照权重对连接的关键帧进行排序
  这里声明了一个变量vector > vPairs
  和mConnectedKeyFrameWeights的区别是吧int放在前面，方便排序

• set KeyFrame::GetConnectedKeyFrames()得到与该关键帧连接的关键帧，这里直接遍历mConnectedKeyFrameWeights，把关键帧提出来

• vector KeyFrame::GetVectorCovisibleKeyFrames()
  得到与该关键帧连接的关键帧(已按权值排序)
  直接return mvpOrderedConnectedKeyFrames;这一变量在UpdateBestCovisibles()算好了

• vector> KeyFrame::GetBestCovisibilityKeyFrames(const int &N)
  得到与该关键帧连接的前N个关键帧(已按权值排序)
  vector> KeyFrame::GetCovisiblesByWeight(const int &w)
  权重大于等于w的关键帧

• set KeyFrame::GetMapPoints()获取当前关键帧中的所有地图点
  直接遍历mvpMapPoints就好了

• int KeyFrame::TrackedMapPoints(const int &minObs)关键帧中，大于等于最少观测数目minObs的MapPoints的数量.这些特征点被认为追踪到了

• 还有添加子关键帧，删除子关键帧，改变当前关键帧的父关键帧，获取当前关键帧的子关键帧和父关键帧等

• void KeyFrame::SetBadFlag() 删除关键帧

  • 要删除其他KeyFrame和自己的联系，删除地图点和自己的联系。
  • 这个函数中比较复杂的是 给子关键帧选择父关键帧:

如果这个关键帧有自己的孩子关键帧，告诉这些子关键帧，它们的父关键帧不行了，赶紧找新的父关键帧。
1. 遍历每一个子关键帧，让它们更新它们指向的父关键帧
2. 子关键帧遍历每一个与它相连的关键帧（共视关键帧）
3. 如果子节点找到了可以作为其新父关键帧的帧，则更换父节点
4. 如果没找到，直接把父节点的父节点作为自己的父节点