orb-slam2 从单目开始的简单学习（8）：LocalMapping

1. LocalMapping总览

在mono_tum.cc文件中在System SLAM(...)中初始化了一个slam系统

ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
    //argv[1]:Vocabulary/ORBvoc.txt字典文件路径      argv[2]:Examples/Monocular/TUM1.yaml相机参数文件路径
    //true:启用可视化查看器

在这个系统中通过这个线程运行LocalMapping::Run

mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);//mpMap = new Map();创建出的map
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);//死循环

1.1 初始化LocalMapping

LocalMapping::LocalMapping(Map *pMap, const float bMonocular):
    mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
    mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true)
    //											是否接受关键帧被设为TRUE
{
}

仅对参数进行了赋值

2 Run()

其实本质上是一个死循环

void LocalMapping::Run()
{

    mbFinished = false;

    while(1)
    {
        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(false);//设置mbAcceptKeyFrames：是否接受tracking来的关键帧//创建时被置为True

        // ----------step1.先处理 新关键帧队列中的关键帧----------------------------------  
        if(CheckNewKeyFrames())//检查新关键帧队列 不 为空
        {
            // BoW转换和在Map中插入 BoW conversion and insertion in Map
            ProcessNewKeyFrame();

            // 检查最近的地图点Check recent MapPoints
            MapPointCulling();//在该帧中新生成的地图点需要考验

            // 三角化产生新地图点Triangulate new MapPoints
            CreateNewMapPoints();

            if(!CheckNewKeyFrames())//关键帧队列 为空
            {
                // 在相邻关键帧查找更多匹配项并融合地图点Find more matches in neighbor keyframes and fuse point duplications
                SearchInNeighbors();//公视程度高的进行融合
            }

            mbAbortBA = false;

            if(!CheckNewKeyFrames() && !stopRequested())//（1）关键帧队列 为空 （2）停止标志位为False 进入循环
            {				
                // 进行局部地图BA优化 Local BA
                if(mpMap->KeyFramesInMap()>2)
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

                //检查冗余的本地关键帧 Check redundant local Keyframes
                KeyFrameCulling();
            }

            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);//插入当前关键帧
        }
        else if(Stop())
        {
            // Safe area to stop
            while(isStopped() && !CheckFinish())
            {
                usleep(3000);
            }
            if(CheckFinish())//系统结束
                break;
        }

        ResetIfRequested();

        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(true);

        if(CheckFinish())
            break;

        usleep(3000);
    }

    SetFinish();
}

2.1 ProcessNewKeyFrame

step1.取得新关键帧，计算该KF中的BoW
step2.将地图点和新关键帧关联起来 并 更新常态和描述子
step3 更新共视关系 并 在地图中添加该帧

void LocalMapping::ProcessNewKeyFrame()
{
//--------step1.取得新关键帧，计算该KF中的BoW---------------------------------

    {//----step1.1取得新关键帧----------
        unique_lock<mutex> lock(mMutexNewKFs);
        mpCurrentKeyFrame = mlNewKeyFrames.front();
        mlNewKeyFrames.pop_front();
    }

    //------step1.2 计算当前帧BoW------------------- Compute Bags of Words structures
    mpCurrentKeyFrame->ComputeBoW();//KeyFrames::ComputeBoW()

// ---step2.将地图点和新关键帧关联起来 并 更新常态和描述子 ---------------------Associate MapPoints to the new keyframe and update normal and descriptor
    const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//返回mvpMapPoints： vector<MapPoint*> 
    for(size_t i=0; i<vpMapPointMatches.size(); i++)
    {
        MapPoint* pMP = vpMapPointMatches[i];
        
        if(pMP)//物理存在
        {
            if(!pMP->isBad())//逻辑存在
            {
            //-----step2.1 地图点存在 且 当前帧的该地图点没有记录该帧（即是跟踪到的） 则进行更新-------------------
            
                if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))//判断该地图点是否在当前关键帧中
                {
                //共视关系实际上是针对同一个MapPoint而言的，Observation实际上上是映射了  一个MapPoint 到 多个关键帧 的关系
                    //------step2.1.1将当前帧添加到共视关系中--------------
                    pMP->AddObservation(mpCurrentKeyFrame, i);
                    
                    //------step2.1.2更新平均观测方向和距离；--------------
                    pMP->UpdateNormalAndDepth();
                    
                    //------step2.1.3计算描述子；--------------
                    pMP->ComputeDistinctiveDescriptors();
                }
                //-----step2.2 在该帧中 新生成的 -----------------
                else // 这仅适用于跟踪插入的新立体点this can only happen for new stereo points inserted by the Tracking
                {
                //新生成的需要MapPointCulling()考验
                    mlpRecentAddedMapPoints.push_back(pMP);
                }
            }
        }
    }    
//-----step3 更新共视关系 并 在地图中添加该帧 ----------------------
    // Update links in the Covisibility Graph
    mpCurrentKeyFrame->UpdateConnections();

    // Insert Keyframe in Map
    mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

• AddObservation
  一个帧中的地图点是极有可能被其他关键帧观测到的，因此Observation实际上是建立了一种 一个地图点到多个帧的映射 关系
