关键帧与地图点（一）：地图点

本次我们主要讲解ORBSLAM2中的地图点，首先我们来看一下论文中对于地图点的定义：

ORBSLAM2中的每个地图点$p_i$存储了以下内容：

1. 在世界坐标系下的3D位置$X_{w,i}$
2. 地图点的观测方向$n_i$，为归一化后的平均观测方向（指连接该地图点和其对应观测关键帧光心的单位方向向量的均值）
3. 一个具有代表性ORB描述子$D_i$，与其他所有能观测到该点云的关键帧中ORB描述子相比，该描述子的汉明距离中值最小
4. 根据ORB特征点的尺度不变性获取到的可以观察到这个地图点的的最大$d_{max}$和最小$d_{min}$距离。

对照代码，分别为

    // MapPoint在世界坐标系下的3D位置
    cv::Mat mWorldPos; 

    // 观测到该MapPoint的KF和该MapPoint在KF中的索引
    std::map<KeyFrame*,size_t> mObservations; 

    // 该MapPoint的平均观测方向
    // 用于判断点是否在可视范围内
    cv::Mat mNormalVector;

	// 最具代表性的描述子
    // 每个3D点也有一个描述子，但是这个3D点可以观测多个二维特征点，从中选择一个最有代表性的
     //通过 ComputeDistinctiveDescriptors() 得到的最有代表性描述子,距离其它描述子的平均距离最小
    cv::Mat mDescriptor; 

    // 通常情况下MapPoint的参考关键帧就是创建该MapPoint的那个关键帧
    KeyFrame* mpRefKF;

    /// 地图点被跟踪到的次数
    int mnVisible;
    int mnFound;

    /// Bad 标志 (we do not currently erase MapPoint from memory)
    bool mbBad;
    
    //? 替换本地图点的点? 
    MapPoint* mpReplaced;

    /// 尺度不变性距离
    float mfMinDistance;
    float mfMaxDistance;

    ///所属的地图
    Map* mpMap;

    ///对当前地图点位姿进行操作的时候的互斥量
    std::mutex mMutexPos;
    ///对当前地图点的特征信息进行操作的时候的互斥量
    std::mutex mMutexFeatures;

接下来我们需要关注几个和地图点相关的非常重要的函数

• 计算平均观测方向和尺度不变性距离

  /**
   * @brief 更新地图点的平均观测方向、观测距离范围
   *
   */
  void MapPoint::UpdateNormalAndDepth()
  {
      // Step 1 获得观测到该地图点的所有关键帧、坐标等信息
      map<KeyFrame*,size_t> observations;
      // 地图点的参考关键帧
      KeyFrame* pRefKF;
      // 地图点的3D位置
      cv::Mat Pos;
      {
          // 加锁
          unique_lock<mutex> lock1(mMutexFeatures);
          unique_lock<mutex> lock2(mMutexPos);
  
          // 不处理标记为Bad的点
          if(mbBad)
              return;
  
          observations=mObservations; // 获得观测到该地图点的所有关键帧
          pRefKF=mpRefKF;             // 观测到该点的参考关键帧（第一次创建时的关键帧）
          Pos = mWorldPos.clone();    // 地图点在世界坐标系中的位置
      }
  
      // 如果观测为空，则不做处理
      if(observations.empty())
          return;
  
      // Step 2 计算该地图点的平均观测方向
  
      // 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向
      // 初始值为0向量，累加为归一化向量，最后除以总数n
      cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
      int n=0;
  
      // 遍历观测到该地图点的所有关键帧
      for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
      {
          KeyFrame* pKF = mit->first;
          // 获取关键帧对应的相机光心位置
          cv::Mat Owi = pKF->GetCameraCenter();
          // 获得连接地图点和观测到它关键帧的光心的向量并归一化
          cv::Mat normali = mWorldPos - Owi;
          normal = normal + normali/cv::norm(normali);                       
          n++;
      } 
  
      cv::Mat PC = Pos - pRefKF->GetCameraCenter();                           // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示）
      const float dist = cv::norm(PC);                                        // 该点到参考关键帧相机的距离
      const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;        // 获取该地图点在参考帧中对应的特征点的金字塔层数
      const float levelScaleFactor =  pRefKF->mvScaleFactors[level];          // 该金字塔层数对应的尺度因子，scale^n，scale=1.2，n为层数
      const int nLevels = pRefKF->mnScaleLevels;                              // 金字塔总层数，默认为8
  
      {
          unique_lock<mutex> lock3(mMutexPos);
          // 使用方法见PredictScale函数前的注释
          mfMaxDistance = dist*levelScaleFactor;                              // 观测到该点的距离上限
          mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];    // 观测到该点的距离下限
          mNormalVector = normal/n;                                           // 获得地图点平均的观测方向
      }
  }
  

