精华内容
下载资源
问答
  • SLAM回环及优化

    千次阅读 2021-08-18 10:18:25
    以及ORB-SLAM为代表包含的局部优化和全局优化来调整外。但是这些处理方式只能减缓误差累计的程度,无法消除,而现在最为常用消除累计误差的方法就是利用回环检测来优化位姿。 当新的关键帧加入到优化模型时,在关键...

    0. 前言

    随着路径的不断延伸,机器人的建图过程会存在不断地累计误差。而传统的以gmapping为代表的使用粒子滤波进行定位的slam建图方式。以及ORB-SLAM为代表包含的局部优化和全局优化来调整外。但是这些处理方式只能减缓误差累计的程度,无法消除,而现在最为常用消除累计误差的方法就是利用回环检测来优化位姿。

    当新的关键帧加入到优化模型时,在关键帧附近进行一次局部优化
    在这里插入图片描述
    全局优化中,所有的关键帧(除了第一帧)和三维点都参与优化。
    在这里插入图片描述

    1. 回环检测

    回环检测作为近年来slam行业必备的部分,是指机器人识别曾到达某场景,使得地图闭环的能力。
    在这里插入图片描述
    回环检测之所以能成为一个难点,是因为:如果回环检测成功,可以显著地减小累积误差,帮助机器人更精准、快速的进行避障导航工作。而错误的检测结果可能使地图变得很糟糕。因此,回环检测在大面积、大场景地图构建上是非常有必要的 。

    词袋模型(bag of words,BoW)早期是一种文本表征方法,后引入到计算机视觉领域,逐渐成为一种很有效的图像特征建模方法。它通过提取图像特征,再将特征进行分类构建视觉字典,然后采用视觉字典中的单词集合可以表征任一幅图像。换句话说,通过BOW可以把一张图片表示成一个向量。这对判断图像间的关联很有帮助,所以目前比较流行的回环解决方案都是采用的BoW及其基础上衍生的算法IAB-MAPFAB-MAP是在滤波框架下计算回环概率,RTAB-MAP采用关键帧比较相似性,DLoopDetector(在DBoW2基础上开发的回环检测库)采用连续帧的相似性检测判断是否存在回环。回环检测主要由BoW模块、算法模块、验证模块三部分组成

    词袋模型(BoW模块)

    每一帧都可以用单词来描述,也就是这一帧中有哪些单词,这里只关心了有没有,而不必关心具体在哪里。只有两帧中单词种类相近才可能构成回环。

    (a)图像预处理,假设训练集有 M M 幅图像,将图像标准化为 p a t c h patch patch,统一格式和规格;
    (b)特征提取,假设 M M 幅图像,对每一幅图像提取特征,共提取出 N N 个SIFT特征;
    (c)特征聚类,采用K-means算法把 N N N个对象分为 K K K个簇 (视觉单词表),使簇内具有较高的相似度,而簇间相似度较低;
    在这里插入图片描述
    (d)统计得到图像的码本,每幅图像以单词表为规范对该幅图像的每一个SIFT特征点计算它与单词表中每个单词的距 离,最近的加1,便得到该幅图像的码本;还需要码本矢量归一化,因为每一幅图像的SIFT特征个数不定,所以需要归一化。
    在这里插入图片描述

    …详情请参照古月居

    展开全文
  • 2. 检测是否有回环 Step1:从队列中取出一个关键帧,作为当前检测闭环关键帧,设置当前关键帧不要在优化的过程中被删除; Step2:如果距离上次闭环没多久(小于10帧),或者map中关键帧总共还没有10帧...

    目录

    基本步骤

    主流程代码实现

    参考


    基本步骤

    在系统初始化是先建立好LoopClosing线程,定义关键帧队列,LocalMapping会在运行过程中将关键帧插入到该队列中,然后主流程如下所示:

    1.  查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来

    2. 检测是否有回环

    • Step 1:从队列中取出一个关键帧,作为当前检测闭环关键帧,设置当前关键帧不要在优化的过程中被删除;
    • Step 2:如果距离上次闭环没多久(小于10帧),或者map中关键帧总共还没有10帧,则不进行闭环检测;
    • Step 3:遍历当前回环关键帧所有连接(>15个共视地图点)的相邻关键帧,计算当前关键帧与每个共视关键的BOW相似度得分,并得到最低得分minScore(认为和当前关键帧具有回环关系的关键帧,不应该低于当前关键帧的相邻关键帧的最低的相似度minScore);
    • Step 4:在所有关键帧中找出闭环候选帧(注意不和当前帧连接);
    • Step 5:在候选帧中检测具有连续性的候选帧
      • a、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”, vpCandidateKFs-->spCandidateGroup
      • b、检测“子候选组”中每一个关键帧是否存在于“连续组”,如果存在 nCurrentConsistency++,则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
      • c、如果nCurrentConsistency大于等于3,那么该”子候选组“代表的候选帧过关,进入mvpEnoughConsistentCandidates
      • 相关的概念说明:(为方便理解,见视频里的图示)
        • 组(group): 对于某个关键帧, 其和其具有共视关系的关键帧组成了一个"组";
        • 子候选组(CandidateGroup): 对于某个候选的回环关键帧, 其和其具有共视关系的关键帧组成的一个"组";
        • 连续(Consistent):  不同的组之间如果共同拥有一个及以上的关键帧,那么称这两个组之间具有连续关系
        • 连续性(Consistency):称之为连续长度可能更合适,表示累计的连续的链的长度:A--B 为1, A--B--C--D 为3等;具体反映在数据类型 ConsistentGroup.second上
        • 连续组(Consistent group): mvConsistentGroups存储了上次执行回环检测时, 新的被检测出来的具有连续性的多个组的集合.由于组之间的连续关系是个网状结构,因此可能存在一个组因为和不同的连续组链都具有连续关系,而被添加两次的情况(当然连续性度量是不相同的)
        • 连续组链:自造的称呼,类似于菊花链A--B--C--D这样形成了一条连续组链.对于这个例子中,由于可能E,F都和D有连续关系,因此连续组链会产生分叉;为了简化计算,连续组中将只会保存最后形成连续关系的连续组们(见下面的连续组的更新)
        • 子连续组: 上面的连续组中的一个组
        • 连续组的初始值: 在遍历某个候选帧的过程中,如果该子候选组没有能够和任何一个上次的子连续组产生连续关系,那么就将添加自己组为连续组,并且连续性为0(相当于新开了一个连续链)
        • 连续组的更新: 当前次回环检测过程中,所有被检测到和之前的连续组链有连续的关系的组,都将在对应的连续组链后面+1,这些子候选组(可能有重复,见上)都将会成为新的连续组;换而言之连续组mvConsistentGroups中只保存连续组链中末尾的组
      • 最终筛选后得到的闭环帧

    3.计算Sim3

    4.进行回环校正

    主流程代码实现

    参考:LoopClosing.cc

    // 回环线程主函数
    void LoopClosing::Run()
    {
        mbFinished =false;
    
        // 线程主循环
        while(1)
        {
            // Check if there are keyframes in the queue
            // Loopclosing中的关键帧是LocalMapping发送过来的,LocalMapping是Tracking中发过来的
            // 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
            // Step 1 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来
            if(CheckNewKeyFrames())
            {
                // Detect loop candidates and check covisibility consistency
                if(DetectLoop())
                {
                   // Compute similarity transformation [sR|t]
                   // In the stereo/RGBD case s=1
                   if(ComputeSim3())
                   {
                       // Perform loop fusion and pose graph optimization
                       CorrectLoop();
                   }
                }
            }
    
            // 查看是否有外部线程请求复位当前线程
            ResetIfRequested();
    
            // 查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环
            if(CheckFinish())
                break;
    
            //usleep(5000);
    		std::this_thread::sleep_for(std::chrono::milliseconds(5));
    
    	}
    
        // 运行到这里说明有外部线程请求终止当前线程,在这个函数中执行终止当前线程的一些操作
        SetFinish();
    }

    参考

    • 计算机视觉life 第32讲-闭环线程中寻找初始闭环候选关键帧

     

    展开全文
  • 以及ORB-SLAM为代表包含的局部优化和全局优化来调整外。但是这些处理方式只能减缓误差累计的程度,无法消除,而现在最为常用消除累计误差的方法就是利用回环检测来优化位姿。 当新的关键帧加入到优化模型时,在关键...

    点击上方“3D视觉工坊”,选择“星标”

    干货第一时间送达

    5fb12bd0887f0c32a34ca4eccd7326bb.png

    作者丨 lovely_yoshino

    来源丨古月居

    bc23a0fc512339b06fa93e0c410e0384.png

    3083330f7997403e927d5eeef72a8540.png

    0. 前言

    随着路径的不断延伸,机器人的建图过程会存在不断地累计误差。而传统的以gmapping为代表的使用粒子滤波进行定位的slam建图方式。以及ORB-SLAM为代表包含的局部优化和全局优化来调整外。但是这些处理方式只能减缓误差累计的程度,无法消除,而现在最为常用消除累计误差的方法就是利用回环检测来优化位姿。

    当新的关键帧加入到优化模型时,在关键帧附近进行一次局部优化。

    f9e97abe371344bdc3f666b4c9924c7b.png

    在全局优化中,所有的关键帧(除了第一帧)和三维点都参与优化。

    a0d02597abb02ceadb9ad30c120c4158.png

    eb05bc79d8b85661d3c6df03bf8fc183.png

    212848581ea5e98db2f44488e3f50ca2.png

    1. 回环检测

    回环检测作为近年来slam行业必备的部分,是指机器人识别曾到达某场景,使得地图闭环的能力。

    8d36f27cf47da7b77097449b7b7536bf.png

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

    词袋模型(bag of words,BoW)早期是一种文本表征方法,后引入到计算机视觉领域,逐渐成为一种很有效的图像特征建模方法。它通过提取图像特征,再将特征进行分类构建视觉字典,然后采用视觉字典中的单词集合可以表征任一幅图像。

    换句话说,通过BOW可以把一张图片表示成一个向量。这对判断图像间的关联很有帮助,所以目前比较流行的回环解决方案都是采用的BoW及其基础上衍生的算法IAB-MAP、FAB-MAP是在滤波框架下计算回环概率,RTAB-MAP采用关键帧比较相似性,DLoopDetector(在DBoW2基础上开发的回环检测库)采用连续帧的相似性检测判断是否存在回环。回环检测主要由BoW模块、算法模块、验证模块三部分组成。

    词袋模型(BoW模块)

    每一帧都可以用单词来描述,也就是这一帧中有哪些单词,这里只关心了有没有,而不必关心具体在哪里。只有两帧中单词种类相近才可能构成回环。

    (a)图像预处理,假设训练集有M幅图像,将图像标准化为patch,统一格式和规格;
    (b)特征提取,假设M幅图像,对每一幅图像提取特征,共提取出N个SIFT特征;
    (c)特征聚类,采用K-means算法把N个对象分为K个簇 (视觉单词表),使簇内具有较高的相似度,而簇间相似度较低;

    d45f394f424c8e78e66d01bd1a06525d.png

    (d)统计得到图像的码本,每幅图像以单词表为规范对该幅图像的每一个SIFT特征点计算它与单词表中每个单词的距 离,最近的加1,便得到该幅图像的码本;还需要码本矢量归一化,因为每一幅图像的SIFT特征个数不定,所以需要归一化。

    6367aa76e41ad10ea2533664fb56a1df.png

    相似度计算(算法模块)

    现在有了字典,但还有一点需要注意。我们利用单词表示图像,目的是发现回环,也就是两帧图像具有较高的相似性。那其实不同的单词对这一目的的贡献性是不同的。例如,我这篇文章中有很多“我们”这个词,但这个词并不能告诉我们太多信息。

    而有些单词例如“回环”、“K-means”就比较具有代表性,能大概告诉我们这句话讲了什么。因此不同的单词应该具有不同的权重。

    (a)贝叶斯估计方法。采用BoW描述机器人每一位置的场景图像,估计已获取图像与对应位置的先验概率,对当前时刻计算该新场景图像与已访问位置匹配的后验概率,概率大于阈值则标记为闭环。

    (b)相似性方法。有了字典以后,给定任意特征点f_i,只要在字典树中逐层查找,最后都能找到与之对应的单词w_i。通常字典足够大,可以说它们来自同一类物体。但是这种方法对所有单词都是同样对待,常规的做法是采用TF-IDF(term frequency-inverse document frequency)

    IDF(Inverse Document Frequency):描述单词在字典中出现的频率(构建字典时),越低越具有代表性

    9dc2e29a5f598229e8de09a5d8b0c1b0.png

    n为所有描述子数, n_i为该单词出现次数。ln的作用大概是降低量级,毕竟n很大。

    TF(Term Frequency):单词在单帧图像中出现的频率,越高越具有代表性

    8eccc8cf73450956a95ebacb8ec05934.png

    n为一帧图像中所有单词数, n_i为一帧图像中该单词出现次数。

    因此将一帧图像转化为单词表示时,我们要计算其单词的权重:

    2b469753d20a5f811d2c83bb2aac411a.png

    因此一帧图像A由单词 w_i,次数n_i以及权重η_i组成。

    验证模块(回环处理)

    回环的判断也并没有这么简单,含有很多的筛选环节,毕竟错误的回环将带来巨大灾难,宁可不要。例如某一位姿附近连续多次(ORB-SLAM中为3次)与历史中某一位姿附近出现回环才判断为回环;回环候选帧仍然要匹配,匹配点足够才为回环。方式主要有两种:

    (a)时间一致性。正确的回环往往存在时间上的连续性,所以如果之后一段时间内能用同样的方法找到回环,则认为当前回环是正确的,也叫做顺序一致性约束。
    (b)结构一致性校验。对回环检测到的两帧进行特征匹配并估计相机运动,因为各个特征点在空间中的位置是唯一不变的,与之前的估计误差比较大小。

    然后下面主要就会涉及如何对找到的回环进行优化了。

    9706b6450f9e037ff2906b2de812ca38.png

    71fd1a40130f40cd2520551a552f360a.png

    2. SLAM优化

    闭环纠正

    在判断出现回环后,两帧计算Sim3变换(因为有尺度漂移),也就是从历史帧直接推算当前位姿。下面我们以ORB-SLAM2为代表减少形成闭环时的Sim3优化。

    单目SLAM一般都会发生尺度(scale)漂移,因此Sim3上的优化是必要的。相对于SE3,Sim3的自由度要多一个,而且优化的目标是矫正尺度因子,因此优化并没有加入更多的变量。

    //BRIEF 形成闭环时进行Sim3优化//Sim3用于回环检测时计算关键帧与回环帧之间的关系 R 、t、 s//相似变换群Sim3int Optimizer::OptimizeSim3(KeyFrame *pKF1, //关键帧1                            KeyFrame *pKF2, //关键帧2                            vector<MapPoint *> &vpMatches1,//两帧匹配关系                             g2o::Sim3 &g2oS12, //两个关键帧间的Sim3变换                            const float th2, //核函数阈值                            const bool bFixScale//是否进行尺度优化                            ){    //创建优化器    g2o::SparseOptimizer optimizer;    g2o::BlockSolverX::LinearSolverType * linearSolver;    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);    //使用LM算法    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);    optimizer.setAlgorithm(solver);    // Calibration    //得到两帧相机内参    const cv::Mat &K1 = pKF1->mK;    const cv::Mat &K2 = pKF2->mK;    // Camera poses    //得到两帧的旋转与平移位姿    const cv::Mat R1w = pKF1->GetRotation();    const cv::Mat t1w = pKF1->GetTranslation();    const cv::Mat R2w = pKF2->GetRotation();    const cv::Mat t2w = pKF2->GetTranslation();    // Set Sim3 vertex    g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();        vSim3->_fix_scale=bFixScale;    //两帧之间的位姿变化为待估计量    vSim3->setEstimate(g2oS12);    vSim3->setId(0);    //不进行固定参数    vSim3->setFixed(false);    //提取相机内参参数    vSim3->_principle_point1[0] = K1.at<float>(0,2);    vSim3->_principle_point1[1] = K1.at<float>(1,2);    vSim3->_focal_length1[0] = K1.at<float>(0,0);    vSim3->_focal_length1[1] = K1.at<float>(1,1);    vSim3->_principle_point2[0] = K2.at<float>(0,2);    vSim3->_principle_point2[1] = K2.at<float>(1,2);    vSim3->_focal_length2[0] = K2.at<float>(0,0);    vSim3->_focal_length2[1] = K2.at<float>(1,1);    //加入图优化顶点    optimizer.addVertex(vSim3);    // Set MapPoint vertices    //建立地图点 顶点    const int N = vpMatches1.size();    const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();    //定义两帧的公共地图点    vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;    vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;    //定义边的尺寸    vector<size_t> vnIndexEdge;    //分配空间    vnIndexEdge.reserve(2*N);    vpEdges12.reserve(2*N);    vpEdges21.reserve(2*N);    const float deltaHuber = sqrt(th2);    int nCorrespondences = 0;    //遍历所有匹配点    for(int i=0; i<N; i++)    {        if(!vpMatches1[i])            continue;        MapPoint* pMP1 = vpMapPoints1[i];        MapPoint* pMP2 = vpMatches1[i];        const int id1 = 2*i+1;        const int id2 = 2*(i+1);        const int i2 = pMP2->GetIndexInKeyFrame(pKF2);        //如果匹配的两帧看到的地图点都存在        if(pMP1 && pMP2)        {            //判断是不是好点            if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)            {                //建立优化器 优化位姿1                g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();                //提取世界坐标转化相机坐标                cv::Mat P3D1w = pMP1->GetWorldPos();                cv::Mat P3D1c = R1w*P3D1w + t1w;                //待优化为相机位姿                vPoint1->setEstimate(Converter::toVector3d(P3D1c));                vPoint1->setId(id1);                //固定原点坐标                vPoint1->setFixed(true);                optimizer.addVertex(vPoint1);                //优化位姿2                g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();                cv::Mat P3D2w = pMP2->GetWorldPos();                cv::Mat P3D2c = R2w*P3D2w + t2w;                vPoint2->setEstimate(Converter::toVector3d(P3D2c));                vPoint2->setId(id2);                vPoint2->setFixed(true);                optimizer.addVertex(vPoint2);            }            else                continue;        }        else            continue;        //成功一个点加一个        nCorrespondences++;        // Set edge x1 = S12*X2        //建立边,就是计算重投影误差        //提取像素坐标        Eigen::Matrix<double,2,1> obs1;        const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];        obs1 << kpUn1.pt.x, kpUn1.pt.y;        //建立边,也就是误差        g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();        //连接的两个顶点        e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));        e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));        //观测值        e12->setMeasurement(obs1);        //信息矩阵        const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];        e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);        //鲁棒核        g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;        e12->setRobustKernel(rk1);        rk1->setDelta(deltaHuber);        //添加边        optimizer.addEdge(e12);        // Set edge x2 = S21*X1        //反向投影在进行优化        Eigen::Matrix<double,2,1> obs2;        const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];        obs2 << kpUn2.pt.x, kpUn2.pt.y;        g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();        e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));        e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));        e21->setMeasurement(obs2);        float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];        e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);        g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;        e21->setRobustKernel(rk2);        rk2->setDelta(deltaHuber);        optimizer.addEdge(e21);        vpEdges12.push_back(e12);        vpEdges21.push_back(e21);        vnIndexEdge.push_back(i);    }    // Optimize!    //开始优化    optimizer.initializeOptimization();    optimizer.optimize(5);    // Check inliers    //清除外点和误差过大的点    int nBad=0;    for(size_t i=0; i<vpEdges12.size();i++)    {        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];        if(!e12 || !e21)            continue;        if(e12->chi2()>th2 || e21->chi2()>th2)        {            size_t idx = vnIndexEdge[i];            vpMatches1[idx]=static_cast<MapPoint*>(NULL);            optimizer.removeEdge(e12);            optimizer.removeEdge(e21);            vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);            vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);            nBad++;        }    }    //确定迭代次数    int nMoreIterations;    if(nBad>0)        nMoreIterations=10;    else        nMoreIterations=5;    if(nCorrespondences-nBad<10)        return 0;    // Optimize again only with inliers    //再次优化,只优化内点    optimizer.initializeOptimization();    optimizer.optimize(nMoreIterations);    int nIn = 0;    for(size_t i=0; i<vpEdges12.size();i++)    {        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];        if(!e12 || !e21)            continue;        if(e12->chi2()>th2 || e21->chi2()>th2)        {            size_t idx = vnIndexEdge[i];            vpMatches1[idx]=static_cast<MapPoint*>(NULL);        }        else            nIn++;    }    // Recover optimized Sim3    //恢复出位姿变化关系    g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));    g2oS12= vSim3_recov->estimate();    return nIn;}

    当我们用第m帧推算了回环帧n的位姿时,使得n的位姿漂移误差较小,但其实同时可以用第n帧来计算n-1帧的位姿,使n-1帧的位姿漂移误差也减小。因此,这里还要有一个位姿传播。

    另外我们可以优化所有的位姿,也就是进行一个位姿图优化(由位姿变换构建位姿约束)。

    最后,我们还可以进行一起全局所有变量的BA优化。

    //  初始化g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >* linearSolver     = new g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >();g2o::BlockSolver_6_3* blockSolver = new g2o::BlockSolver_6_3(linearSolver);g2o::OptimizationAlgorithmLevenberg* solver= new g2o::OptimizationAlgorithmLevenberg( blockSolver );    optimizer.setAlgorithm( solver );optimizer.setVerbose( false );// 初始顶点    g2o::VertexSE3* v = new g2o::VertexSE3();   v->setId( cutIndex );   v->setEstimate( Eigen::Isometry3d::Identity() );    v->setFixed( true );    optimizer.addVertex( v )// 添加新的节点(顶点)g2o::VertexSE3* vNext = new g2o::VertexSE3();vNext->setId(currFrame.frameID);vNext->setEstimate( Eigen::Isometry3d::Identity());optimizer.addVertex(vNext);// 添加边的数据g2o::EdgeSE3* edgeNew = new g2o::EdgeSE3();edgeNew->setVertex(0, optimizer.vertex(pousFrame.frameID));edgeNew->setVertex(1, optimizer.vertex(currFrame.frameID));edgeNew->setRobustKernel( new g2o::RobustKernelHuber() );//edgeNew->setInformation( Eigen::Matrix2d::Identity());  //the size is worthy to research// 信息矩阵// 两个节点的转换矩阵Eigen::Isometry3d T = cvMat2Eigen( m_result.rotaMatrix, m_result.tranMatrix );    edgeNew->setMeasurement(T);optimizer.addEdge(edgeNew);// 开始优化optimizer.initializeOptimization();optimizer.optimize(10);

    09fc4e18263989aaf566ab2515989dcd.png

    54bfde39c4d19a0a63a9d10f10b48d5c.png

    08b715492b8a96873358f246b614aafc.png

    3. 参考链接

    https://baijiahao.baidu.com/s?id=1627227355343777871&wfr=spider&for=pc
    https://zhuanlan.zhihu.com/p/45573552
    https://blog.csdn.net/stihy/article/details/62219842

    本文仅做学术分享,如有侵权,请联系删文。

    3D视觉精品课程推荐:

    1.面向自动驾驶领域的多传感器数据融合技术

    2.面向自动驾驶领域的3D点云目标检测全栈学习路线!(单模态+多模态/数据+代码)
    3.彻底搞透视觉三维重建:原理剖析、代码讲解、及优化改进
    4.国内首个面向工业级实战的点云处理课程
    5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
    6.彻底搞懂视觉-惯性SLAM:基于VINS-Fusion正式开课啦
    7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
    8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

    9.从零搭建一套结构光3D重建系统[理论+源码+实践]

    10.单目深度估计方法:算法梳理与代码实现

    重磅!3DCVer-学术论文写作投稿 交流群已成立

    扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

    同时也可申请加入我们的细分方向交流群,目前主要有3D视觉CV&深度学习SLAM三维重建点云后处理自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

    一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。

    9339d6e9c780053c49a43341904c6c2d.png

    ▲长按加微信群或投稿

    82d1b8ae344858cd7f1d6bf8d7e6163e.png

    ▲长按关注公众号

    3D视觉从入门到精通知识星球:针对3D视觉领域的视频课程(三维重建系列三维点云系列结构光系列手眼标定相机标定激光/视觉SLAM自动驾驶等)、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近4000星球成员为创造更好的AI世界共同进步,知识星球入口:

    学习3D视觉核心技术,扫描查看介绍,3天内无条件退款

    5ac99839d08aeac1a057c3d12ecc3da3.png

     圈里有高质量教程资料、答疑解惑、助你高效解决问题

    觉得有用,麻烦给个赞和在看~  

    展开全文
  • 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

    展开全文
  • ORB_SLAM2回环检测

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

    万次阅读 2019-11-06 08:33:21
    视觉SLAM笔记(48) 局部地图 1. 迭代优化的问题 2. 特征点引入地图 2. 地图 3. 地图点类 3. 视觉里程类 3.1. 关键帧 3.2. 优化地图 3.3. 特征匹配 4. 结果对比 5. 缺点 1. 迭代优化的问题 引入迭代优化方法...
  • Orb-SLAM回环检测的代码思路

    千次阅读 2018-09-07 21:59:46
    DetectLoop() (检测回环): LoopClosing.cc中DetectLoop()函数 (1)首先从队列中取出一个关键帧,判断:如果距离上次闭环没多久(小于10帧),或者map中关键帧总共还没有10帧.如果是的话,则不进行闭环检测。在...
  • 《基于 2D 激光雷达和实时回环优化的 SLAM 算法》,此论文在 SLAM 的背景下,使用了一些机器学习的方法(最小二乘、损失函数等),很有学习的价值。(精译版;翻译不易,转载请注明出处;翻译过程加入了个人理解,如...
  • 视觉slam中的回环检测概述

    千次阅读 2020-07-23 20:09:50
    slam中,前端负责提供轨迹与地图的初值,后端负责对轨迹与地图进行优化。...拿视觉slam来说,虽然对于当前位姿的估计我会使用ba进行优化,但是这个ba一般是基于局部地图的优化,首先必然会存在误差(ba只
  • SLAM学习——回环检测

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

    千次阅读 2018-11-02 14:42:37
    请求局部地图线程停止,并且中止现有的全局优化进程 // Send a stop signal to Local Mapping // Avoid new keyframes are inserted while correcting the loop mpLocalMapper-&gt;RequestStop(); // If a ...
  • ​综述 | SLAM回环检测方法

    千次阅读 2019-08-27 19:30:50
    本文作者任旭倩,公众号:计算机视觉life成员,由于格式原因,公式显示可能出问题,建议阅读原文链接:综述 | SLAM回环检测方法在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一帧位姿解算当前帧位姿,...
  • SLAM(一)-回环检测

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

    千次阅读 2019-08-25 16:35:31
    研究背景: 后端优化原理大多人都比较清楚,但是实现起来不是个容易的事情,而回环检测部分在自己测试之后发现,这部分在vSLAM中也是至关重要的,本文的最后给出了在没有好的回环下得到的地图数据。 1. 问题描述 ...
  • 浅谈SLAM回环检测技术

    千次阅读 2018-10-23 16:25:29
    在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一帧位姿解算当前帧位姿,因此其中的误差便这样一帧一帧的传递下去,也就是我们所说的累计误差。 我们的位姿约束都是与上一帧建立的,第五帧的位姿误差中...
  • 回环检测(SLAM十四讲ch12)

    千次阅读 2018-09-03 17:14:02
    除了利用优化方法在局部和全局调整位姿,也可以利用回环检测(loop closure)来优化位姿。 问题的关键是:如何判断两帧图片的相似度。最直观的做法是:特征匹配,比较匹配的数量是否足够多。此时,可以采用词袋模型...
  • 再看一下启用后端,但是不启用回环的结果 放大一下看看约束的效果 再看看启用后端优化与回环检测的结果 放大一下看看约束的效果 多了一条约束有没有,这就是回环约束,优化之后的图很整齐. 最终的建图效果是这样的 ...
  • ORB-Slam2详解6 回环

    千次阅读 2017-12-04 14:33:58
    毋庸置疑的是,随着相机的运动,我们计算的相机位姿,三角化得到的点云位置,都是有误差的,即使我们使用局部的或全局的BA去优化,仍然会存在累积误差。而消除误差最有效的办法是发现闭环,并根据闭环对所有结果进行...
  • 下面着重考虑局部地图 local mapping的VO,被称为——地图VO 这里的局部地图和slam的建图不一样,暂时还没有考虑全局地图。这里只是局部地图,简单的把一些比较近的、视野内的特征点缓存到一个地方,是一个小地图。...
  • ORB-SLAM(六)回环检测

    千次阅读 2017-11-02 00:11:02
    除了利用优化方法在局部和全局调整位姿,也可以利用回环检测(loop closure)来优化位姿。 这件事情就好比一个人走在陌生的城市里,一开始还能分清东南西北,但随着在小街小巷转来转去,已经不知道自己在什么地方了...
  • 点击上方“AI算法修炼营”,选择加星标或“置顶”标题以下,全是干货什么是SLAM?同时定位与地图构建 (simultaneous localization and mapping, SL...
  • 1.回环检测初始化,给变量赋初值 //回环检测初始化接口 mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR); //回环检测初始化,变量赋初值 LoopClosing::LoopClosing...
  • 回环检测(Loop Closure Detection),视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一帧位姿解算当前帧位姿,因此其中的误差便这样一帧一帧的传递下去,也就是我们所说的累计误差。我们之所以用前一帧递...
  • ORB_SLAM2--检查是否产生了回环

    千次阅读 2018-11-02 09:17:46
    提取关键帧前锁住回环检测队列,防止被修改,然后使用队列中的头帧作为当前的关键帧。设置当前关键帧不可被擦除。 { unique_lock&lt;mutex&gt; lock(mMutexLoopQueue); mpCurrentKF = ...
  • 视觉SLAM笔记(57) 回环检测

    万次阅读 2019-11-17 10:43:58
    回环检测的意义、实现方法、准确率和召回率、词袋模型
  • 基础知识1.1 准确率和召回率1.2 词袋模型1.3 字典1.4 字典的数据结构1.5 相似度的计算1.6 相似度评分的处理1.7 检测回环后的验证2. 实践与代码解析2.1 创建字典 回环检测的关键是,如何有效地检测出相机经过同一个...
  • 视觉 SLAM 十四讲 —— 第十二讲 回环检测 回环检测概述 回环检测的意义 前面我们已经介绍过了:前端提供特征点的提取和轨迹、地图的初值,而后端负责对这所有的数据进行优化。然而,如果像 VO 那样仅考虑相邻...
  • ORBSLAM2回环检测之几何验证简介  回环检测的目的是找到当前场景在历史中是否出现过,如果出现过,那会给我们提供一个非常强的约束条件,把我们偏离很多的轨迹一下子修正到正确的位置上。当然,这么好的东西,有利...
  • 1.DetectLoopCandidates(KeyFrame* pKF, float minScore) @param pKF 需要闭环的关键帧 @param minScore 相似性分数最低要求 ... //spConnectedKeyFrames与pKF局部相连的关键帧 set<KeyFrame*> spConnecte...
  • 在目前实际的视觉SLAM中,闭环检测多采用DBOW2模型https://github.com/dorian3d/DBoW2,而bagofwords又运用了数据挖掘的K-means聚类算法,笔者只通过bagofwords模型用在图像处理中进行形象讲解,并没有涉及太多对...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,448
精华内容 579
关键字:

局部回环slam