• UpdateNormalAndDepth
  计算了平均方向和距离

mlpRecentAddedMapPoints会在之后隐性传递给MapPointCulling

2.2 MapPointCulling

仅对ProcessNewKeyFrame放入 mlpRecentAddedMapPoints的 新生成的地图点进行检查和考验

void LocalMapping::MapPointCulling()
{
    // 仅对ProcessNewKeyFrame新生成的地图点进行检查 Check Recent Added MapPoints
    list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;

    int nThObs;
    if(mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;
//step1.遍历新增地图点进行考验（坏点，地图点的查找率）
    while(lit!=mlpRecentAddedMapPoints.end())
    {
        MapPoint* pMP = *lit;
        if(pMP->isBad())//----------step1.1是否为坏点------------------
        {
            lit = mlpRecentAddedMapPoints.erase(lit);
        }

        
        else if(pMP->GetFoundRatio()<0.25f )//----------step1.2地图点的查找率------------------
        {//GetFoundRatio()：返回(mnFound)/mnVisible 
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        
        //该地图点 第一次观察到该地图点的帧id 与当前帧id相隔距离 + 该关键点的被观察到的次数
        //				从创建开始连续3个关键帧内观测数目少于cnThObs
        
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
        {
        //----------step1.3 离第一帧有一定距离 & 共视少------------------
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }

//----------step1.4 离第一帧id>=3------------------
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
            lit = mlpRecentAddedMapPoints.erase(lit);
            
        else//----step1.5 没问题，不置为坏点-------
            lit++;
    }
}

• 消除一个点：先置为坏点SetBadFlag，再删除
  从而不用加锁，提高了并行度

2.2.1 GetFoundRatio()

返回(mnFound)/mnVisible，有点 召回率 那意思

• mnFound:地图点被多少帧（包括普通帧）中的特征点匹配到，帧数越多越好

数量变化除了来自初始化赋值之外，就是来自MapPoint::IncreaseFound函数，该函数在投入tracking线程中track()函数中的系统更新中被TrackLocalMap()调用

• mnVisible:地图点应该被看到的次数

数量变化除了来自初始化赋值之外，就是来自MapPoint::IncreaseVisible函数，该函数在投入tracking线程中track()函数中的系统更新中被TrackLocalMap()的SearchLocalPoints()调用

【可供参考的文档】

2.2 SearchInNeighbors

步骤总结：
step1 通过寻找共视程度最高的关键帧成为vpNeighKFs
step2 遍历所有的共视关键帧寻找未融合关键帧
step3 （当前关键帧---->共视关键帧）将当前地图点融合到各共视关键帧中
step4 （共视关键帧---->当前关键帧）将各共视关键帧的地图点融合到当前关键帧中
step5 更新地图点信息
step5 更新共视图

void LocalMapping::SearchInNeighbors()
{
//--------step1 通过寻找共视程度最高的关键帧成为vpNeighKFs---------------------- Retrieve neighbor keyframes
    int nn = 10;
    if(mbMonocular)
        nn=20;
    const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
    							//得到前nn个最佳公式关键帧
    vector<KeyFrame*> vpTargetKFs;
//---------step2 遍历所有的共视关键帧寻找未融合关键帧-----------------------------
    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;

        //---------step2.1 在当前帧的（一级）共视关键帧的基础上进一步寻找共视关键帧--------------------- Extend to some second neighbors
        const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
        //---------step2.2 遍历 二级关键帧---------------
        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);
        }
    }


    // 根据目标KF中当前KF的投影搜索匹配项Search matches by projection from current KF in target KFs
//-------step3 （当前关键帧---->共视关键帧）将当前地图点融合到各共视关键帧中-----------------
    ORBmatcher matcher;
    vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//获得当前帧中的地图点集
    //-------step3.1 遍历vpTargetKFs进行融合
    for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;
		//   融合帧  当前帧中的地图点集
        matcher.Fuse(pKFi,vpMapPointMatches);//将地图点投影到KeyFrame并搜索重复的地图点
    }


    // 根据当前KF中目标KF的投影搜索匹配项Search matches by projection from target KFs in current KF
//-------step4 （共视关键帧---->当前关键帧）将各共视关键帧的地图点融合到当前关键帧中-----------------
    vector<MapPoint*> vpFuseCandidates;
    vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());
    
    //-------step4.1 遍历所有共视关键帧的地图点将未添加的地图点的地图点放入vpFuseCandidates---------
    for(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
    {
        KeyFrame* pKFi = *vitKF;

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

        for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
        {
            MapPoint* pMP = *vitMP;
            if(!pMP)
                continue;
            if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
                continue;
            pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
            vpFuseCandidates.push_back(pMP);
        }
    }
    
    //-------step4.2将未添加的地图点融合到当前帧中-----------
    matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);


    // ------step5 更新地图点信息---------------------Update points
    vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
    {
        MapPoint* pMP=vpMapPointMatches[i];
        if(pMP)
        {
            if(!pMP->isBad())
            {
                pMP->ComputeDistinctiveDescriptors();
                pMP->UpdateNormalAndDepth();
            }
        }
    }

    // ------step5 更新共视图-----------------Update connections in covisibility graph
    mpCurrentKeyFrame->UpdateConnections();
}