• 计算最具代表性的描述子

  /**
   * @brief 计算地图点最具代表性的描述子
   *
   * 由于一个地图点会被许多相机观测到，因此在插入关键帧后，需要判断是否更新代表当前点的描述子 
   * 先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值
   */
  void MapPoint::ComputeDistinctiveDescriptors()
  {
      // 检索所有观测到的描述子
      vector<cv::Mat> vDescriptors;
  
      // 该地图点的所有观测
      map<KeyFrame*,size_t> observations;
  
      // Step 1 获取该地图点所有有效的观测关键帧信息
      {
          unique_lock<mutex> lock1(mMutexFeatures);
          if(mbBad)
              return;
          observations=mObservations;
      }
  
      // 如果观测为空直接返回
      if(observations.empty())
          return;
  
      vDescriptors.reserve(observations.size());
  
      // Step 2 遍历观测到该地图点的所有关键帧，对应的orb描述子，放到向量vDescriptors中
      for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
      {
          // mit->first取观测到该地图点的关键帧
          // mit->second取该地图点在关键帧中的索引
          KeyFrame* pKF = mit->first;
  
          if(!pKF->isBad())        
              // 取对应的描述子向量                                               
              vDescriptors.push_back(pKF->mDescriptors.row(mit->second));     
      }
  
      if(vDescriptors.empty())
          return;
  
  
      // Step 3 计算这些描述子两两之间的距离
      // N表示为一共多少个描述子
      const size_t N = vDescriptors.size();
  	
      // 将Distances表述成一个对称的矩阵
      // float Distances[N][N];
  	std::vector<std::vector<float> > Distances;
  	Distances.resize(N, vector<float>(N, 0));
  	for (size_t i = 0; i<N; i++)
      {
          // 和自己的距离当然是0
          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++)
      {
          // 第i个描述子到其它所有描述子之间的距离
          // vector vDists(Distances[i],Distances[i]+N);
  		vector<int> vDists(Distances[i].begin(), Distances[i].end());
  
          // 从小到大排序
  		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();       
      }
  }
  

• 根据地图点距离关键帧/普通帧相机光心的距离来预测地图点对应的观测应该在哪一层的金字塔

  // 下图中横线的大小表示不同图层图像上的一个像素表示的真实物理空间中的大小
  //              ____
  // Nearer      /____\     level:n-1 --> dmin
  //            /______\                       d/dmin = 1.2^(n-1-m)
  //           /________\   level:m   --> d
  //          /__________\                     dmax/d = 1.2^m
  // Farther /____________\ level:0   --> dmax
  //
  //           log(dmax/d)
  // m = ceil(------------)
  //            log(1.2)
  // 这个函数的作用:
  // 在进行投影匹配的时候会给定特征点的搜索范围,考虑到处于不同尺度(也就是距离相机远近,位于图像金字塔中不同图层)的特征点受到相机旋转的影响不同,
  // 因此会希望距离相机近的点的搜索范围更大一点,距离相机更远的点的搜索范围更小一点,所以要在这里,根据点到关键帧/帧的距离来估计它在当前的关键帧/帧中,
  // 会大概处于哪个尺度
  
  /**
   * @brief 预测地图点对应特征点所在的图像金字塔尺度层数
   * 
   * @param[in] currentDist   相机光心距离地图点距离
   * @param[in] pKF           关键帧
   * @return int              预测的金字塔尺度
   */
  int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF)
  {
      // dmax / d
      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;
  }
  

  这里用到了高中数学中的换底公式，也就是
  $$m=lo{g}_{1,2}^{\frac{d_{max}}{d}}=\frac{lo{g}_e^{\frac{d_{max}}{d}}}{lo{g}_e^{1.2}}$$