ORB-SLAM2 --- KeyFrame::SetBadFlag函数

最新推荐文章于 2023-03-21 17:17:12 发布

APS2023

最新推荐文章于 2023-03-21 17:17:12 发布

阅读量804

点赞数 1

分类专栏：视觉SLAM - ORBSLAM2 原理解析文章标签：算法 c++ 数据结构 slam

本文链接：https://blog.csdn.net/qq_41694024/article/details/128570430

版权

视觉SLAM - ORBSLAM2 原理解析专栏收录该内容

66 篇文章 46 订阅

订阅专栏

1.函数作用

2.code

3.函数解析

1.函数作用

真正地执行删除关键帧的操作。

需要删除的是该关键帧和其他所有帧、地图点之间的连接关系。

2.code

void KeyFrame::SetBadFlag()
{   
    // Step 1 首先处理一下删除不了的特殊情况
    {
        unique_lock<mutex> lock(mMutexConnections);

        // 第0关键帧不允许被删除
        if(mnId==0)
            return;
        else if(mbNotErase)
        {
            // mbNotErase表示不应该删除，于是把mbToBeErased置为true，假装已经删除，其实没有删除
            mbToBeErased = true;
            return;
        }
    }

    // Step 2 遍历所有和当前关键帧相连的关键帧，删除他们与当前关键帧的联系
    for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
        mit->first->EraseConnection(this); // 让其它的关键帧删除与自己的联系

    // Step 3 遍历每一个当前关键帧的地图点，删除每一个地图点和当前关键帧的联系
    for(size_t i=0; i<mvpMapPoints.size(); i++)
        if(mvpMapPoints[i])
            mvpMapPoints[i]->EraseObservation(this); 

    {
        unique_lock<mutex> lock(mMutexConnections);
        unique_lock<mutex> lock1(mMutexFeatures);

        // 清空自己与其它关键帧之间的联系
        mConnectedKeyFrameWeights.clear();
        mvpOrderedConnectedKeyFrames.clear();

        // Update Spanning Tree 
        // Step 4 更新生成树，主要是处理好父子关键帧，不然会造成整个关键帧维护的图断裂，或者混乱
        // 候选父关键帧
        set<KeyFrame*> sParentCandidates;
        // 将当前帧的父关键帧放入候选父关键帧
        sParentCandidates.insert(mpParent);

        // Assign at each iteration one children with a parent (the pair with highest covisibility weight)
        // Include that children as new parent candidate for the rest
        // 每迭代一次就为其中一个子关键帧寻找父关键帧（最高共视程度），找到父的子关键帧可以作为其他子关键帧的候选父关键帧
        while(!mspChildrens.empty())
        {
            bool bContinue = false;

            int max = -1;
            KeyFrame* pC;
            KeyFrame* pP;

            // Step 4.1 遍历每一个子关键帧，让它们更新它们指向的父关键帧
            for(set<KeyFrame*>::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++)
            {
                KeyFrame* pKF = *sit;
                // 跳过无效的子关键帧
                if(pKF->isBad())    
                    continue;

                // Check if a parent candidate is connected to the keyframe
                // Step 4.2 子关键帧遍历每一个与它共视的关键帧    
                vector<KeyFrame*> vpConnected = pKF->GetVectorCovisibleKeyFrames();

                for(size_t i=0, iend=vpConnected.size(); i<iend; i++)
                {
                    // sParentCandidates 中刚开始存的是这里子关键帧的“爷爷”，也是当前关键帧的候选父关键帧
                    for(set<KeyFrame*>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++)
                    {
                        // Step 4.3 如果孩子和sParentCandidates中有共视，选择共视最强的那个作为新的父
                        if(vpConnected[i]->mnId == (*spcit)->mnId)
                        {
                            int w = pKF->GetWeight(vpConnected[i]);
                            // 寻找并更新权值最大的那个共视关系
                            if(w>max)
                            {
                                pC = pKF;                   //子关键帧
                                pP = vpConnected[i];        //目前和子关键帧具有最大权值的关键帧（将来的父关键帧） 
                                max = w;                    //这个最大的权值
                                bContinue = true;           //说明子节点找到了可以作为其新父关键帧的帧
                            }
                        }
                    }
                }
            }

            // Step 4.4 如果在上面的过程中找到了新的父节点
            // 下面代码应该放到遍历子关键帧循环中?
            // 回答：不需要！这里while循环还没退出，会使用更新的sParentCandidates
            if(bContinue)
            {
                // 因为父节点死了，并且子节点找到了新的父节点，就把它更新为自己的父节点
                pC->ChangeParent(pP);
                // 因为子节点找到了新的父节点并更新了父节点，那么该子节点升级，作为其它子节点的备选父节点
                sParentCandidates.insert(pC);
                // 该子节点处理完毕，删掉
                mspChildrens.erase(pC);
            }
            else
                break;
        }

        // If a children has no covisibility links with any parent candidate, assign to the original parent of this KF
        // Step 4.5 如果还有子节点没有找到新的父节点
        if(!mspChildrens.empty())
            for(set<KeyFrame*>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++)
            {
                // 直接把父节点的父节点作为自己的父节点 即对于这些子节点来说,他们的新的父节点其实就是自己的爷爷节点
                (*sit)->ChangeParent(mpParent);
            }

        mpParent->EraseChild(this);
        // mTcp 表示原父关键帧到当前关键帧的位姿变换，在保存位姿的时候使用
        mTcp = Tcw*mpParent->GetPoseInverse();
        // 标记当前关键帧已经挂了
        mbBad = true;
    }  

    // 地图和关键帧数据库中删除该关键帧
    mpMap->EraseKeyFrame(this);
    mpKeyFrameDB->erase(this);
}