通过matcher.Fuse（A，B）实现将点集B中的关键点融合到帧A中

2.3 CreateNewMapPoints();

意义：用当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳

2.3.1 对极约束 和 三角化

因为单目相机缺少位置信息，所以对极约束和三角化一般是一起使用的。通过对极约束估计相机位姿，使用三角化计算深度。
对极约束 和三角化使用的一般步骤：
（1）寻找对应点
（2）使用三角化计算深度
（3）验证重投影误差等

2.3.2 代码解析

完整代码解析的话建议是移步到CreateNewMapPoints。我写的没有他的细，就不献丑了。

注意“特征点反投影,其实得到的是在各自相机坐标系下的一个非归一化的方向向量”有问题 我也在评论中指出了！博主人很好

基于CreateNewMapPoints（）这个函数的意义：

• 怎么产生新的地图点？
  整体的逻辑思路是这样的：
  （1）取出共视程度比较大的共视关键帧
  （2）遍历共视关键帧寻找匹配点
  （3）三角化每一对匹配点
  （4）检查后更新到当前帧中

MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);

• SVD
  然而x3D这个点在单目相机中只会来自三角化后的SVD

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

            }

(1) cv::SVD::compute

参数

• src:分解矩阵。深度必须为CV_32F或CV_64F。
• w:计算的奇异值
• u:计算的左奇异向量
• vt :右奇异向量的vt转置矩阵

（2）分解矩阵A
【参考文档】三角形法

2.4 LocalBundleAdjustment

局部地图BA优化这部分可以参考【orb-slam2 从单目开始的简单学习（7）：Optimizer】

2.5 KeyFrameCulling

• 判断关键帧多余的条件
  如果一个关键帧看到的90%的贴图点至少出现在其他3个关键帧中（以相同或更精细的比例），则认为该关键帧是多余的

2.5.1 代码完整解析

void LocalMapping::KeyFrameCulling()
{
    // 检查冗余关键帧（仅本地关键帧）Check redundant keyframes (only local keyframes)
    //如果一个关键帧看到的90%的地图点至少出现在其他3个关键帧中（以相同或更精细的比例），则认为该关键帧是多余的 A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
    // in at least other 3 keyframes (in the same or finer scale)
    // 我们只考虑接近的立体点We only consider close stereo points

    vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
    
//----step1 遍历关键帧集------
    for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
    {
        KeyFrame* pKF = *vit;
        if(pKF->mnId==0)//不能删除首个关键帧（可能被设置为参考帧了）
            continue;
        const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();

        int nObs = 3;
        const int thObs=nObs;
        int nRedundantObservations=0;
        int nMPs=0;
        //----step2 遍历该帧的地图点------
        for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
        {
            MapPoint* pMP = vpMapPoints[i];
            if(pMP)
            {
                if(!pMP->isBad())
                {
                    if(!mbMonocular)
                    {// 对于双目或RGB-D，仅考虑近处（不超过基线的40倍 ）的地图点
                        if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
                            continue;
                    }

                    nMPs++;//统计该关键帧中的地图点总数
                    if(pMP->Observations()>thObs)//共视超过3个
                    {
                        const int &scaleLevel = pKF->mvKeysUn[i].octave;
                        const map<KeyFrame*, size_t> observations = pMP->GetObservations();
                        int nObs=0;
                        //----step3 遍历该地图点的所有共视关系记录重复观测---------------
                        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;
			
			// 尺度约束，要求MapPoint在该局部关键帧的特征尺度大于（或近似于）其它关键帧的特征尺度
			//以当前帧为参照，共视关键帧金字塔大于当前帧 误检测概率upup且大概率不相同
                            if(scaleLeveli<=scaleLevel+1)
                            {
                                nObs++;
                                if(nObs>=thObs)
                                    break;
                            }
                        }//for:observations
                        if(nObs>=thObs)//
                        {
                            nRedundantObservations++;
                        }
                    }
                }
            }
        }//for:MapPoint  

        if(nRedundantObservations>0.9*nMPs)//条件限制
            pKF->SetBadFlag();//先置为坏
    }
}

3.ComputeF12

计算两帧变化的基础矩阵

cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)//计算基础矩阵
{
    cv::Mat R1w = pKF1->GetRotation();//Tcw
    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;

    cv::Mat t12x = SkewSymmetricMatrix(t12);//求反对称矩阵
    
//这种是基于帧的内参不相同
    const cv::Mat &K1 = pKF1->mK;
    const cv::Mat &K2 = pKF2->mK;


    return K1.t().inv()*t12x*R12*K2.inv();//求基础矩阵
}

3.1 基础矩阵和本质矩阵公式表达

首先必须先知道基础矩阵和本质矩阵

3.2 变化过程

    cv::Mat R12 = R1w*R2w.t();
    cv::Mat t12 = -R1w*R2w.t()*t2w+t1w;

这段代码就代表了下面这个关系: