orb-slam2代码总结（二）建图

参考：orb-slam2源码注释版

SLAM的全称是同时建图与定位，在视觉SLAM中，这里的建图指的就是生成和管理MapPoint，MapPoint与激光SLAM中点云不同的是，点云是没有特征可言的，而MapPoint具有标识其唯一性的属性，如，mDescriptor、Visible、mnFound、nObs等. 本次将对orb-slam源码中涉及MapPoint的地方进行梳理.

1、创建新的MapPoints

生成新的MapPoints(即，建图)存在于下面三个地方

初始化时，使用H矩阵或者F矩阵得到R，t后，使用线性三角化生成新的MapPoints
tracking线程，CreateNewKeyFrame() 会为双目和RGBD添加MapPoint
local mapping线程，CreateNewMapPoints() 通过线性三角化方式生成新的MapPoint

2、为当前帧添加MapPoints

tracking线程主要任务就是为当前帧跟踪尽可能多的MapPoints，只有这样约束才够多，估计的位姿就越准确. 为当前帧添加MapPoints有下面三个地方：

TrackReferenceKeyFrame() 函数，通过 SearchByBoW()，上一帧中包含了MapPoints，对这些MapPoints进行tracking，由此增加当前帧的MapPoints
TrackWithMotionModel() 函数，通过 SearchByProjection()，将上一帧的MapPoints投影到当前帧(根据速度模型可以估计当前帧的Tcw)，在投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
TrackLocalMap() 函数，SearchLocalPoints() 函数，通过 SearchByProjection()，只对除了 <新增加的，即 TrackReferenceKeyFrame() 函数中匹配上的> MapPoint进行 SearchByProjection()，以向mCurrentFrame增加新的MapPoint.

3、`mDescriptor`属性

不仅关键点有描述子，MapPoint也有描述子
一个MapPoint可能被很多关键帧观测到，那么该MapPoint的描述子取哪一个呢？orb-slam使用ComputeDistinctiveDescriptors() 函数选择最具代表性的描述子，因此当MapPoint添加了新的关键帧观测后，需要判断是否更新当前点的最适合的描述子.
选取规则是，先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值.

4、MapPoint的法向量和深度

orb关键点具有旋转不变性和尺度不变性，它在提取的时候是有尺度的，不同的金字塔层表示不同的尺度，层数越高，分辨率越低，即表示该关键点是在更低的分辨率下提出出来的，就表示这个MapPoint距离相机光心的距离越近. 同理，金字塔层数越低，分辨率越高，才能识别出远点.
一个MapPoint会被许多相机观测到因此每次插入对某个关键帧的Observation后，需要调用 UpdateNormalAndDepth() 函数，

由该MapPoint的3D位置到所有 <该MapPoint的观测到关键帧光心> 向量均值确定法向量
由它在参考关键帧（创建该MapPoint时指定）对应的关键点的金字塔层，更新该MapPoint的最大深度mfMaxDistance和最小深度mfMinDistance（貌似后两个属性，只跟MapPoint的3D位置和参考关键帧有关，插入新的关键帧观测不会改变这俩属性吧？）. 如下：

{
    unique_lock<mutex> lock3(mMutexPos);
    // 另见PredictScale函数前的注释
    mfMaxDistance = dist*levelScaleFactor;                           // 观测到该点的距离下限
    mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离上限
    mNormalVector = normal/n;                                        // 获得平均的观测方向
}

最大深度mfMaxDistance和最小深度mfMinDistance这两个属性会在 PredictScale() 时用到，如下：

m = c e i l (\frac{l o g (d_{m a x} / d)}{l o g (1.2)})

$m=ceil(\frac{log(d_{max}/d)}{log(1.2)})$

4、`Visible`属性

该属性表示，能观测到该MapPoint的图像帧数目计数器.
1) 在tracking线程跟踪局部地图时，使用 SearchLocalPoints() 向当前帧增加新的MapPoints时，

首先对当前帧已经匹配上MapPoints各自的Visible属性增加计数
然后搜索局部地图，只要该MapPoint满足到当前帧的投影条件，就增加一次计数，如下：

if(mCurrentFrame.isInFrustum(pMP,0.5)){
    pMP->IncreaseVisible();
    nToMatch++;
}

2) 满足上面的投影条件就增加计数，但这些MapPoint并不一定能和当前帧的特征点匹配上，这就是 SearchByProjection() 函数要做的事情. 注意，这里并没有对这些匹配上的MapPoints与当前帧进行关联，仅仅是把匹配上的MapPoints添加到当前帧mvpMapPoints属性中

例如：有一个MapPoint（记为M），在某一帧F的视野范围内，但并不表明该点M可以和F这一帧的某个特征点能匹配上

3) 在local mapping中fuse当前关键帧新生成的MapPoints时、以及形成闭环时，会调用 Replace() 函数进行替换，会对观测更多的MapPoint增加计数，增加的数目为原来那个MapPoint的计数值.

5、`mnFound`属性

相比Visible属性，mnFound的要求就要严的多.
1) TrackLocalMap()函数中，在PoseOptimization()优化位姿之后，对非mvbOutlier的点执行IncreaseFound()，如下：

