ORB-SLAM3 Local Mapping 源码分析（二）

2021SC@SDUSC

2、地图点（MapPoint）的删除
  在上一步(关键帧的插入)中会将一部分地图点加入到mlpRecentAddedMapPoints这个vector中，而在这一步骤中需要剔除地图（Map）中新加入但质量不好的点。

利用函数MapPointCulling()

void LocalMapping::MapPointCulling()
{
    list::iterator lit = mlpRecentAddedMapPoints.begin();
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;

// 第一步：根据不同类型的相机设置不同的观测阈值
    int nThObs;
    if(mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;
// 第二步：遍历检查的新添加的MapPoints
    int borrar = mlpRecentAddedMapPoints.size();

    while(lit!=mlpRecentAddedMapPoints.end())
    {
        MapPoint* pMP = *lit;

        if(pMP->isBad())

   // 第三步：如果已经是坏的MapPoints，直接从检查链表中删除
            lit = mlpRecentAddedMapPoints.erase(lit);

        else if(pMP->GetFoundRatio()<0.25f)
        {
  // 第四步：跟踪到该MapPoint的Frame数，若相比预计可观测到该MapPoint的Frame数的比例小于25%，那么删除
         // (mnFound/mnVisible） < 25%
         // 其中mnFound 表示地图点被多少帧看到，次数越多越好
         // 其中mnVisible表示地图点应该被看到的次数
         // (mnFound/mnVisible）：对于宽FOV镜头这个比例会高，对于窄FOV镜头这个比例会低
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
        {
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
        {
            lit++;
            borrar--;
        }
    }
    //cout << "erase MP: " << borrar << endl;
}

其中评价一个点是坏点需要被删除掉的条件为：
（1）IncreaseFound / IncreaseVisible < 0.25
（2）观测到该点的关键帧数量太少

3、新地图点的插入（CreateNewMapPoints()）

这一步很长，也非常关键。

设置双目和RGBD的公示关键帧数为10，单目为20，这里仍然以单目为例，用mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn)得到与当前关键帧共视程度最高的20帧相邻关键帧，然后用ORBmatcher matcher(0.6,false)对特征点匹配进行配置，最佳距离 < 0.6*次佳距离。分别取出当前关键帧从世界坐标系到相机坐标系的旋转和平移矩阵，组成变换矩阵Tcw1。然后进一步得到当前关键帧光心在世界坐标系中的坐标以及相机的内参。ratioFactor变量用于点的深度验证，nnew变量用来记录成功三角化的地图点数目。

void LocalMapping::CreateNewMapPoints()
{

    // 定义变量nn表示搜索最佳共视关键帧的数目
    // 不同传感器的要求不一样,单目需要有更多的具有较好共视关系的关键帧来建立地图
    int nn = 10;
    if(mbMonocular)
        nn=20;

    // 第一步：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧
    const vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

    // 特征点匹配配置 最佳距离 < 0.6*次佳距离。
    ORBmatcher matcher(0.6,false); 

    // 取出当前帧从世界坐标系到相机坐标系的变换矩阵
    cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();
    cv::Mat Rwc1 = Rcw1.t(); //R为正交矩阵，矩阵的逆即为其转置
    cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
    cv::Mat Tcw1(3,4,CV_32F);
    Rcw1.copyTo(Tcw1.colRange(0,3));
    tcw1.copyTo(Tcw1.col(3));

    // 得到当前关键帧光心在世界坐标系中的坐标和内参。
    cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
    const float &fx1 = mpCurrentKeyFrame->fx;
    const float &fy1 = mpCurrentKeyFrame->fy;
    const float &cx1 = mpCurrentKeyFrame->cx;
    const float &cy1 = mpCurrentKeyFrame->cy;
    const float &invfx1 = mpCurrentKeyFrame->invfx;
    const float &invfy1 = mpCurrentKeyFrame->invfy;

    // 用于后面的点深度的验证;这里的1.5是经验值
    // mfScaleFactor = 1.2
    const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;

    // 记录三角化成功的地图点数目
    int nnew=0;
// 第二步：遍历相邻关键帧vpNeighKFs
    for(size_t i=0; i0 && CheckNewKeyFrames())
            return;

        KeyFrame* pKF2 = vpNeighKFs[i];

        // Check first that baseline is not too short
        // 邻接的关键帧光心在世界坐标系中的坐标
        cv::Mat Ow2 = pKF2->GetCameraCenter();
        // 基线向量，两个关键帧间的相机位移
        cv::Mat vBaseline = Ow2-Ow1;
        // 基线长度
        const float baseline = cv::norm(vBaseline);

        // 第三步：判断相机运动的基线是不是足够长
        if(!mbMonocular)
        {
            // 如果是双目相机，关键帧间距小于本身的基线时不生成3D点
            // 因为太短的基线下能够恢复的地图点不稳定
            if(baselinemb)
            continue;
        }
        else    
        {
            // 单目相机情况
            // 邻接关键帧的场景深度中值
            const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
            // baseline与景深的比例
            const float ratioBaselineDepth = baseline/medianDepthKF2;
            // 如果比例特别小，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点
            if(ratioBaselineDepth<0.01)
                continue;
        }
// 第四步：根据两个关键帧的位姿计算它们之间的基础矩阵
        cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);