ORB SLAM2学习笔记之mono_kitti（六）

LocalMapping 线程概述

这个线程接收来自Tracking线程传进来的关键帧，进行一些处理，主要有：将关键帧插入地图，剔除不合格的地图点mappoints，生成新的地图点mappoints，融合当前帧与其他关键帧重复的地图点mappoints，局部BA优化，剔除冗余关键帧，传关键帧到loopclosing线程里等。看到网上有个很好的流程图，如下所示：

---

注意：

• 线程们是一起动作的，也就是说自LocalMapping被构造出来，有一些参数就已经初始化了，就已经和Tracking同时运行。
• 它一直在查看Tracking线程的动作，随时处理传进来的关键帧，同时Tracking也在检查由LocalMapping更新的状态考虑要不要插进新关键帧。

---

SetAcceptKeyFrames

这个函数特别重要，对于单目来说，函数中如果把标志位置为false，那么tracking线程直接就不能产生新的关键帧（Tracking中有个函数NeedNewKeyFrame 会返回false，false的话不可以生成新关键帧不可插入关键帧队列）不能传给LocalMapping线程。
当处理完一个关键帧的时候，函数flag为true，可以接受新关键帧。

ProcessNewKeyFrame

函数用于计算关键帧特征点的BoW映射，将关键帧插入地图。

• 第一步：从缓冲队列中取出一帧关键帧

{
     unique_lock<mutex> lock(mMutexNewKFs);
     // 从列表中获得一个等待被插入的关键帧
     mpCurrentKeyFrame = mlNewKeyFrames.front();
     mlNewKeyFrames.pop_front();
}

其中mlNewKeyFrames是Tracking线程向LocalMapping中插入的关键帧序列。

• 第二步：计算该关键帧特征点的Bow映射关系

mpCurrentKeyFrame->ComputeBoW();

• 第三步：跟踪局部地图过程中新匹配上的mappoints和当前关键帧绑定

    // 在TrackLocalMap函数中将局部地图中的MapPoints与当前帧进行了匹配，
    // 但没有对这些匹配上的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.push_back(pMP);
                }
            }
        }
    }

意思就是在跟踪局部地图（ TrackLocalMap ）的时候，进行了局部 mappoints 的投影用于优化位姿，这些点并不是当前帧产生的，但是并没有对这些地图点进行与当前关键帧的连接，需要对这些点更新各种属性，这时执行 if 语句。
（相似的更新属性过程在 Tracking 线程的 CreateInitialMapMonocular 函数中。）
（当然，单目刚初始化成功的时候，mappoints肯定都是当前帧里的，会进入 else 语句中执行更新mlpRecentAddedMapPoints）

• 第四步：更新关键帧间的连接关系

mpCurrentKeyFrame->UpdateConnections();

详情见Tracking线程。

• 第五步：将该关键帧插入到地图中

mpMap->AddKeyFrame(mpCurrentKeyFrame);

Map类中的 mspKeyFrames会更新。

MapPointCulling

函数用于剔除不好的mappoints，何为不好呢。。。

• ①坏点需要剔除（mbBad为 true）

if(pMP->isBad())
     {
         lit = mlpRecentAddedMapPoints.erase(lit);
     }

• ②跟踪到该mappoint的Frame数相比预计可观测到该mappoint的Frame数的比例小于1/4的，也就是说 能观测到它的帧数远远大于所能跟踪到他的帧数 （一般来说能跟踪到的一定能观测到，但观测到的不一定能跟踪上）

else if(pMP->GetFoundRatio()<0.25f)
     {
         pMP->SetBadFlag();
         lit = mlpRecentAddedMapPoints.erase(lit);
     }

• ③从该mappoint建立的那一关键帧起，到当前关键帧已经 ≥ 2，但观测数却 ≤ 2
  （注意，刚初始化生成mappoints的时候，mappoints的第一帧id号为初始帧的下一帧，并且在添加观测的时候已经同时把初始帧和初始帧的下一帧都作为观测帧，这时候已经满足了所有的点有了两帧的观测，如果需要新的关键帧，会对mappoints再添加一次观测，如果mappoints是好点的话不会被剔除掉的）

else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
     {
         pMP->SetBadFlag();
         lit = mlpRecentAddedMapPoints.erase(lit);
     }

另外的，如果从该mappoint建立的那一关键帧起，到当前关键帧已经 ≥ 3，说明经过层层筛选已经是个好点了，这是也把它逐出队列，但是不把它置为坏点，只是后续不再用到它。

else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
         lit = mlpRecentAddedMapPoints.erase(lit);

CreateNewMapPoints

此函数用于相机运动过程中与相邻关键帧通过三角化生成一些新的mappoints（单目有两处生成新的mappoints：①单目初始化；②此函数）

• 第一步：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs

    const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

• 第二步：得到当前关键帧位姿矩阵，得到光心坐标，焦距等相机参数

    cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();
    cv::Mat Rwc1 = Rcw1.t();
    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;

    const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;

• • • • • • • 遍历vpNeighKFs，得到对每一帧都执行如下操作：

• 第三步： 得到临近关键帧相机光心坐标，和当前帧的基线长度，然后和景深作比较，如果景深过深或者基线太短就放弃这一帧

    cv::Mat Ow2 = pKF2->GetCameraCenter();
    cv::Mat vBaseline = Ow2-Ow1;
    const float baseline = cv::norm(vBaseline);

    const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
    const float ratioBaselineDepth = baseline/medianDepthKF2;

    if(ratioBaselineDepth<0.01)
        continue;

