欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-06-11 18:43:13
...

LocalMapping线程介绍

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

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

LocalMapping线程主要流程

ORBSlam2学习研究(Code analysis)-ORBSlam2中的视觉里程计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();
}