if(!mCurrentFrame.mvbOutlier[i]){
    mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
    ......
}

2) 在local mapping中fuse当前关键帧新生成的MapPoints时、以及形成闭环时，会调用 Replace() 函数进行替换，会对观测更多的MapPoint增加计数，增加的数目为原来那个MapPoint的计数值.

Visible与mnFound作用：
local mapping线程中的 MapPointCulling() 函数，会根据VI-B条件1，能找到该点的帧不应该少于理论上观测到该点的帧的1/4，如果低于阈值，调用 SetBadFlag() 函数，擦除该MapPoint.

6、`nObs`属性

记录哪些KeyFrame的那个特征点能观测到该MapPoint，单目+1，双目或者grbd+2. 注意，该属性只对关键帧有效，如下：

void MapPoint::AddObservation(KeyFrame* pKF, size_t idx) {
    unique_lock<mutex> lock(mMutexFeatures);
    if(mObservations.count(pKF))
        return;
......
}

这个函数是建立关键帧共视关系的核心函数，能共同观测到某些MapPoints的关键帧是共视关键帧.

6.1 AddObservation 增加计数

1) 在tracking线程，CreateNewKeyFrame() 函数中对双目和RGBD相机，需要生成新的MapPoints，这一步跟 updateLastFrame() 函数内容相似，但这里生成的MapPoints不再是临时的MapPoints（mlpTemporalPoints），而是添加到关键帧里面，同时MapPoint也会添加对该关键帧的观测、计算该MapPoint的平均观测方向、观测距离范围、最佳描述子，并加入到mpMap中（因为这是新生成的MapPoint）.
除了这一步，以及初始化的时候，tracking线程跟踪过程中，都只与已存在地图中的MapPoints进行匹配，并不进行关联，只有在该普通帧确定为关键帧时，才进行关联，关联这步是发生在local mapping线程中ProcessNewKeyFrame() 函数，即下一步要说的就是它.

2) local mapping线程，ProcessNewKeyFrame() 函数，由于mpCurrentKeyFrame中一些MapPoints在 TrackLocalMap() 函数中的MapPoints与当前关键帧进行了匹配，但没有对这些匹配上的MapPoints与当前帧进行关联，所以在这里添加其对mpCurrentKeyFrame的观测.

if(!pMP->IsInKeyFrame(mpCurrentKeyFrame)){
    // 添加观测
    pMP->AddObservation(mpCurrentKeyFrame, i);
    // 获取该点的平均观测方向和观测距离范围
    pMP->UpdateNormalAndDepth();
    // 加入关键帧后，更新3D点的最佳描述子
    pMP->ComputeDistinctiveDescriptors();
}
else /** @todo this can only happen for new stereo points inserted by*/
{                                                              
    // 将双目或RGBD跟踪过程中新插入的MapPoints放入mlpRecentAddedMapPoints，等待检查
    // CreateNewMapPoints函数中通过三角化也会生成MapPoints                 
    // 这些MapPoints都会经过MapPointCulling函数的检验                     
    mlpRecentAddedMapPoints.push_back(pMP);                    
}

3) local mapping线程，CreateNewMapPoints() 函数中，通过三角化生成新的3D点（注意，这里还不能叫MapPoint），这些3D点需要通过平行、重投影误差、尺度一致性等检查后，才建立一个对应3D点的MapPoint对象，然后添加观测，最后加入到mlpRecentAddedMapPoints列表中（还要继续检查）.

4) 在local mapping中fuse当前关键帧新生成的MapPoints时、以及形成闭环时，会调用 Replace() 函数进行替换，会对观测更多的MapPoint，让该MapPoint替换掉原来MapPoint对应的KeyFrame，让原来MapPoint对应的KeyFrame用pMP替换掉原来的MapPoint，详细解释见下面mpReplaced属性.

6.2 EraseObservation 减少计数

减少nObs属性，与增加相反，单目-1，双目或者grbd -2，并在mObservations属性擦出对该关键帧的观测，此外要注意，如果要擦出的关键帧是这个MapPoint的参考关键帧(mpRefKF），则需要重新设置参考关键帧，进一步，如果少于两个关键帧观测到该MapPoint，则删除该MapPoint，即通过 MapPoint::SetBadFlag() 实现，如下：

// 如果该keyFrame是参考帧，该Frame被删除后重新指定RefFrame
if(mpRefKF==pKF)
    mpRefKF=mObservations.begin()->first; //重设参考关键帧

// 如果少于2个关键帧观测到该MapPoint，则删除该MapPoint*/
if(nObs<=2)
    bBad=true;

1) LocalBundleAdjustment() 函数会对连接偏差比较大，在关键帧中剔除对该MapPoint的观测，同时在MapPoint中剔除对该关键帧的观测，通过调用MapPoint的成员函数 EraseObservation() 实现，如下

