【干货】从LDSO中移植回环检测到VI-DSO

乔克1998

已于 2023-10-16 13:43:35 修改

阅读量222

点赞数

文章标签：计算机视觉算法

于 2023-10-13 15:37:23 首次发布

本文链接：https://blog.csdn.net/weixin_49227107/article/details/133784738

版权

目的

给VI-DSO加入回环检测功能模块，从LDSO中移植。

好激动好期待，像是要给程序动手术一样

准备工作

首先，我大概解剖了一下LDSO是如何在DSO的基础上加入回环检测的。

在run_dso_tum_mono.cc主函数里，先把FullSystem类实例化：shared_ptr fullSystem(new FullSystem(voc));
FullSystem类的构造函数里，实例化LoopClosing类（主角），回环检测线程mainLoop就存在于LoopClosing类的构造函数里。
mainLoop = thread(&LoopClosing::Run, this); 看看这个Run()都做了哪些事吧～～

 void LoopClosing::Run() {
        finished = false;

        while (1) {
        //needFinish 初值为false
            if (needFinish) {
                LOG(INFO) << "find loop closing thread need finish flag!" << endl;
                break;
            }

            {
                // get the oldest one 如果关键帧queue是空的，等半秒再继续循环
                unique_lock<mutex> lock(mutexKFQueue);
                if (KFqueue.empty()) {
                    lock.unlock();
                    usleep(5000);
                    continue; //这里cotinue执行完，从头开始while循环
                }
                //把KFqueue中的第一个关键帧给currentKF
                currentKF = KFqueue.front();
                //移除KFqueue的第一个关键帧
                KFqueue.pop_front();
                //如果关键帧队列里超过20帧，就清空队列
                if (KFqueue.size() > 20)
                    KFqueue.clear(); 
                //把currentKFpush到allKF vector
                allKF.push_back(currentKF);
            }
            //感觉是计算的当前帧的orb特征词典  和voc没关系
            currentKF->ComputeBoW(voc);  
            //如果检测到了回环帧，poseGraphRunning初始是false，Idle()就是！poseGraphRunning就是true
            if (DetectLoop(currentKF)) {
                bool mapIdle = globalMap->Idle();  //mapIdle为true
                //如果修正了回环，并且mapIdle为true，就执行一次优化OptimizeALLKFs()
                if (CorrectLoop(Hcalib)) {
                    // start a pose graph optimization
                    if (mapIdle) {
                        LOG(INFO) << "call global pose graph!" << endl;
                        bool ret = globalMap->OptimizeALLKFs();
                        if (ret)
                            needPoseGraph = false;
                    } else {
                        LOG(INFO) << "still need pose graph optimization!" << endl;
                        needPoseGraph = true;
                    }
                }
            }

//如果还需要优化并且poseGraphRunning是false（没有在优化），再优化
            if (needPoseGraph && globalMap->Idle()) {
                LOG(INFO) << "run another pose graph!" << endl;
                if (globalMap->OptimizeALLKFs())
                    needPoseGraph = false;
            }

            usleep(5000);
        }

        finished = true; //回环结束
    }

总结一下，就是先去关键帧队列里取第一个关键帧（currentKF），其他的清空，计算currentKF的特征词典，和词典中比较出最相似的关键候选帧（candidateKF），如果candidateKF满足条件，就修正回环（CorrectLoop），并执行后端优化。

CorrectLoop(Hcalib)和OptimizeALLKFs()这俩函数我还没看，太长了…
除了这个loop主线程，在FullSystem.cc里还有一些关于LoopClosing的操作：

在makeKeyFrame()中, ======= add new Immature points & new residuals ===========添加一些record改动
在makeKeyFrame()中, ======== Marginalize Frames ============添加了将当前kf添加到地图中并检测loop的操作
还有一个地方，就是在FullSystem::~FullSystem()析构函数里，blockUntilMappingIsFinished()函数中加入了 loopClosing->SetFinish，如果globalMap->NumFrames() > 4，就进行最终的姿势图优化。并更新世界坐标系点UpdateAllWorldPoints()。

好了，涉及到LOOP的地方就是这么多，虽然有些粘连的地方没有去仔细看，但是我认为还是边移植边学边改！