ORB-SLAM2代码阅读笔记：LocalMapping和三角测

• void LocalMapping::Run()
  局部建图主程序：
  当等待列表不为空时，进入程序：

  • 计算关键帧特征点的BoW映射，将关键帧插入地图
  • ProcessNewKeyFrame函数中引入的不合格MapPoints
  • 相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints
  • 处理完队列中最后一个关键帧时，检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
  • 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping，当局部地图中的关键帧大于2个的时候进行局部地图的BA

• void LocalMapping::ProcessNewKeyFrame()
  计算关键帧特征点的BoW映射，将关键帧插入地图，每次只对一帧进行操作：

  • 计算该关键帧特征点的Bow映射关系
  • 跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定

  const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
  // 对当前处理的这个关键帧中的所有的地图点展开遍历
  for(size_t i=0; i<vpMapPointMatches.size(); i++)
  {
      MapPoint* pMP = vpMapPointMatches[i];
      if(pMP)
      {
          if(!pMP->isBad())
          {
              // 非当前帧生成的MapPoints
              // 为当前帧在tracking过程跟踪到的MapPoints更新属性
              if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
              {
                  // 添加观测
                  pMP->AddObservation(mpCurrentKeyFrame, i);
                  // 获得该点的平均观测方向和观测距离范围
                  pMP->UpdateNormalAndDepth();
                  // 加入关键帧后，更新3d点的最佳描述子
                  pMP->ComputeDistinctiveDescriptors();
              }
              else // this can only happen for new stereo points inserted by the Tracking
              {
                  mlpRecentAddedMapPoints，等待检查
                  // CreateNewMapPoints函数中通过三角化也会生成MapPoints
                  // 这些MapPoints都会经过MapPointCulling函数的检验
                  mlpRecentAddedMapPoints.push_back(pMP);  // 认为这些由当前关键帧生成的地图点不靠谱,将其加入到待检查的地图点列表中
              }
          }
      }
  }    
  

  • 更新关键帧之间的连接关系
  • 讲关键帧插入到 地图中

• void LocalMapping::MapPointCulling()
  剔除 ProcessNewKeyFrame 和 CreateNewMapPoints 函数中引入的质量不好的 MapPoints

  • 跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例需大于25%
  • 从该点建立开始，到现在已经过了不小于2个关键帧
  • 从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点

• void LocalMapping::CreateNewMapPoints()
  相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints，

  • 在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs
  • 遍历相邻关键帧，根据两个关键帧的位姿计算它们之间的基本矩阵
  • 通过极线约束限制匹配时的搜索范围，进行特征点匹配
  • 对每对匹配通过三角化生成3D点
    三角化恢复3D点

    cv::Mat A(4,4,CV_32F);
              A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
              A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
              A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
              A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);
  
              cv::Mat w,u,vt;
              cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
              x3D = vt.row(3).t();
              // 归一化之前的检查
              if(x3D.at<float>(3)==0)
                  continue;
              // Euclidean coordinates 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标
              x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
  

  • 之后进行检验（检验地图点在两帧中的深度，在两帧中的重投影误差，尺度连续性）），如果检验通过则建立当前帧的地图点及其属性（a.观测到该MapPoint的关键帧 b.该MapPoint的描述子 c.该MapPoint的平均观测方向和深度范围）

  // step 6.8：三角化生成3D点成功，构造成MapPoint
          MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
  
          // step 6.9：为该MapPoint添加属性：
          // a.观测到该MapPoint的关键帧
          // b.该MapPoint的描述子
          // c.该MapPoint的平均观测方向和深度范围
          pMP->AddObservation(mpCurrentKeyFrame,idx1);            
          pMP->AddObservation(pKF2,idx2);
  
          mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
          pKF2->AddMapPoint(pMP,idx2);
  
          pMP->ComputeDistinctiveDescriptors();
  
          pMP->UpdateNormalAndDepth();
  
          mpMap->AddMapPoint(pMP);
  
          // step 6.10：将新产生的点放入检测队列
          // 这些MapPoints都会经过MapPointCulling函数的检验
          mlpRecentAddedMapPoints.push_back(pMP);
  
          nnew++;
  