• 第四步：根据这两帧计算F矩阵，根据矩阵计算匹配点索引，存在vMatchedIndices里。

     // Compute Fundamental Matrix
     cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);

     // Search matches that fullfil epipolar constraint
     vector<pair<size_t,size_t> > vMatchedIndices;
     matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);

---

SearchForTriangulation

这种前缀带有Search的都存到ORBmatcher类里 ，处理方式都差不多，这里的处理方式是生成BoW向量（一堆节点）：

• BoW向量FeatureVector继承于std中的map类，结构为{(node1,feature_vector1) (node2,feature_vector2)…}
• 迭代器的first为node的编号，second为该node的所有特征点编号（feature_vector1）。

首先遍历两个关键帧中的节点，如果是同一个节点（节点编号一样），那么就遍历该节点下所有的特征点索引，取出该索引的关键点keypoint，取出相应描述子，比较两个描述子之间的距离，距离很大就查找下一个，一点一点遍历，更新最小距离，筛选不好的点，例如像素坐标系下特征点如果离极点太近的话就不合格，最后如果特征点到极线距离小于一定距离就更新最佳索引和最佳距离（对于一对匹配点来讲）。

---

• 第五步：三角化每一对点（如果满足条件的话）

①先将特征点反投影到归一化平面：

    cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);
    cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);

②然后将两个归一化平面上的点所代表的射线旋转到世界坐标系（注意，这里没有加平移t，所以其实他们只是在各自的归一化平面一起向世界坐标系旋转了一下，和世界坐标系原点三点不在同一直线上）

    cv::Mat ray1 = Rwc1*xn1;
    cv::Mat ray2 = Rwc2*xn2;
    const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));

这里计算了夹角的余弦，也就是所谓的 视差角 的余弦。
③三角化视差余弦小一点的3d点（前面的应该都满足，最后阈值可以自己定）

    if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
            {
                
                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);
            }

• 第六步：检查
  ①3d点深度是否合理（是否在前方）
  ②重投影误差是否过大
  ③尺度连续性检查

• 第七步：生成mappoints，添加属性

    MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
    pMP->AddObservation(mpCurrentKeyFrame,idx1);            
    pMP->AddObservation(pKF2,idx2);
    mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
    pKF2->AddMapPoint(pMP,idx2);

    pMP->ComputeDistinctiveDescriptors();

    pMP->UpdateNormalAndDepth();

    mpMap->AddMapPoint(pMP);

SearchInNeighbors

函数用于检查并融合当前关键帧与相邻帧（分为两级）重复的MapPoints

• 第一步：获得当前关键帧在covisibility图中权重排名前20的邻接关键帧

    if(mbMonocular)
        nn=20;
    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进行融合

vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;

        // 投影当前帧的MapPoints到相邻关键帧pKFi中，并判断是否有重复的MapPoints
        // 1.如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）
        // 2.如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint
        matcher.Fuse(pKFi,vpMapPointMatches);
    }

• 第四步：将一级二级相邻帧的mappoints分别与当前帧的mappoints进行融合

    vector<MapPoint*> vpFuseCandidates;
    vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());

    // 步骤3：将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合
    // 遍历每一个一级邻接和二级邻接关键帧
    for(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
    {
        KeyFrame* pKFi = *vitKF;

        vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();

        // 遍历当前一级邻接和二级邻接关键帧中所有的MapPoints
        for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
        {
            MapPoint* pMP = *vitMP;
            if(!pMP)
                continue;
            
            // 判断MapPoints是否为坏点，或者是否已经加进集合vpFuseCandidates
            if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
                continue;

            // 加入集合，并标记已经加入
            pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
            vpFuseCandidates.push_back(pMP);
        }
    }

    matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);

两步融合的区别（我认为）在于：第一步是把当前帧的所有mappoints（注意：是所有）分别投到一二级关键帧里，进行融合选择；而第二步是把一二级所有的mappoints投射到当前关键帧里进行融合，感觉中间是有交集但是不全是重合的。

• 第五步：更新属性

    vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
    {
        MapPoint* pMP=vpMapPointMatches[i];
        if(pMP)
        {
            if(!pMP->isBad())
            {
                // 在所有找到pMP的关键帧中，获得最佳的描述子
                pMP->ComputeDistinctiveDescriptors();

                // 更新平均观测方向和观测距离
                pMP->UpdateNormalAndDepth();
            }
        }
    }
    mpCurrentKeyFrame->UpdateConnections();

感觉每操作完一次mappoints都会对其进行更新。

KeyFrameCulling

• 第一步：根据Covisibility Graph提取当前帧的共视关键帧（按顺序排好）

vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();

• • • • • • • • • 遍历共视关键帧
• 第二步：提取每个共视关键帧的mappoints

const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();

• • • • • • • 遍历每个共视关键帧的mappoints
• 第三步：判断是否有三个关键帧观测到该mappoints，如果有的话遍历他们确保尺度状况一样

        for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
        {
            MapPoint* pMP = vpMapPoints[i];
            if(pMP)
            {
                if(!pMP->isBad())
                {

                    nMPs++;
                    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++;
                                if(nObs>=thObs)
                                    break;
                            }
                        }
                        // 该MapPoint至少被三个关键帧观测到
                        if(nObs>=thObs)
                        {
                            nRedundantObservations++;  // + 1
                        }
                    }
                }
            }
        }

• 第四步：判断并剔除
  若该局部关键帧90%以上的mappoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧

       if(nRedundantObservations>0.9*nMPs)
            pKF->SetBadFlag();

相关博客

线程的各种停止标志位和与其他线程的通信详情见这个博客：https://blog.csdn.net/hzwwpgmwy/article/details/80493247

（会持续更新～