3.函数解析

先介绍mbNotErase作用：表示要删除该关键帧及其连接关系但是这个关键帧有可能正在回环检测或者计算sim3操作，这时候虽然这个关键帧冗余，但是却不能删除，仅设置mbNotErase为true，这时候调用setbadflag函数时，不会将这个关键帧删除，只会把mbTobeErase变成true，代表这个关键帧可以删除但不到时候，先记下来以后处理。在闭环线程里调用 SetErase函数会根据mbToBeErased来删除之前可以删除还没删除的帧。

①首先处理一下删除不了的特殊情况：

即SLAM的第一帧不能被删除，因为其他帧的定位都靠第一帧，因此把mbToBeErased置为true，假装已经删除，其实没有删除。

②遍历所有和当前关键帧相连的关键帧，删除他们与当前关键帧的联系：

mConnectedKeyFrameWeights存储当前关键帧的共视信息，记录当前关键帧共视关键帧的信息（哪一帧和当前关键帧有共视，共视程度是多少），是一个map<KeyFrame*,int>型变量，关于它的具体信息请参阅我的博客：

ORB-SLAM2 --- KeyFrame::UpdateConnections 函数解析https://blog.csdn.net/qq_41694024/article/details/128516249
    for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
        mit->first->EraseConnection(this); // 让其它的关键帧删除与自己的联系
void KeyFrame::EraseConnection(KeyFrame* pKF)
{
    // 其实这个应该表示是否真的是有共视关系
    bool bUpdate = false;

    {
        unique_lock<mutex> lock(mMutexConnections);
        if(mConnectedKeyFrameWeights.count(pKF))
        {
            mConnectedKeyFrameWeights.erase(pKF);
            bUpdate=true;
        }
    }

    // 如果是真的有共视关系,那么删除之后就要更新共视关系
    if(bUpdate)
        UpdateBestCovisibles();
}
即对于每个与当前帧有共视的帧，如图中红色帧，将该红色帧中关于此帧的共视信息删掉（因为红色帧也有mConnectedKeyFrameWeights向量，保存着共视关系，其中有一条就是 <被删除帧>，共视地图点的索引，我们要删掉这条信息），同时删除之后要更新共视关系。
void KeyFrame::UpdateBestCovisibles()
{
    // 互斥锁，防止同时操作共享数据产生冲突
    unique_lock<mutex> lock(mMutexConnections);
    // http://stackoverflow.com/questions/3389648/difference-between-stdliststdpair-and-stdmap-in-c-stl (std::map 和 std::list<std::pair>的区别)
    
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(mConnectedKeyFrameWeights.size());
    // 取出所有连接的关键帧，mConnectedKeyFrameWeights的类型为std::map<KeyFrame*,int>，而vPairs变量将共视的地图点数放在前面，利于排序
    for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
       vPairs.push_back(make_pair(mit->second,mit->first));

    // 按照权重进行排序（默认是从小到大）
    sort(vPairs.begin(),vPairs.end());

    // 为什么要用链表保存？因为插入和删除操作方便，只需要修改上一节点位置，不需要移动其他元素
    list<KeyFrame*> lKFs;   // 所有连接关键帧
    list<int> lWs;          // 所有连接关键帧对应的权重（共视地图点数目）
    for(size_t i=0, iend=vPairs.size(); i<iend;i++)
    {
        // push_front 后变成从大到小
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    // 权重从大到小排列的连接关键帧
    mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
    // 从大到小排列的权重，和mvpOrderedConnectedKeyFrames一一对应
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}
③遍历每一个当前关键帧的地图点，删除每一个地图点和当前关键帧的联系：
void MapPoint::EraseObservation(KeyFrame* pKF)
{
    bool bBad=false;
    {
        unique_lock<mutex> lock(mMutexFeatures);
        // 查找这个要删除的观测,根据单目和双目类型的不同从其中删除当前地图点的被观测次数
        if(mObservations.count(pKF))
        {
            int idx = mObservations[pKF];
            if(pKF->mvuRight[idx]>=0)
                nObs-=2;
            else
                nObs--;

            mObservations.erase(pKF);

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

            // If only 2 observations or less, discard point
            // 当观测到该点的相机数目少于2时，丢弃该点
            if(nObs<=2)
                bBad=true;
        }
    }

    if(bBad)
        // 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
        SetBadFlag();
}
④更新生成树，主要是处理好父子关键帧，不然会造成整个关键帧维护的图断裂，或者混乱

⑤地图和关键帧数据库中删除该关键帧