ORBSlam2学习研究(Code analysis)-ORBSlam2中的视觉里程计LocalMapping

LocalMapping线程介绍

ORBSlam2中的视觉里程计VO不仅仅包含Tracking线程，LocalMapping也是VO中的一部分。Tracking线程负责对图像进行预处理并提取特征还原真实3维场景下的点特征，并提取关键帧交给LocalMapping线程。

而LocalMapping线程主要的工作就是通过不断的加入新KeyFrame和新地图点，剔除冗余KeyFrame和冗余地图点，来维护一个稳定的KeyFrame集合，从而可以进行后面的LoopClosing操作

LocalMapping线程主要流程

主要的几个功能点在

• 计算新关键帧的描述子并插入地图
• 地图点的筛选
• 新地图点的三角化和再优化
• 局部BA优化
• 关键帧的筛选

计算新关键帧的描述子并插入地图

我们先计算当前关键帧的BoW向量

    // 计算当前关键帧的BOW向量
    mpCurrentKeyFrame->ComputeBoW();

然后更新当前关键帧所看到的地图点的观测值

    const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();

    for(size_t i=0; i<vpMapPointMatches.size(); i++)
    {
        MapPoint* pMP = vpMapPointMatches[i];
        if(pMP)
        {
            if(!pMP->isBad())
            {
                if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
                {
                    pMP->AddObservation(mpCurrentKeyFrame, i);
                    pMP->UpdateNormalAndDepth(); //更新法线和深度
                    pMP->ComputeDistinctiveDescriptors();// 更新描述子
                }
                else // this can only happen for new stereo points inserted by the Tracking
                {
                    mlpRecentAddedMapPoints.push_back(pMP);
                }
            }
        }
    }

接着更新Covisibility图的边，并将该关键帧插入到地图中

    // 更新Covisibility图中的边
    mpCurrentKeyFrame->UpdateConnections();

    // 将关键帧插入到地图中
    mpMap->AddKeyFrame(mpCurrentKeyFrame);

地图点的筛选

在这一步需要检查最近加入的这些地图点，并将一些冗余的地图点从最近地图点的列表中去除
有四处地图点的剔除规则

• 如果地图点是被标记为bad的时候

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

• 有超过25%的关键帧可以看到这个点

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

• 未被超过2个关键帧看到，并且当前关键帧的ID和看到该点的第一个关键帧之间的ID之差大于等于2的时候

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

• 当前关键帧的ID和看到该点的第一个关键帧之间的ID之差大于等于3的时候

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

新地图点的三角化和再优化

在这一步中，重新当前关键帧附近的关键帧与当前关键帧之间的对极约束，通过该对极约束还原当前关键帧到附近关键帧的变换，并通过这种变换进行三角化计算

• 获取Cov图中附近的关键帧，这里的nn代表最多取多少个关键帧

int nn = 10;
if (mbMonocular)
    nn = 20;
const vector<KeyFrame *> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

• 计算当前帧和每个附近关键帧之间的对极约束，计算满足对极约束的匹配点
  这段代码求解两个关键帧之间的对极约束的情况，这段代码之上求解的是基线等对极约束的所需参数，这段代码之下求解的是对极约束得到的R和t

if (!mbMonocular)
{
    if (baseline < pKF2->mb)
        continue;
}
else
{
    const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
    const float ratioBaselineDepth = baseline / medianDepthKF2;
    if (ratioBaselineDepth < 0.01)
        continue;
}
// 计算基础矩阵
cv::Mat F12 = ComputeF12(mpCurrentKeyFrame, pKF2);
// 搜索满足完全对极约束的匹配点
vector<pair<size_t, size_t>> vMatchedIndices;
matcher.SearchForTriangulation(mpCurrentKeyFrame, pKF2, F12, vMatchedIndices, false);

• 对当前帧和某一个关键帧之间的匹配点进行三角化计算，求解匹配点的深度。
• 接着分别检查新得到的点在两个平面上的重投影误差，如果大于一定的值，直接抛弃该点。
• 检查尺度一致性

局部BA优化

这部分在我的另一个Blog里面已经详细描述过了
ORBSlam2的位姿优化算法

关键帧的筛选

关键帧的筛选的规则，在代码中已经给出了

检查冗余关键帧（仅限局部关键帧）
如果它看到的90％的MapPoints至少在其他3个关键帧（同样或更精细的尺度）中被看到，则关键帧被认为是冗余的

这里直接把代码贴出来

vector<KeyFrame *> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();//获取附近的局部关键帧的vector变量
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;
    for (size_t i = 0, iend = vpMapPoints.size(); i < iend; i++)
    {//每个匹配地图点都遍历一遍
        MapPoint *pMP = vpMapPoints[i];
        if (pMP)
        {
            if (!pMP->isBad())
            {
                if (!mbMonocular)
                {
                    if (pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0)
                        continue;
                }
                nMPs++;
                if (pMP->Observations() > thObs)
                {
                    const int &scaleLevel = pKF->mvKeysUn[i].octave;//当前关键点所处层级
                    const map<KeyFrame *, size_t> observations = pMP->GetObservations();//当前地图点被观察数
                    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;
                        if (scaleLeveli <= scaleLevel + 1)
                        {
                            nObs++;
                            if (nObs >= thObs)//被观察数达到3
                                break;
                        }
                    }
                    if (nObs >= thObs)
                    {
                        nRedundantObservations++;//冗余观察数累加
                    }
                }
            }
        }
    }
    if (nRedundantObservations > 0.9 * nMPs)
        pKF->SetBadFlag();
}