精华内容
下载资源
问答
  • SLAM回环检测技术解读

    千次阅读 2019-05-22 17:47:42
    随着路径的不断延伸,机器人在建图过程中会存在一些累计误差,除了利用局部优化、全局优化等来调整之外,还可以利用回环检测来优化位姿。 回环检测是什么? 回环检测,又称闭环检测,是指机器人识别曾到达某场景,...

     

    随着路径的不断延伸,机器人在建图过程中会存在一些累计误差,除了利用局部优化、全局优化等来调整之外,还可以利用回环检测来优化位姿。

    回环检测是什么?

    回环检测,又称闭环检测,是指机器人识别曾到达某场景,使得地图闭环的能力。说的简单点,就是机器人在左转一下,右转一下建图的时候能意识到某个地方是“我”曾经来过的,然后把此刻生成的地图与刚刚生成的地图做匹配。

     什么是回环检测

    回环检测成功

    回环检测之所以能成为一个难点,是因为:如果回环检测成功,可以显著地减小累积误差,帮助机器人更精准、快速的进行避障导航工作。而错误的检测结果可能使地图变得很糟糕。因此,回环检测在大面积、大场景地图构建上是非常有必要的 。

     回环检测是什么

    回环检测失败

    机器人回环检测能力如何提升?

    那么,怎么才能让机器人的回环检测能力得到一个质的提升呢?首先要有一个算法上的优化。

    1、基于图优化的SLAM算法

    基于图优化的SLAM 3.0 算是提升机器人回环检测能力的一大突破。

    SLAM 3.0采用图优化的方式进行建图,进行了图片集成与优化处理,当机器人运动到已经探索过的原环境时, SLAM 3.0可依赖内部的拓扑图进行主动式的闭环检测。当发现了新的闭环信息后,SLAM 3.0使用Bundle Adjuestment(BA)等算法对原先的位姿拓扑地图进行修正(即进行图优化),从而能有效的进行闭环后地图的修正,实现更加可靠的环境建图。

     SLAM30.闭环

    SLAM 3.0闭环检测

    SLAM 3.0环路闭合逻辑:先小闭环,后大闭环 ;选择特征丰富的点作为闭环点;多走重合之路,完善闭环细节。即使在超大场景下建图,也不慌。

     超大场景下建图完整闭合过程

    超大场景下建图完整闭合过程

    2、词袋模型

    除了SLAM算法的升级和优化之外,现在还有很多系统采用成熟的词袋模型方法来帮助机器人完成闭环,说的简单点就是把帧与帧之间进行特征比配。

    从每幅图像中提取特征点和特征描述,特征描述一般是一个多维向量,因此可以计算两个特征描述之间的距离;

    将这些特征描述进行聚类(比如k-means),类别的个数就是词典的单词数,比如1000;也可以用Beyes、SVM等;

    将这些词典组织成树的形式,方便搜索。

    词袋模型
    利用这个树,就可以将时间复杂度降低到对数级别,大大加速了特征匹配。

    3、相似度计算

    这种做法是从外观上根据两幅图像的相似性确定回环检测关系,那么,如何确定两个地图之间的相关性呢?

    比如对于图像A和图像B,我们要计算它们之间的相似性评分:s(A,B)。如果单单用两幅图像相减然后取范数,即为: s(A,B)=||A−B||s(A,B)=||A−B||。但是由于一幅图像在不同角度或者不同光线下其结果会相差很多,所以不使用这个函数。而是使用相似度计算公式。

    这里,我们提供一种方法叫TF-IDF。

    TF的意思是:某特征在一幅图像中经常出现,它的区分度就越高。另一方面,IDF的思想是,某特征在字典中出现的频率越低,则分类图像时的区分度越高。

    对于IDF部分,假设所有特征数量为n,某个节点的Wi所含的数量特征为Ni,那么该单词的IDF为:

    回环检测是什么意思

    TF是指某个特征在单副图像中出现的频率。假设图像A中单词Wi出现了N次,而一共出现的单词次数是n,那么TF为: 

    回环检测是什么意思

    于是Wi的权重等于TF乘IDF之积,即 

    回环检测是什么意思

    考虑权重以后,对于某副图像,我们可以得到许多个单词,得到BOW:

    回环检测是什么意思

    (A表示某幅地图)

    如何计算俩副图像相似度,这里使用了L1范数形式:

    回环检测是什么意思

    4、深度学习及其他

    除了上面的几种方式之外,回环检测也可以建成一个模型识别问题,利用深度学习的方法帮助机器人完成回环检测。比如:决策树、SVM等。

    ……

    最后,当回环出现以后,也不要急着就让机器人停止运动,要继续保持运动,多走重合的路,在已经完成闭合的路径上,进一步扫图完善细节。

     回环检测是什么意思

    继续走重合之路,完善闭环细节

    本文来源于思岚科技,原文地址:http://www.slamtec.com/en/News/Detail/188

    展开全文
  • SLAM学习——回环检测

    万次阅读 2017-07-28 00:40:29
    1.回环检测 ...有些时候,我们把仅有前端和局部后端的系统称为VO,把带有回环检测和全局后端的系统称为SLAM回环检测的方法:基于里程计的几何关系和基于外观。 基于几何关系是说,当我们发现当...

    1.回环检测

    回环检测的关键,就是如何有效的检测出相机经过同一个地方这件事。它关系到我们估计的轨迹和地图在长时间下的正确性。

    由于回环检测提供了当前数据与所有历史数据的关联,在跟踪算法丢失后,我们还可以利用重定位。有些时候,我们把仅有前端和局部后端的系统称为VO,把带有回环检测和全局后端的系统称为SLAM。

    回环检测的方法:基于里程计的几何关系和基于外观。
    基于几何关系是说,当我们发现当前相机运动到了之前的某个位置时,检测它们有没有回环关系。但是由于误差积累的存在,我们往往没办法正确发现“运动到了之前的某个位置附近”这个事实。
    基于外观的(主流做法),它和前端、后端的估计都无关,仅根据俩副图像的相似性确定回环检测关系,这种做法摆脱了积累误差,使回环检测模块成为SLAM一个相对独立的模块。

    在基于外观回环检测算法中,核心问题是如何计算图像间的相似性。比如对于图像A和图像B,我们要计算它们之间的相似性评分:s(A,B)。如果单单用俩副图像相减然后取范数 s(A,B)=||AB||但是由于一副图像在不同角度或者不同光线下其结果会相差很多,所以不使用这个函数。

    程序判断与实际总有误差,以下是我们的分类结果:

    这里写图片描述

    其中假阳性又称为感知偏差,假阴性称为感知变异。为方便,则真阳性(True Positive)简称TP,假阳性称为(False Positive)简称FP,以此类推。当然,所有的结果我们都希望FP和FN尽可能要低。对于某特定算法,我们可以统计它在某个数据集上的TP、FP、FP、FN的出现次数,并统计俩个统计量:准确率Presicion=TP/(TP+FP)和召回率Recall=TP/(TP+FN)。
    从上述公式可以得到,准确率描述的是,算法提取的所有回环中确实是真实回环的概率。而召回率是说在所有真实回环中被正确检测出来的概率。一般而言,我们更倾向于提高算法的准确性,稍微牺牲下召回率。如果出现假阳性,那么就会在后端的Pose graph添加错误的边,导致误差不准。

    2.词袋模型

    1.确定概念,例如一副图像上有人、狗等,那么“人”“狗” 等概念就是Bag-of-Words中的“单词”,许多单词放在一起,组成了“字典”。
    2.确定一副图像中出现了哪些在字典中定义的概念——用单词出现的情况(直方图)描述整副图像,转换成向量。
    3.比较上一步中的描述的相似程度。

    举个例子,若“人”“狗”都在字典里(这里的字典是已经知道的),不防设为w1,w2假设A图像中有人没狗,则可以描述成:A=1w1+0w2,由于字典是固定的,那么只需要一个向量就可以描述整副图像了,例如 A=[1,0]T

    通过这个词袋模型,我们可以得到俩张图片在这个词袋里面的向量a,bRw,那么可以计算 s(a,b)=11W||ab||1,如果俩个向量相等,那么我们得到1,完全相反(即a为0的地方b为1)则得到0。

    3.字典

    假设我们对所有的图片进行特征点的提取,比如有N个。我们现在可以寻找一个有个单词的字典,每个单词可以看作相邻特征点的集合,可以用K-means均值来做:

    1.随机选取k个中心点:c1,cn
    2.对每一个样本,计算它与每个中心点之间的距离,取最小的作为它的归类。
    3.重新计算每个类的中心点。
    4.如果每个中心点变化很小,那么退出,算法收敛;否则返回第二步。

    K-means存在小问题,就是:需要指定聚类数量,随机选取中心点等。

    为了解决这个问题,我们使用一种K叉树来表达字典。假定我们构建一个深度为d、每次分叉为k的树,那么做法如下:

    1.在根节点上,用K-means把所有样本聚成k类(有时候为保证聚类均匀性会使用k-means++),这样得到了第一层。
    2.对于第一层的每个节点,把属于该节点的样本再聚类k类,得到下一层。
    3.以此类推,最后得到叶子层。叶子层即所谓的Words。

    一方面,这样一个k分支,深度为d的树,可以容纳 kd 个单词。另一方面,在查找某个特征对应的单词时,只需要将它与每个中间节点的聚类中心比较(一共d次),即可找到最后的单词。

    紧接着我们创建字典,直接上代码:(高博用的是opencv3,我用的是opencv2,略有不同)

    #include "DBoW3/DBoW3.h" //使用了BOW这个库
    #include <opencv2/core/core.hpp>
    #include <opencv2/highgui/highgui.hpp>
    #include <opencv2/features2d/features2d.hpp>
    #include <iostream>
    #include <vector>
    #include <string>
    
    using namespace cv;
    using namespace std;
    
    int main( int argc, char** argv )
    {
        // read the image
        cout<<"reading images... "<<endl;
        vector<Mat> images;
        for ( int i=0; i<10; i++ )//这里选取了10张图片
        {
            string path = "/home/hansry/Slam_Book/src/Feature_traning/data/"+to_string(i+1)+".png";
            images.push_back( imread(path) );
        }
        // detect ORB features
        cout<<"detecting ORB features ... "<<endl;
        Ptr< Feature2D > detector = ORB::create("FAST");//由于这里用的是opencv2,所以略有不同
        vector<Mat> descriptors; 
        for ( Mat& image:images )
        {
            vector<KeyPoint> keypoints;
            Mat descriptor;
            ORB orb;
            orb( image, Mat(), keypoints, descriptor );
            descriptors.push_back( descriptor );
        }
    
        // create vocabulary
        cout<<"creating vocabulary ... "<<endl;
        DBoW3::Vocabulary vocab;
        vocab.create( descriptors );
        cout<<"vocabulary info: "<<vocab<<endl;
        vocab.save( "vocabulary.yml.gz" );
        cout<<"done"<<endl;
    
        return 0;
    }

    Cmakelist如下:

    cmake_minimum_required(VERSION 2.8.3)
    project(Feature_traning)
    
    set(CMAKE_CXX_FLAGS "-std=c++11 -O3")
    find_package(OpenCV  REQUIRED )
    set( DBoW3_INCLUDE_DIRS "/usr/local/include" )
    set( DBoW3_LIBS "/usr/local/lib/libDBoW3.so" )
    catkin_package(
    #  INCLUDE_DIRS include
    #  LIBRARIES test
    #  CATKIN_DEPENDS opencv roscpp
    #  DEPENDS system_lib
    )
    
    include_directories(include)
    include_directories(
      ${catkin_INCLUDE_DIRS}
      ${OpenCV_INCLUDE_DIRS}
    )
    add_executable(Feature_traning src/Feature_traning.cpp)
    
    target_link_libraries(Feature_traning
      ${catkin_LIBRARIES}
      ${OpenCV_LIBRARIES}
       ${DBoW3_LIBS}
    )

    结果如下图所示:

    这里写图片描述

    我们这里使用的是默认构造函数,所以默认分支数量为10,深度L为5,单词数量为4983,没有达到最大容量。weighting是权重,scoring是评分。

    4.相似度计算

    在上一part中,我们创建了字典,那么有了字典以后,给定任意特征点fi,只要在字典树中逐层查找,最后都能找到与之对应的单词 wj,如果字典足够大,我们可以说这俩个来自同一类物体。

    假设一副图像提取N个特征,找到这N个特征对应的单词之后,就相当于有了该图像在单词列表中的分布,相当于“这幅图里有一个人和一辆汽车”等。但是有的时候,我们会希望对单词的区分性或重要性加以评估,会给单词加权重。

    这里,我们提供一种方法叫TF-IDE,TF的思想是:某单词在一副图像中经常出现,它的区分度就越高。另一方面,IDE的思想是,某单词在字典中出现的频率越低,则分类图像时的区分度越高。

    对于IDF部分,假设所有特征数量为n,某个叶子节点的wi所含的数量特征为ni,那么该单词的IDF为:

    IDFi=lognni

    TF是指某个特征在单副图像中出现的频率。假设图像A中单词 wi出现了ni 次,而一共出现的单词次数是n,那么TF为:
    TFi=nin

    于是wi的权重等于TF乘IDF之积,即 ηi=TFi×IDFi

    考虑权重以后,对于某副图像,我们就可以得到许多个单词,得到BOW:

    (A是一幅图像)A={(w1,η1),(w2,η2),,(wn,ηn)}=ΔvA(是一个向量,非零部分表示含有哪些单词)

    如何计算俩副图像相似度,这里使用了L1范数形式:

    s(vAvB)=2i=1N|vAi|+|vBi||vAivBi|

    展开全文
  • ORB_SLAM2回环检测

    2021-02-25 09:58:11
    局部建图线程中,处理完一个关键帧后,会将其放入回环检测线程 void LocalMapping::Run() ............................................ mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);     检测回环 ...

    词典是特征点的描述子的集合,属于同一类特征的特征点的描述子组成单词。
    在局部建图线程中,处理完一个关键帧后,会将其放入回环检测线程

      在使用关键帧数据库搜索候选关键帧组(DetectLoopCandidates)的时候,没有考虑关键帧的连续性。因此在DetectLoop()函数中检测三个连续的闭环候选关键帧,均与当前帧有较高的相似度。
      考虑到单目相机的尺度漂移,计算当前帧与候选闭环帧的Sim3变换,而不是T。(1)使用词袋加速算法,找到与当前帧具有足够多的相同单词的候选关键帧集,并初步计算Sim3变换。(2)将候选闭环帧中的路标点投影到当前帧中,通过优化的方法进一步计算Sim3。(3)将闭环帧及其共视帧的路标点投影至当前帧中进行匹配。(4)判断所选闭环帧是否可靠((3)步中获得的匹配数目大于40)。
      闭环校正。计算了当前帧和闭环帧的Sim3变换后,去更新当前帧及其共视帧的位姿,以及路标点的坐标。其中涉及了地图点的融合,共视图的更新,本质图的优化。随后建立了一个全局BA优化线程,去优化所有的关键帧位姿及路标点坐标。

      回环检测是指:找到与当前帧4相似的关键帧1,这样的话就可以根据1直接估计4的位姿,而不是1–2--3–4。减少了误差传递。对于当前帧4的共视帧6,可以1—4---6来获得,而不是1–2--3–4--6(也就是回环校正,不过在回环校正的时候,还会去校正路标点坐标)。
    在这里插入图片描述

    void LocalMapping::Run()
    ............................................
    mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
    


    检测回环

      获得满足连续条件的闭环关键帧,插入到容器mvpEnoughConsistentCandidates中。
      虽然使用关键帧数据库,可以获得当前帧的候选闭环关键帧:

    vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
    

    但这里检测出的关键帧不满足连续性,这里筛选出那些具有连续性的关键帧。产生闭环的关键帧应具有特点:连续的三个关键帧,且三个关键帧间的相似度评分很高。

    bool LoopClosing::DetectLoop()
    

    遍历当前帧的共视关键帧

    遍历当前帧的共视关键帧(>15个路标点),使用DBow计算两帧间的词袋相似度。

        const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
        const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
        float minScore = 1;
        for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
        {
            KeyFrame* pKF = vpConnectedKeyFrames[i];
            if(pKF->isBad())
                continue;
            const DBoW2::BowVector &BowVec = pKF->mBowVec;
            // 计算两个关键帧的相似度得分;得分越低,相似度越低
            float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
            // 更新最低得分
            if(score<minScore)
                minScore = score;
        }
    

    寻找候选的闭环关键帧

    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
    

    筛选获得闭环帧

    (1)组(group): 对于某个关键帧, 其和其具有共视关系的关键帧组成了一个"组";
    (2)子候选组(CandidateGroup): 对于某个候选的回环关键帧, 其和其具有共视关系的关键帧组成的一个"组";
    (3)连续(Consistent): 不同的组之间如果共同拥有一个及以上的关键帧,那么称这两个组之间具有连续关系;
    (4)连续性(Consistency):称之为连续长度可能更合适,表示累计的连续的链的长度:A–B 为1, A–B--C–D 为3等;具体反映在数据类型 ConsistentGroup.second上;
    (5)连续组(Consistent group): mvConsistentGroups存储了上次执行回环检测时, 新的被检测出来的具有连续性的多个组的集合.由于组之间的连续关系是个网状结构,因此可能存在 一个组因为和不同的连续组链都具有连续关系,而被添加两次的情况(当然连续性度量是不相同的);
    (6)连续组链:自造的称呼,类似于菊花链A–B--C–D这样形成了一条连续组链.对于这个例子中,由于可能E,F都和D有连续关系,因此连续组链会产生分叉;为了简化计算,连续组中将只会保存最后形成连续关系的连续组们。(见下面的连续组的更新)
    (7)子连续组: 上面的连续组中的一个组;
    (8)连续组的初始值: 在遍历某个候选帧的过程中,如果该子候选组没有能够和任何一个上次的子连续组产生连续关系,那么就将添加自己组为连续组,并且连续性为0;
    (9)连续组的更新: 当前次回环检测过程中,所有被检测到和之前的连续组链有连续的关系的组,都将在对应的连续组链后面+1,这些子候选组(可能有重复,见上)都将会成为新的连续组; 换而言之连续组mvConsistentGroups中只保存连续组链中末尾的组

    (1)ConsistentGroup:连续组遍历,类型为pair<set<KeyFrame*>,int>,其中set<KeyFrame*>为连续组中的关键帧,int为连续组的长度。
    (2)spCandidateGroup:每个闭环关键帧的共视关键帧集。

    遍历每一个候选闭环关键帧
    (1)找到当前候选闭环关键帧的共视关键帧(构成一个子候选组)

    set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
    

    (2)遍历上一次闭环检测到的子连续组

    for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
    

    (3) 遍历子候选组,判断子候选组中的关键帧是否存在于子连续组中,如果存在,结束循环

                for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++)
                {
                    if(sPreviousGroup.count(*sit))
                    {
                        // 如果存在,该“子候选组”与该“子连续组”相连
                        bConsistent=true;
                        // 该“子候选组”至少与一个”子连续组“相连,跳出循环
                        bConsistentForSomeGroup=true;
                        break;
                    }
                }
    

    (4)将该子候选组插入到子连续组中,如果子连续组的长度满足要求,则将子候选关键帧插入候选闭环容器中

    (5)如果子候选组中的关键帧在所有的子连续组中都找不到,则创建一个新的子连续组,插入到连续组中

            if(!bConsistentForSomeGroup)
            {
                ConsistentGroup cg = make_pair(spCandidateGroup,0);
                vCurrentConsistentGroups.push_back(cg);
            }
    


    计算Sim3变换,获得闭环帧

    (1)通过Bow加速描述子的匹配,筛选出与当前帧的匹配特征点数大于20的候选帧集合,利用RANSAC粗略地计算出当前帧与闭环帧的Sim3(当前帧—闭环帧)
    (2)根据估计的Sim3,将每个候选帧中的路标点投影到当前帧中找到更多匹配,通过优化的方法计算更精确的Sim3(当前帧—闭环帧)。有一个帧成功了,就结束此次的循环。
    (3)找到候选帧的共视关键帧,找到所有的路标点,投影到当前帧中进行匹配(当前帧–闭环帧+共视帧)(不进行优化)。
    (4)判断候选帧是否可靠(如果(3)步匹配上的路标点的数量大于40,则闭环帧可靠)。

    计算当前关键帧和上一步中的闭环候选关键帧的Sim3变换。Sim3变换多了一个尺度变换。
    计算SIm3,而不是T的原因就是存在尺度漂移。

    /**
     * @brief 计算当前关键帧和上一步闭环候选帧的Sim3变换
     * 1. 遍历闭环候选帧集,筛选出与当前帧的匹配特征点数大于20的候选帧集合,并为每一个候选帧构造一个Sim3Solver
     * 2. 对每一个候选帧进行 Sim3Solver 迭代匹配,直到有一个候选帧匹配成功,或者全部失败
     * 3. 取出闭环匹配上关键帧的相连关键帧,得到它们的MapPoints放入 mvpLoopMapPoints
     * 4. 将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配
     * 5. 判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配
     * 6. 清空mvpEnoughConsistentCandidates
     * @return true         只要有一个候选关键帧通过Sim3的求解与优化,就返回true
     * @return false        所有候选关键帧与当前关键帧都没有有效Sim3变换
     */
    bool LoopClosing::ComputeSim3()
    

    注意以上匹配的结果均都存在成员变量mvpCurrentMatchedPoints中,实际的更新步骤见CorrectLoop()步骤3
    对于双目或者是RGBD输入的情况,计算得到的尺度=1

    遍历上一步中的候选关键帧

    如果候选关键帧在局部建图线程中被删除的话,就直接跳过。

        // Step 1. 遍历闭环候选帧集,初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合,并为每一个候选帧构造一个Sim3Solver
        for(int i=0; i<nInitialCandidates; i++)
        {
            // Step 1.1 从筛选的闭环候选帧中取出一帧有效关键帧pKF
            KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
    
            // 避免在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除
            pKF->SetNotErase();
    
            // 如果候选帧质量不高,直接PASS
            // 在局部建图线程中,如果一个关键帧被删除,则isBad为true
            if(pKF->isBad())
            {
                vbDiscarded[i] = true;
                continue;
            }
    

    通过词袋算法获得当前帧与候选帧间匹配上的特征点数量

            // Step 1.2 将当前帧 mpCurrentKF 与闭环候选关键帧pKF匹配
            // 通过bow加速得到 mpCurrentKF 与 pKF 之间的匹配特征点
            // vvpMapPointMatches 是匹配特征点对应的地图点,本质上来自于候选闭环帧
            int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
    
            // 粗筛:匹配的特征点数太少,该候选帧剔除
            if(nmatches<20)
            {
                vbDiscarded[i] = true;
                continue;
            }
            else
            {
                // Step 1.3 为保留的候选帧构造Sim3求解器
                // 如果 mbFixScale(是否固定尺度) 为 true,则是6 自由度优化(双目 RGBD)
                // 如果是false,则是7 自由度优化(单目)
                Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
    
                // Sim3Solver Ransac 过程置信度0.99,至少20个inliers 最多300次迭代
                pSolver->SetRansacParameters(0.99,20,300);
                vpSim3Solvers[i] = pSolver;
            }
    
            // 保留的候选帧数量
            nCandidates++;
    

    对每个候选关键帧求解Sim3变换,成功的关键帧进行SearchBySim3

    (1)遍历每个候选帧

    for(int i=0; i<nInitialCandidates; i++)
    

    (2)迭代计算每个候选帧的Sim3变换,如果达到迭代次数上限后,还没有合格的结果,就删除当前候选帧

                KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
    
                // 内点(Inliers)标志
                // 即标记经过RANSAC sim3 求解后,vvpMapPointMatches中的哪些作为内点
                vector<bool> vbInliers; 
            
                // 内点(Inliers)数量
                int nInliers;
    
                // 是否到达了最优解
                bool bNoMore;
    
                // Step 2.1 取出从 Step 1.3 中为当前候选帧构建的 Sim3Solver 并开始迭代
                Sim3Solver* pSolver = vpSim3Solvers[i];
    
                // 最多迭代5次,返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换(T12)
                cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
    
                // If Ransac reachs max. iterations discard keyframe
                // 总迭代次数达到最大限制还没有求出合格的Sim3变换,该候选帧剔除
                if(bNoMore)
                {
                    vbDiscarded[i]=true;
                    nCandidates--;
                }
    

    (3)如果计算出了Sim3变换,就将当前帧mpCurrentKF和候选关键帧pKF中的路标点相互投影匹配,以匹配上更多的路标点。随后根据匹配关系进行优化。只要有一个优化成功了,就直接结果while循环

                if(!Scm.empty())
                {
                    // 取出经过Sim3Solver 后匹配点中的内点集合
                    vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
                    for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
                    {
                        // 保存内点
                        if(vbInliers[j])
                           vpMapPointMatches[j]=vvpMapPointMatches[i][j];
                    }
    
                    // Step 2.2 通过上面求取的Sim3变换引导关键帧匹配,弥补Step 1中的漏匹配
                    // 候选帧pKF到当前帧mpCurrentKF的R(R12),t(t12),变换尺度s(s12)
                    cv::Mat R = pSolver->GetEstimatedRotation();
                    cv::Mat t = pSolver->GetEstimatedTranslation();
                    const float s = pSolver->GetEstimatedScale();
    
                    // 查找更多的匹配(成功的闭环匹配需要满足足够多的匹配特征点数,之前使用SearchByBoW进行特征点匹配时会有漏匹配)
                    // 通过Sim3变换,投影搜索pKF1的特征点在pKF2中的匹配,同理,投影搜索pKF2的特征点在pKF1中的匹配
                    // 只有互相都成功匹配的才认为是可靠的匹配
                    matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
    
                    // Step 2.3 用新的匹配来优化 Sim3,只要有一个候选帧通过Sim3的求解与优化,就跳出停止对其它候选帧的判断
                    // OpenCV的Mat矩阵转成Eigen的Matrix类型
                    // gScm:候选关键帧到当前帧的Sim3变换
                    g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
                
                    // 如果mbFixScale为true,则是6 自由度优化(双目 RGBD),如果是false,则是7 自由度优化(单目)
                    // 优化mpCurrentKF与pKF对应的MapPoints间的Sim3,得到优化后的量gScm
                    const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
    
                    // 如果优化成功,则停止while循环遍历闭环候选
                    if(nInliers>=20)
                    {
                        // 为True时将不再进入 while循环
                        bMatch = true;
                        // mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧
                        mpMatchedKF = pKF;
    
                        // gSmw:从世界坐标系 w 到该候选帧 m 的Sim3变换,都在一个坐标系下,所以尺度 Scale=1
                        g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
    
                        // 得到g2o优化后从世界坐标系到当前帧的Sim3变换
                        mg2oScw = gScm*gSmw;
                        mScw = Converter::toCvMat(mg2oScw);
                        mvpCurrentMatchedPoints = vpMapPointMatches;
    
                        // 只要有一个候选帧通过Sim3的求解与优化,就跳出停止对其它候选帧的判断
                        break;
                    }
                }
    
    

    如果所有候选关键帧经过SIM3变换均失败,则返回false

        // 退出上面while循环的原因有两种,一种是求解到了bMatch置位后出的,另外一种是nCandidates耗尽为0
        if(!bMatch)
        {
            // 如果没有一个闭环匹配候选帧通过Sim3的求解与优化
            // 清空mvpEnoughConsistentCandidates,这些候选关键帧以后都不会在再参加回环检测过程了
            for(int i=0; i<nInitialCandidates; i++)
                mvpEnoughConsistentCandidates[i]->SetErase();
            // 当前关键帧也将不会再参加回环检测了
            mpCurrentKF->SetErase();
            // Sim3 计算失败,退出了
            return false;
        }
    

    取出与当前帧闭环匹配上的关键帧及其共视关键帧,以及这些共视关键帧的地图点

        vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
    
        // 包含闭环匹配关键帧本身,形成一个“闭环关键帧小组“
        vpLoopConnectedKFs.push_back(mpMatchedKF);
        mvpLoopMapPoints.clear();
    
        // 遍历这个组中的每一个关键帧
        for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
        {
            KeyFrame* pKF = *vit;
            vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
    
            // 遍历其中一个关键帧的所有有效地图点
            for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
            {
                MapPoint* pMP = vpMapPoints[i];
                if(pMP)
                {
                    // mnLoopPointForKF 用于标记,避免重复添加
                    if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
                    {
                        mvpLoopMapPoints.push_back(pMP);
                        // 标记一下
                        pMP->mnLoopPointForKF=mpCurrentKF->mnId;
                    }
                }
            }
        }
    

    将闭环关键帧及其共视关键帧的所有地图点投影到当前关键帧进行投影匹配

    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
    

    判断闭环是否可靠

    统计当前帧与候选闭环及其共视帧中匹配上的路标点的数量,小于40的话,认为不可靠。

        // Step 4:将闭环关键帧及其共视关键帧的所有地图点投影到当前关键帧进行投影匹配
        // 根据投影查找更多的匹配(成功的闭环匹配需要满足足够多的匹配特征点数)
        // 根据Sim3变换,将每个mvpLoopMapPoints投影到mpCurrentKF上,搜索新的匹配对
        // mvpCurrentMatchedPoints是前面经过SearchBySim3得到的已经匹配的点对,这里就忽略不再匹配了
        // 搜索范围系数为10
        matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
    
        // If enough matches accept Loop
        // Step 5: 统计当前帧与检测出的所有闭环关键帧的匹配地图点数目,超过40个说明成功闭环,否则失败
        int nTotalMatches = 0;
        for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
        {
            if(mvpCurrentMatchedPoints[i])
                nTotalMatches++;
        }
    
        if(nTotalMatches>=40)
        {
            // 如果当前回环可靠,保留当前待闭环关键帧,其他闭环候选全部删掉以后不用了
            for(int i=0; i<nInitialCandidates; i++)
                if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
                    mvpEnoughConsistentCandidates[i]->SetErase();
            return true;
        }
        else   
        {
            // 闭环不可靠,闭环候选及当前待闭环帧全部删除
            for(int i=0; i<nInitialCandidates; i++)
                mvpEnoughConsistentCandidates[i]->SetErase();
            mpCurrentKF->SetErase();
            return false;
        }
    


    闭环校正

    闭环关键帧的位姿会根据Sim3进行校正,同时与其相连的关键帧的位姿即路标点的位置也会被校正。
    Sim3校正对旋转矩阵没有影响,只会影响平移矩阵。


    (1)在上一帧计算当前帧和闭环帧的Sim3位姿变换时,建立了闭环帧及其共视帧的路标点与当前帧的联系,因此先更新共视图。
    (2)根据计算的当前帧和闭环帧的Sim3变换,去更新当前帧及其共视帧的位姿,以及路标点的坐标。
    (3)因为闭环帧已经经过了多次优化,认为是精确的,因此建立闭环帧及其共视帧的路标点与当前帧及其共视帧的联系,进行路标点的匹配、融合。
    (4)优化本质图(只优化位姿)
    (5)建立一个全局BA优化线程

    结束局部地图线程、全局BA,为闭环校正做准备

        mpLocalMapper->RequestStop();
    
        if(isRunningGBA())
        {
            // 如果有全局BA在运行,终止掉,迎接新的全局BA
            unique_lock<mutex> lock(mMutexGBA);
            mbStopGBA = true;
            // 记录全局BA次数
            mnFullBAIdx++;
            if(mpThreadGBA)
            {
                // 停止全局BA线程
                mpThreadGBA->detach();
                delete mpThreadGBA;
            }
        }
    

    更新共视图

    在闭环检测、计算Sim3的过程中,建立了当前帧的特征点和其闭环帧、闭环帧的共视帧的路标点间的联系,因此这里需要更新一个共视图。

        // Step 1:根据共视关系更新当前关键帧与其它关键帧之间的连接关系
        // 因为之前闭环检测、计算Sim3中改变了该关键帧的地图点,所以需要更新
        mpCurrentKF->UpdateConnections();
    

    更新当前帧、与其相连的关键帧的位姿。

    当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化,通过相对位姿关系,可以确定与当前帧相连的关键帧与世界坐标系之间的Sim3变换

    (1)通过mg2oScw(认为是准的)来进行位姿传播,得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿.遍历"当前关键帧组"(当前帧+共视帧)

            // Step 2.1:通过mg2oScw(认为是准的)来进行位姿传播,得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿(还没有修正)
            // 遍历"当前关键帧组""
            for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
            {
                KeyFrame* pKFi = *vit;
                cv::Mat Tiw = pKFi->GetPose();
                if(pKFi!=mpCurrentKF)      //跳过当前关键帧,因为当前关键帧的位姿已经在前面优化过了,在这里是参考基准
                {
                    // 得到当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的相对变换
                    cv::Mat Tic = Tiw*Twc;
                    cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
                    cv::Mat tic = Tic.rowRange(0,3).col(3);
    
                    // g2oSic:当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的Sim3 相对变换
                    // 这里是non-correct, 所以scale=1.0
                    g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
                    // 当前帧的位姿固定不动,其它的关键帧根据相对关系得到Sim3调整的位姿
                    g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
                    // Pose corrected with the Sim3 of the loop closure
                    // 存放闭环g2o优化后当前关键帧的共视关键帧的Sim3 位姿
                    CorrectedSim3[pKFi]=g2oCorrectedSiw;
                }
    
                cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
                cv::Mat tiw = Tiw.rowRange(0,3).col(3);
                g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
                // Pose without correction
                // 存放没有矫正的当前关键帧的共视关键帧的Sim3变换
                NonCorrectedSim3[pKFi]=g2oSiw;
            }
    

    (2)校正当前帧的共视关键帧的路标点坐标
    路标点世界坐标------(未校正的T)--------路标点相机坐标-------(校正的Sim3)--------路标点世界坐标
    保持路标点和帧间的相对位置不变。
    要记得更新地图点的平均观测方向和观测范围

            // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
            // Step 2.2:得到矫正的当前关键帧的共视关键帧位姿后,修正这些关键帧的地图点
            // 遍历待矫正的共视关键帧
            for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
            {
                KeyFrame* pKFi = mit->first;
                g2o::Sim3 g2oCorrectedSiw = mit->second;
                g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
    
                g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];
    
                vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();
                // 遍历待矫正共视关键帧中的每一个地图点
                for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++)
                {
                    MapPoint* pMPi = vpMPsi[iMP];
                    if(!pMPi)
                        continue;
                    if(pMPi->isBad())
                        continue;
                    if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId) // 标记,防止重复矫正
                        continue;
    
                    // 矫正过程本质上也是基于当前关键帧的优化后的位姿展开的
                    // Project with non-corrected pose and project back with corrected pose
                    // 将该未校正的eigP3Dw先从世界坐标系映射到未校正的pKFi相机坐标系,然后再反映射到校正后的世界坐标系下
                    cv::Mat P3Dw = pMPi->GetWorldPos();
                    // 地图点世界坐标系下坐标
                    Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
                    // map(P) 内部做了变换 R*P +t  
                    // 下面变换是:eigP3Dw: world →g2oSiw→ i →g2oCorrectedSwi→ world
                    Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
    
                    cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
                    pMPi->SetWorldPos(cvCorrectedP3Dw);
                    // 记录矫正该地图点的关键帧id,防止重复
                    pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
                    // 记录该地图点所在的关键帧id
                    pMPi->mnCorrectedReference = pKFi->mnId;
                    // 因为地图点更新了,需要更新其平均观测方向以及观测距离范围
                    pMPi->UpdateNormalAndDepth();
                }
    

    (3)将共视关键帧的Sim3转换为SE3,根据更新的Sim3,更新关键帧的位姿

                // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
                // Step 2.3:将共视关键帧的Sim3转换为SE3,根据更新的Sim3,更新关键帧的位姿
                // 其实是现在已经有了更新后的关键帧组中关键帧的位姿,但是在上面的操作时只是暂时存储到了 KeyFrameAndPose 类型的变量中,还没有写回到关键帧对象中
                // 调用toRotationMatrix 可以自动归一化旋转矩阵
                Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix(); 
                Eigen::Vector3d eigt = g2oCorrectedSiw.translation();                  
                double s = g2oCorrectedSiw.scale();
                // 平移向量中包含有尺度信息,还需要用尺度归一化
                eigt *=(1./s); 
    
                cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
                // 设置矫正后的新的pose
                pKFi->SetPose(correctedTiw);
    

    更新当前帧路标点

    更新当前帧中的路标点,应为在ComputeSim3()函数获取闭环帧的时候,将闭环帧及其共视帧的路标点和当前帧的特征点进行了匹配

            // Step 3:检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突,对冲突的进行替换或填补
            // mvpCurrentMatchedPoints 是当前关键帧和闭环关键帧组的所有地图点进行投影得到的匹配点
            for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
            {
                if(mvpCurrentMatchedPoints[i])
                {
                    //取出同一个索引对应的两种地图点,决定是否要替换
                    // 匹配投影得到的地图点
                    MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
                    // 原来的地图点
                    MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i); 
                    if(pCurMP)
                        // 如果有重复的MapPoint,则用匹配的地图点代替现有的
                        // 因为匹配的地图点是经过一系列操作后比较精确的,现有的地图点很可能有累计误差
                        pCurMP->Replace(pLoopMP);
                    else
                    {
                        // 如果当前帧没有该MapPoint,则直接添加
                        mpCurrentKF->AddMapPoint(pLoopMP,i);
                        pLoopMP->AddObservation(mpCurrentKF,i);
                        pLoopMP->ComputeDistinctiveDescriptors();
                    }
                }
            } 
    
        }
    

    路标点融合

    在局部建图的时候,已经获得了当前帧及其共视帧所联系的路标点。这里又已知闭环帧及其共视帧的路标点。闭环帧出现在当前帧之前,进行了多次优化,因此其路标点准确。所以将闭环帧及其共视帧的路标点投影到当前帧组中进行匹配,融合。

    SearchAndFuse(CorrectedSim3);
    

    更新当前帧的连接关系

    前面进行了路标点的融合,这里要更改一下连接关系。
    map<KeyFrame*, set<KeyFrame*> > LoopConnections:
    KeyFrame:当前帧及其共视关键帧中的一帧
    set<KeyFrame*>:KeyFrame的共视关键帧

    优化共视图

    Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
    

    添加当前帧与闭环匹配帧之间的边

        mpMatchedKF->AddLoopEdge(mpCurrentKF);
        mpCurrentKF->AddLoopEdge(mpMatchedKF);
    

    建立一个线程进行全局BA优化

    mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
    

    全局BA优化线程

    优化所有的关键帧及路标点

    void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
    
    展开全文
  • slam中,前端负责提供轨迹与地图的初值,后端负责对轨迹与地图进行优化。...拿视觉slam来说,虽然对于当前位姿的估计我会使用ba进行优化,但是这个ba一般是基于局部地图的优化,首先必然会存在误差(ba只

    在slam中,前端负责提供轨迹地图初值,后端负责对轨迹与地图进行优化。在小范围的环境中,前端与后端便已经够用了。但是当活动的范围增加以后,这一类只依靠内部传感器(相机、imu等)的定位方法不可避免的会出现累积误差,这时因为虽然我们可以使用ba等优化方式进行优化,但是这种优化方式毕竟还是局部的(比如某一个位姿下会有大部分的地图点是看不到的)。

    回环检测

    回环检测与累积误差

    拿视觉slam来说,虽然对于当前位姿的估计我会使用ba进行优化,但是这个ba一般是基于局部地图的优化,首先必然会存在误差(ba只是尽量减小它),然后以后某一时刻的位姿会依赖此处的位姿结果进行估计,于是累积误差就出现了。

    一般累积误差最常见的是位姿的累积误差,在单目视觉中,还会出现尺度累积误差

    针对上述的累积误差问题,我们可以使用回环检测来修正。一般回环检测的步骤如下:

    1. 检测到回环的发生

    2. 计算回环侯选帧与当前帧的运动

    3. 验证回环是否成立

    4. 闭环

    回环检测可以修正累积误差,并且在各种图优化模型中,加入回环的约束可以有效的提高优化结果的精度。

    检测回环

    检测回环的方法很多。朴素的方法:

    1. 最简单的方法是对任意两幅图像都做一遍特征匹配,根据正确匹配的数量确定哪两幅图像存在关联。这种方法朴素但是有效,缺点是任意两幅图像做特征匹配,计算量实在太大,因此不实用

    2. 随机抽取历史数据进行回环检测,比如在nn帧中随机抽5帧与当前帧比较。时间效率提高很多,但是抽到回环几率不高,也就是检测效率不高

    系统的方法:

    1. 基于里程计几何关系的方法
      • 大概思路是当我们发现当前相机运动到之前的某个位置附近时,检测他们是否存在回环
      • 缺点:由于累积误差,很难确定运动到了之前某个位置
    2. 基于外观的方法
      • 仅仅依靠两幅图像的相似性确定回环
      • 核心问题是如何计算图像的相似性
      • 这个问题可以看成一个二分类问题,并且slam特性要求这个分类准确率必须要高,召回率可以相对低一点。

    目前最广泛使用并且最有效的方法是基于外观的一种方法,使用词袋模型进行回环检测

    需要注意的问题

    相似性评分的处理

    使用词袋模型,对于任意两张图片我们都可以给出一个相似性评分,但是有些环境本身就很相似,比如每间办公室中的桌椅可能款式一样,使得任意两幅图像之间的相似度都挺高的。考虑这种情况,我们通常会取一个先验相似度,它表示某一时刻关键帧图像与上一时刻关键帧的相似性。然后其他的分枝都参照这个值进行归一化:

    一般我们会假设,如果当前帧与之前某关键帧的相似度超过当前帧与上一个关键帧相似度的3倍,我们才会认为可能存在回环。这样处理可以避免引入绝对的相似性阈值

    关键帧的处理

    由于上面采用的相似性评分方式,如果我们在选取关键帧时选的很近,会使得两个相邻关键帧之间相似性太高,这样历史中的回环会很难检测出来。所以用于回环检测的关键帧最后是稀疏一些,彼此不太相同,同时又能涵盖整个环境。

    回环聚类

    如果我们的机器人将第1帧第n帧检测出了回环,那么很有可能接下来的第n+1帧、第n+2帧都会和第1帧构成回环。但是确认第1帧与第n帧的回环对整个轨迹优化帮助很大,接下来的第n+1帧与第1帧的回环帮助就没那么大了。因为我们之前已经消除了累积误差,更多的回环带来的帮助不会更大。此时,我们通常把相近的回环聚成一类,使得算法不要检测同一类的回环。

    回环检测后的验证

    使用词袋的回环检测完全依赖外观,因此会出现由于外观相似而出现的错误检测(比如酒店里布置完全一样的两件客房)。所以在回环检测后,我们会进行验证。

    验证方法很多,比如

    1. 时间一致性检测。设立回环的缓存机制,认为单次检测到的回环不足以构成良好的约束,而是在一段时间内一直检测到的回环才是正确的回环。

    2. 空间一致性检测。对回环检测到的两帧进行特征匹配,估计相机运动。再将运动放到之前的位姿图中检测是否出入过大。

    机器学习

    词袋模型,本身是一种非监督的机器学习过程(基于树结构的聚类,数结构加速查找)。而回环检测本身是一个分类问题,只是这个分类问题是一种稠密的分类,分类类别近似于连续变量。

    由于目前基于词袋模型的物体识别已经明显不如神经网络了,而回环检测又是一项相似的问题,之后这部分的工作也许可以尝试使用深度学习来替换。

     

    展开全文
  • 回环检测(SLAM十四讲ch12)

    千次阅读 2018-09-03 17:14:02
    除了利用优化方法在局部和全局调整位姿,也可以利用回环检测(loop closure)来优化位姿。 问题的关键是:如何判断两帧图片的相似度。最直观的做法是:特征匹配,比较匹配的数量是否足够多。此时,可以采用词袋模型...
  • ORB-SLAM(六)回环检测

    千次阅读 2017-07-03 19:38:35
    ORB-SLAM(六)回环检测 上一篇提到,无论在单目、双目还是RGBD中,追踪得到的位姿都是有误差的。随着路径的不断延伸,前面帧的误差会一直传递到后面去,导致最后一帧的位姿在世界坐标系里的误差有可能...
  • SLAM(一)-回环检测

    千次阅读 2017-11-29 14:50:05
    局部区域,人不断移动从而在脑海中建造增量式地图,但时间长了,人也会分不清现在到底朝向哪边,与起始点的关系如何。假如也恰好在某一时刻回到了之前路过的位置,如果这个人对环境足够敏感,他就能发现这一事实...
  • ORB-Slam2详解6 回环

    千次阅读 2017-12-04 14:33:58
    毋庸置疑的是,随着相机的运动,我们计算的相机位姿,三角化得到的点云位置,都是有误差的,即使我们使用局部的或全局的BA去优化,仍然会存在累积误差。而消除误差最有效的办法是发现闭环,并根据闭环对所有结果进行...
  • 1.DetectLoopCandidates(KeyFrame* pKF, float minScore) @param pKF 需要闭环的关键帧 @param minScore 相似性分数最低要求 ... //spConnectedKeyFrames与pKF局部相连的关键帧 set<KeyFrame*> spConnecte...
  • 请求局部地图线程停止,并且中止现有的全局优化进程 // Send a stop signal to Local Mapping // Avoid new keyframes are inserted while correcting the loop mpLocalMapper-&gt;RequestStop(); // If a ...
  • 分为追踪线程、局部地图构建线程、回环检测线程和可视化显示线程。或许还有添加后来添加的点云3d地图构建线程。言归正传。 1.局部地图初始化 mpLocalMapper = new LocalMapping(mpMap, mSensor==RGBD); //变量赋值 ...
  • 回环线程主要使用局部建图处理的最后一个关键帧KiK_iKi​去进行检测和回环,具体步骤见下:
  • 局部构图模块一样回环检测模块同样是存在一个线程函数run; void LoopClosing::Run() { mbFinished =false; while(1) { // Check if there are keyframes in the queue // Loopclosing中的关键帧是Local...
  •  为什么叫“局部”地图构建,我的理解是这个线程的主要任务是像地图中插入关键帧(包括地图点等信息),以及需要进行LocalBA优化一个局部地图,这是相对于回环检测时进行的全局优化来说的,所以称为局部地图构建。...
  • SLAM概述

    2020-11-19 16:39:29
    1. SLAM的定义 2. SLAM的基本框架 传感器信息读取。 在视觉SLAM中主要为相机图像信息的...后端接受不同时刻视觉里程计测量的相机位姿,以及回环检测的信息,对它们进行优化,得到全局一致的轨迹和地图。由于接在VO
  • 该系统是精简版的小型双目视觉slam系统 前端由视觉里程计构成,后端是一个局部优化,该系统缺少回环检测,详细请看slam十四讲。 下面对该系统进行分析: 首先分析源码中的数据结构: 定义了以下类: 特征类 ...
  • 图2 ORB-SLAM2由三个平行的线程组成,跟踪,局部建图和回环检测。在一次回环检测后,会执行第四个线程,去执行BA优化。跟踪的线程在双目或者RGB-D输入之前进行,因此剩下的系统模块能够跟传感器模块独立运行。单目的...
  • DS-SLAM学习

    2020-02-26 14:54:22
    DS-SLAM理解介绍 介绍 共有5个线程 跟踪、语义分割、局部建图、回环检测和语义地图创建 在动态环境下,其精度相比提高了一个数量级
  • 它将整个SLAM过程划分为三个线程:(1)tracking线程:追踪,它是整个系统数据流入的开端,也是VO视觉里程计的过程(2)local mapping线程:局部地图构建(3)local closing线程:回环检测三个线程分别存放在对应的...
  • 版权声明:本文为博主原创文章,未经博主允许不得转载。   ORB-SLAM[1]完全继承了PTAM...为了实现这两点改进,ORB-SLAM把PTAM的mapping线程基于局部BA和全局BA拆分成了local mapp...
  • ORB-SLAM2初步

    2017-05-09 21:24:00
    一、ORB-SLAM简介  最近开始入坑SLAM,经过简单调研,各位大咖认为,目前最优秀的视觉...整个系统基于ORB特征实现,包含了跟踪定位、局部地图构建、回环检测三个主线程。通常认为ORB-SLAM是基于PTAM的而进一步发展...
  • 经典视觉SLAM框架

    2019-10-05 12:00:46
    经典视觉SLAM框架 整个视觉SLAM流程包括以下步骤: 1.传感器信息读取。在视觉SLAM中主要为相机图像信息的读取和预处理。 ...后端接受不同时刻视觉里程计测量的相机位姿以及回环检测的信息,...
  • 一起学ORBSLAM2(11)ORBSLAM的localmapping

    千次阅读 2018-06-05 22:35:05
    https://blog.csdn.net/qq_30356613/article/category/6897125ORBSLAM局部建图线程实际做的工作是来维护全局map以及管理关键帧的,对tracking得到的关键帧进行筛选融合,以及对关键帧中的地图点进行融合,剔除冗余...
  • orbslam算法框架

    2019-05-11 21:47:00
    ORB-SLAM[1]完全继承了PTAM...为了实现这两点改进,ORB-SLAM把PTAM的mapping线程基于局部BA和全局BA拆分成了local mapping和loop closing两个线程,用ORB描述子替换了patch匹...
  • ORB-SLAM3论文解读

    2020-11-06 22:45:51
    局部建图线程 回环和地图合并线程 相机模型 相机模型带来的重定位问题 相机模型带来的双目问题 视觉惯性SLAM部分 IMU初始化 纯视觉最大后验估计 纯惯性最大后验估计 ...
  • ORB-SLAM2——整体框架

    2019-11-04 16:32:51
    ORB_SLAM2由3+1个平行线程组成,包括跟踪、局部建图、回环检测以及在回环检测后的全局BA优化。之所以说是3+1,因为第四个线程仅在回环检测并确认后才执行。 三个主要并行线程: Tracking:寻找局部地图特征点并...
  • 1. 视觉SLAM 系统概述 SLAM 是Simultaneous Localization and Mapping 的缩写,中文译作“同时定位与地图构建” 。...后端接受不同时刻视觉里程计测量的相机位姿,以及回环检测的信息,对它们进行优化,得到全局一致的

空空如也

空空如也

1 2 3
收藏数 57
精华内容 22
关键字:

局部回环slam