if(!vToErase.empty())
{
    for(size_t i=0;i<vToErase.size();i++)
    {
        KeyFrame* pKFi = vToErase[i].first;
        MapPoint* pMPi = vToErase[i].second;
        pKFi->EraseMapPointMatch(pMPi);
        pMPi->EraseObservation(pKFi);
    }
}

2) 在擦除关键帧的时候，记得要解除关键帧和MapPoints的观测关系，即 KeyFrame::SetBadFlag() 函数要做的事之一.

7、`mbBad`属性

设置true，即为坏点，相当于擦除了该MappPoint（实际上要等程序结束，调用析构函数才能回收分配的内存）.
成员函数 MapPoint::EraseObservation() 、Replace() 函数（详细解释参见mpReplaced属性）和 MapPoint::SetBadFlag() 函数会设置该属性

8、`mpReplaced`属性

Replace()函数会对该属性操作，有两个地方会对该属性进行修改：

1) 在形成闭环的时候，会更新KeyFrame与MapPoint之间的关系，<闭环帧的中MapPoints> 对应的描述子与 <当前帧的MapPoints> 对应的描述子相近(即二者匹配上了)，就认为这两个MapPoint是同一个MapPoint(因为它们是来自同一个特征点)，那么就使用闭环帧对应的MapPoints替换当前帧中MapPoints

2) local mapping线程中，由于当前关键帧产生新的MapPoints点后，这些MapPoints有可能会被其他关键帧找到，所以在SearchInNeighbors() 函数中，分别与一级二级相邻帧(的MapPoints)中重复的进行合并，将会出现下面两种情况：

如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）
如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint，如下：

if(pMPinKF)// 如果这个点有对应的MapPoint                                                    
{                                                                                  
    if(!pMPinKF->isBad())// 如果这个MapPoint不是bad，选择哪一个呢？                              
    {                                                                              
        if(pMPinKF->Observations() > pMP->Observations()) /** @attention 选择观测数更多的*/
            pMP->Replace(pMPinKF);                                                 
        else                                                                       
            pMPinKF->Replace(pMP);                                                 
    }                                                                                                               
}
else// 如果这个点没有对应的MapPoint            
{                                    
    pMP->AddObservation(pKF,bestIdx);
    pKF->AddMapPoint(pMP,bestIdx);   
}

第一种情况比较复杂，其宗旨是使用选择观测数更多的MapPoint替换较少的MapPoint，下面是替换过程的实现代码，
当if条件成立时很容易理解，即pMP不在this的mObservations中的关键帧（等同于说，这个关键帧不曾观测到该MapPoint），直接替换即可.
当if条件不成立时，注意，下面要说的this和pMP的值都是在if条件不成立时才成立！！this是指在local mapping中当前关键帧新生成的MapPoint，该MapPoint对应的关键点记为a，pKF表示当前关键帧（obs除了生成该MapPoint那两个关键帧外，还有在local mapping线程创建完新的MapPoints后，tracking线程利用这个MapPoint创建了新的关键帧），pMP表示的一定不是当前关键帧在local mapping新生成的MapPoint（因为if不成立），而是来自当前关键帧的一二级相邻关键帧生成过的MapPoint，而这个MapPoint又正好已经被当前关键帧（pKF）匹配上了（匹配时，假设当前关键帧使用的是关键点b），为啥这两个不同的关键点会冲突呢，是因为这俩货都跟当前关键帧的一二级相邻关键帧中同一个关键点相似，设为关键点c，我们再来梳理一下，a与c的描述子很像，b又与c的描述子很像，但描述子不具备传递性，你想想那么描述子的向量维度是那么高！！！随便就能找到这样三个向量，a与c很像，b又与c很像，但是a与b不像. 写了这么多，还不如一张图来的清晰，见下图 ^v^

for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
{                                                                                     
    // Replace measurement in keyframe                                                
    KeyFrame* pKF = mit->first;                                                       

    if(!pMP->IsInKeyFrame(pKF))                                                       
    {                                                                                 
        pKF->ReplaceMapPointMatch(mit->second, pMP);// 让KeyFrame用pMP替换掉原来的MapPoint    
        pMP->AddObservation(pKF,mit->second);// 让MapPoint替换掉对应的KeyFrame               
    }                                                                                 
    else                                                                              
    {                                                                                 
        // 产生冲突，即pKF中有两个特征点a,b（这两个特征点的描述子是近似相同的），这两个特征点对应两个MapPoint为this,pMP          
        // 然而在fuse的过程中pMP的观测更多，需要替换this，因此保留b与pMP的联系，去掉a与this的联系                                                 
        pKF->EraseMapPointMatch(mit->second);                                         
    }                                                                                 
}

这里写图片描述

Replace() 这个函数同样会设置mbBad属性为true，但MapPoint的内存不会被释放，故保留mpReplaced属性，用于保存替换该MapPoint的那个MapPoint.
保留mpReplaced属性的作用是，由于tracking中需要用到mLastFrame，这里检查并更新上一帧中被替换的MapPoints(使用替换后的). 这个鬼费了这么大劲，就这么点用么……可能主要用在闭环那里吧（防止闭环出现之后造成跟丢），local mapping线程用处不大吧？？

<完>
@leatherwang