SLAM——ORB-SLAM3代码分析（十二）LoopClosing（5）

最新推荐文章于 2023-03-19 10:43:14 发布

Jevies

最新推荐文章于 2023-03-19 10:43:14 发布

阅读量879

点赞数 1

文章标签：自动驾驶 c++

本文链接：https://blog.csdn.net/qq_45794281/article/details/121891936

版权

2021SC@SDUSC

LoopClosing分析（5）

这一篇博客接着分析LoopCLosing，依旧将分析以注释的形式直接写在代码中。

RequestReset函数是由外部线程调用，用来请求复位当前线程。

void LoopClosing::RequestReset()
{
    // 标志置位
    {
        unique_lock<mutex> lock(mMutexReset);
        mbResetRequested = true;
    }

    // 堵塞,直到回环检测线程复位完成
    while(1)
    {
        {
        unique_lock<mutex> lock2(mMutexReset);
        if(!mbResetRequested)
            break;
        }
		//usleep(5000);
		std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

ResetIfRequested函数则是由当前线程调用，用来检查是否有外部线程请求复位当前线程：如果有的话就复位回环检测线程

void LoopClosing::ResetIfRequested()
{
    unique_lock<mutex> lock(mMutexReset);
    // 如果有来自于外部的线程的复位请求,那么就复位当前线程
    if(mbResetRequested)
    {
        mlpLoopKeyFrameQueue.clear();   // 清空参与和进行回环检测的关键帧队列
        mLastLoopKFid=0;                // 上一次没有和任何关键帧形成闭环关系
        mbResetRequested=false;         // 复位请求标志复位
    }else if(mbResetActiveMapRequested)
    {
        for (list<KeyFrame*>::const_iterator it=mlpLoopKeyFrameQueue.begin(); it != mlpLoopKeyFrameQueue.end();)
        {
            KeyFrame* pKFi = *it;
            if(pKFi->GetMap() == mpMapToReset)
            {
                it = mlpLoopKeyFrameQueue.erase(it);
            }
            else
                ++it;
        }

        mLastLoopKFid=mpAtlas->GetLastInitKFid(); //TODO old variable, it is not use in the new algorithm
        mbResetActiveMapRequested=false;
    }
}

RunGlobalBundleAdjustment是一个较为重要的函数，它是全局BA线程的主函数。其参数nLoopKF 在调用的时候是当前关键帧的id。

ORB-SLAM3 采用g2o 图优化来完成BA：

void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
{
    Verbose::PrintMess("Starting Global Bundle Adjustment", Verbose::VERBOSITY_NORMAL);

    const bool bImuInit = pActiveMap->isImuInitialized();

    if(!bImuInit)
        Optimizer::GlobalBundleAdjustemnt(pActiveMap,10,&mbStopGBA,nLoopKF,false);
    else
        Optimizer::FullInertialBA(pActiveMap,7,false,nLoopKF,&mbStopGBA);

    // 记录GBA已经迭代次数,用来检查全局BA过程是否是因为意外结束的
    int idx =  mnFullBAIdx;
    // mbStopGBA直接传引用过去了,这样当有外部请求的时候这个优化函数能够及时响应并且结束掉

    // Step 1 执行全局BA，优化所有的关键帧位姿和地图中地图点

    // Update all MapPoints and KeyFrames
    // Local Mapping was active during BA, that means that there might be new keyframes
    // not included in the Global BA and they are not consistent with the updated map.
    // We need to propagate the correction through the spanning tree
    // 更新所有的地图点和关键帧
    // 在global BA过程中local mapping线程仍然在工作，这意味着在global BA时可能有新的关键帧产生，但是并未包括在GBA里，
    // 所以和更新后的地图并不连续。需要通过spanning tree来传播
    {
        unique_lock<mutex> lock(mMutexGBA);
        // 如果全局BA过程是因为意外结束的,那么直接退出GBA
        if(idx!=mnFullBAIdx)
            return;
			 if(!bImuInit && pActiveMap->isImuInitialized())
            return;
        // 如果当前GBA没有中断请求，更新位姿和地图点
        // 这里和上面那句话的功能还有些不同,因为如果一次全局优化被中断,往往意味又要重新开启一个新的全局BA;为了中断当前正在执行的优化过程mbStopGBA将会被置位,同时会有一定的时间
        // 使得该线程进行响应;而在开启一个新的全局优化进程之前 mbStopGBA 将会被置为False
        // 因此,如果被强行中断的线程退出时已经有新的线程启动了,mbStopGBA=false,为了避免进行后面的程序,所以有了上面的程序;
        // 而如果被强行中断的线程退出时新的线程还没有启动,那么上面的条件就不起作用了(虽然概率很小,前面的程序中mbStopGBA置位后很快mnFullBAIdx就++了,保险起见),所以这里要再判断一次
        if(!mbStopGBA)
        {
            cout << "Global Bundle Adjustment finished" << endl;
            cout << "Updating map ..." << endl;
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            // 等待直到local mapping结束才会继续后续操作
            while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished())
            {
				usleep(1000);
			}

            // Get Map Mutex
            // 后续要更新地图所以要上锁
            unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

            // Correct keyframes starting at map first keyframe
            // 从第一个关键帧开始矫正关键帧。刚开始只保存了初始化第一个关键帧
            list<KeyFrame*> lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end());

            // 问：GBA里锁住第一个关键帧位姿没有优化，其对应的pKF->mTcwGBA是不变的吧？那后面调整位姿的意义何在？
            // 回答：注意在前面essential graph BA里只锁住了回环帧，没有锁定第1个初始化关键帧位姿。所以第1个初始化关键帧位姿已经更新了
            // 在GBA里锁住第一个关键帧位姿没有优化，其对应的pKF->mTcwGBA应该是essential BA结果，在这里统一更新了
            // Step 2 遍历并更新全局地图中的所有spanning tree中的关键帧
            while(!lpKFtoCheck.empty())
            {
                KeyFrame* pKF = lpKFtoCheck.front();
                const set<KeyFrame*> sChilds = pKF->GetChilds();
                cv::Mat Twc = pKF->GetPoseInverse();
                // 遍历当前关键帧的子关键帧
                for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++)
                {
                    KeyFrame* pChild = *sit;
                    // 记录避免重复
                    if(!pChild || pChild->isBad())
                        continue;if(pChild->mnBAGlobalForKF!=nLoopKF)
                    {
                        // 从父关键帧到当前子关键帧的位姿变换 T_child_farther
                        cv::Mat Tchildc = pChild->GetPose()*Twc;
                        // 再利用优化后的父关键帧的位姿，转换到世界坐标系下，相当于更新了子关键帧的位姿
                        // 这种最小生成树中除了根节点，其他的节点都会作为其他关键帧的子节点，这样做可以使得最终所有的关键帧都得到了优化
                        pChild->mTcwGBA = Tchildc*pKF->mTcwGBA;
                         cv::Mat Rcor = pChild->mTcwGBA.rowRange(0,3).colRange(0,3).t()*pChild->GetRotation();
                        if(!pChild->GetVelocity().empty()){
                            //cout << "Child velocity: " << pChild->GetVelocity() << endl;
                            pChild->mVwbGBA = Rcor*pChild->GetVelocity();
                        }
                        else
                            Verbose::PrintMess("Child velocity empty!! ", Verbose::VERBOSITY_NORMAL);


                        //cout << "Child bias: " << pChild->GetImuBias() << endl;
                        pChild->mBiasGBA = pChild->GetImuBias();

                        // 做个标记，避免重复
                        pChild->mnBAGlobalForKF=nLoopKF;

                    }
                    lpKFtoCheck.push_back(pChild);
                }
                // 记录未矫正的关键帧的位姿
                pKF->mTcwBefGBA = pKF->GetPose();
                // 记录已经矫正的关键帧的位姿
                pKF->SetPose(pKF->mTcwGBA);
                // 从列表中移除
                lpKFtoCheck.pop_front();
            }

            // Correct MapPoints
            const vector<MapPoint*> vpMPs = mpMap->GetAllMapPoints();

            // Step 3 遍历每一个地图点并用更新的关键帧位姿来更新地图点位置
            for(size_t i=0; i<vpMPs.size(); i++)
            {
                MapPoint* pMP = vpMPs[i];

                if(pMP->isBad())
                    continue;

                // 如果这个地图点直接参与到了全局BA优化的过程,那么就直接重新设置器位姿即可
                if(pMP->mnBAGlobalForKF==nLoopKF)
                {
                    // If optimized by Global BA, just update
                    pMP->SetWorldPos(pMP->mPosGBA);
                }
                else 
                {
                    // 如这个地图点并没有直接参与到全局BA优化的过程中,那么就使用其参考关键帧的新位姿来优化自己的坐标
                    // Update according to the correction of its reference keyframe
                    KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();

                    // 如果参考关键帧并没有经过此次全局BA优化，就跳过 
                    if(pRefKF->mnBAGlobalForKF!=nLoopKF)
                        continue;
if(pRefKF->mTcwBefGBA.empty())
                        continue;
                    // Map to non-corrected camera
                    cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3);
                    cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3);
                    // 转换到其参考关键帧相机坐标系下的坐标
                    cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw;

                    // Backproject using corrected camera
                    // 然后使用已经纠正过的参考关键帧的位姿,再将该地图点变换到世界坐标系下
                    // Map to non-corrected camera
                    cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3);
                    cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3);
                    cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw;

                    // Backproject using corrected camera
                    cv::Mat Twc = pRefKF->GetPoseInverse();
                    cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
                    cv::Mat twc = Twc.rowRange(0,3).col(3);

                    pMP->SetWorldPos(Rwc*Xc+twc);
                }
            }
					pActiveMap->InformNewBigChange();
            pActiveMap->IncreaseChangeIndex();
            // 释放
            mpLocalMapper->Release();

            Verbose::PrintMess("Map updated!", Verbose::VERBOSITY_NORMAL);
        }

        mbFinishedGBA = true;
        mbRunningGBA = false;
    } 
}

RunGlobalBundleAdjustment函数用于闭环优化，在闭环结束前新开一个线程，全局优化，在此之前会OptimizeEssentialGraph。它会和用于单目初始化的CreateInitialMapMonocular函数组成GlobalBundleAdjustment函数，用于全局BA。

Jevies

关注

1
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
SLAM——ORB-SLAM3代码分析（十二）LoopClosing（5）

2021SC@SDUSCLoopClosing分析（5）这一篇博客接着分析LoopCLosing，依旧将分析以注释的形式直接写在代码中。RequestReset函数是由外部线程调用，用来请求复位当前线程。void LoopClosing::RequestReset(){ // 标志置位 { unique_lock<mutex> lock(mMutexReset); mbResetRequested = true; }
复制链接

扫一扫