• void LocalMapping::SearchInNeighbors()
  检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints

  • 获取相邻关键帧和相邻关键帧的相邻关键帧

  // 候选的一级相邻关键帧
  const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
  // 筛选之后的一级相邻关键帧以及二级相邻关键帧
  vector<KeyFrame*> vpTargetKFs;
  // 开始对所有候选的一级关键帧展开遍历：
  for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
  {
      KeyFrame* pKFi = *vit;
      // 没有和当前帧进行过融合的操作
      if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)
          continue;
      vpTargetKFs.push_back(pKFi);// 加入一级相邻帧
      pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;// 并标记已经加入
  
      // Extend to some second neighbors
      const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
      // 遍历得到的二级相邻关键帧
      for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
      {
          KeyFrame* pKFi2 = *vit2;
          // 当然这个二级关键帧要求没有和当前关键帧发生融合,并且这个二级关键帧也不是当前关键帧
          if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
              continue;
          vpTargetKFs.push_back(pKFi2);// 存入二级相邻帧
      }
  }
  

  • 将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合，这里用的ORBMatcher中的Fuse()，ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint > &vpMapPoints, const float th)将地图点投影到关键帧的方式进行特征匹配选取最优的匹配点，如果匹配点与当前点的描述子距离小于阈值的情况下进行融合。融合的方式是用被观察次数大的地图点代替被观察次数少的地图点，用函数MapPoint::Replace(MapPoint pMP)融合，融合地图点作如下操作：（1）将被替换地图点的被观察次数,被查找次数,以及观察到该地图点的关键帧都清空,坏地图点标志置位（2）将被替换地图点的被观察次数,被查找次数都加到替换地图点pMP中,并将当前地图点在关键帧中的位置用代替地图点代替（3）最后将本地图点从全局地图map中删除

  这其中的融合就是删除冗余关键帧和地图点，
  - 更新当前帧MapPoints的描述子，深度，观测主方向等属性
  - 更新当前帧的MapPoints后更新与其它帧的连接关系,更新covisibility图

• cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
  根据两关键帧的姿态计算两个关键帧之间的基本矩阵，原理可以看高翔的SLAM十四讲

  cv::Mat R1w = pKF1->GetRotation();
  cv::Mat t1w = pKF1->GetTranslation();
  cv::Mat R2w = pKF2->GetRotation();
  cv::Mat t2w = pKF2->GetTranslation();
  
  cv::Mat R12 = R1w*R2w.t();
  
  cv::Mat t12 = -R1w*R2w.t()*t2w+t1w;
  
  // 得到 t12 的反对称矩阵
  cv::Mat t12x = SkewSymmetricMatrix(t12);
  
  const cv::Mat &K1 = pKF1->mK;
  const cv::Mat &K2 = pKF2->mK;
  
  // Essential Matrix: t12叉乘R12
  // Fundamental Matrix: inv(K1)*E*inv(K2)
  return K1.t().inv()*t12x*R12*K2.inv();
  

void LocalMapping::KeyFrameCulling()
关键帧剔除,在Covisibility Graph中的关键帧，其90%以上的MapPoints能被其他关键帧（至少3个）观测到，则认为该关键帧为冗余关键帧。这个函数用于数据融合

        // STEP3：遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到
        for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
        {
            MapPoint* pMP = vpMapPoints[i];
            if(pMP)
            {
                if(!pMP->isBad())
                {
                    if(!mbMonocular)
                    {
                        // 对于双目，仅考虑近处的MapPoints，不超过mbf * 35 / fx 
                        // 不过配置文件中给的是近点的定义是 40 * fx
                        if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
                            continue;
                    }

                    nMPs++;
                    // MapPoints至少被三个关键帧观测到
                    if(pMP->Observations()>thObs)
                    {
                        const int &scaleLevel = pKF->mvKeysUn[i].octave;
                        const map<KeyFrame*, size_t> observations = pMP->GetObservations();
                        // 判断该MapPoint是否同时被三个关键帧观测到
                        int nObs=0;
                        // 遍历当前这个邻接关键帧上的地图点的所有观测信息
                        for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
                        {
                            KeyFrame* pKFi = mit->first;
                            if(pKFi==pKF)
                                continue;
                            const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;

                            // Scale Condition 
                            // 尺度约束，要求MapPoint在该局部关键帧的特征尺度大于（或近似于）其它关键帧的特征尺度
                            if(scaleLeveli<=scaleLevel+1)
                            {
                                nObs++;
                                // 已经找到三个同尺度的关键帧可以观测到该MapPoint，不用继续找了
                                if(nObs>=thObs)
                                    break;
                            }
                        }
                        // 该MapPoint至少被三个关键帧观测到
                        if(nObs>=thObs)
                        {
                            nRedundantObservations++;
                        }
                    }
                }
            }
        }

        // STEP4：该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧
        if(nRedundantObservations>0.9*nMPs)
            // 剔除的时候就设置一个 bad flag 就可以了
            pKF->SetBadFlag();

三角测量

三角测量的用途是用来确定深度信息的，从而确定地图点的三维点坐标。已知匹配特征点对和各自相机矩阵,估计三维点X，存在,都属于模型：

image
image
采用DLT方法，x叉乘PX=0,得到：
image
对于x'有同样关系：
image
可以写作AX=0
通过SVD分解可以得到X的解