VSLAM(9)：回环检测

聪明的笨小子

已于 2023-08-01 02:28:41 修改

阅读量1.1k

点赞数 1

分类专栏：视觉SLAM14讲文章标签：计算机视觉自动驾驶 c++ 人工智能

于 2023-07-28 02:06:10 首次发布

本文链接：https://blog.csdn.net/zhangzhixin1998/article/details/131929764

版权

视觉SLAM14讲专栏收录该内容

10 篇文章 2 订阅

订阅专栏

文章详细介绍了回环检测在视觉SLAM系统中的重要性，包括其定义、方法、准确率和召回率的衡量标准。回环检测通过特征匹配和图像相似度计算来识别相机是否重复经过同一地点，以纠正累计误差并提供重定位功能。词袋模型和K-means聚类用于生成字典，进行图像特征的表示和比较。在实际应用中，还需要考虑回环验证以确保检测的准确性。

摘要由CSDN通过智能技术生成

根据前面的介绍，视觉SLAM前端主要是视觉里程计，负责提取特征点和轨迹，地图的初值；后端负责对数据进行优化。视觉里程计会计算前后帧之间的相对运动，这样会导致前面计算的误差会累加到下一时刻，使得SLAM出现不断增加的累计误差，无法构建全局一致的轨迹和地图。

一. 回环检测

1.1 回环检测的定义

如下图所示，当前端只给出了相邻帧间的估计，由于当前帧的位姿由上一帧的位姿加上相邻帧的相对位姿决定，所以上一帧位姿的误差就延续到了当前帧。以此类推，误差就会越累计越大，使得结果跟真实轨迹越差越远，满满趋向于不稳定。

后端虽然可以在一定程度上缓解误差问题，但是只有相邻帧的时候，对于累计误差也无法消除。这时候就要引入回环检测，来给出除了相邻帧外的，一些时隔更久远的约束。回环检测的目的就是识别相机是否经过同一个地方，采集到了相似的数据。如果成功检测，可以为后端提供额 $O(N^2)$ 外的约束信息，使得最终的估计是一个全局一致（Global Consistent）的估计。而回环检测的关键就是如何有效的检测出相机经过同一个地方这个事。

回环检测除了为后端优化提供额外的约束条件以外，还为SLAM提供了重定位的功能。当跟踪算法失效，例如特征点丢失，这时候的就需要使用回环检测来重新定位机器人现在的位置。一般定义的SLAM问题是包括前端和局部后端优化以及回环检测的算法框架，而只有前两者的系统称为视觉里程计，即VO。

1.2 回环检测方法

对于回环检测的实现方法，最简单的就是将当前的帧的图像特征点与之前的所有的图像特征点做一遍特征匹配[1]，根据匹配得分来确定是否存在回环。但是这样的问题就是随着机器人的运动，过去的图像会越来越多，盲目的假设当前帧与之前的所有帧都可能存在匹配关系会导致检测数量不断增加，算法的复杂度就是 $O(N^2)$ ，这样对于大部分的场景是不适用的。另一个简单的办法就是，随机抽取历史帧进行回环检测，例如抽取历史中的五帧与当前帧进行对比[2]，这样算法的复杂度就是一个常数。但是这样的问题就是随着轨迹不断增多，如果真的存在回环，那么抽到回环的概率会随着帧数的增长而不断减小，使得检测效率不高。（如果抽取的帧数随着帧数增大而不断增大会不会好好一点？）

另一种回环检测的方法是基于外观的，仅根据两张图像的相似度来判断是否存在回环。这种方法摆脱了累计误差，使得回环检测成为SLAM系统中一个相对独立的模块。基于外观的回环检测方法再参考文献[3]-[5]中有提到。基于外观回环检测核心问题就是如何计算图像之间的相似度： $s(A,B)$ ，当这个值大于某一个阈值的时候，就可以认为出现了回环。对于计算图像相似度最简单的方法就是让两个图像相减，然后取某种范数来表示相似度，例如 $s(A,B) = ||A-B||$ ，但是这种方法会受到光照，相机曝光等的影响，同样当相机的视角发生改变时，即使光度不变，像素也会存在位移，导致最终匹配结果不佳。

对于相似关系函数的好坏，可以从感知偏差（Perceptual Aliasing）和感知变异（Perceptual Variablity）两个角度来讨论。

1.3 准确率和召回率

回环检测会根据检测的情况分为真阳，假阳，假阴和真阴四种情况，如下表所示：

假阳性（True Positive）又称为感知偏差，而假阴性（False Negative）称为感知变异。一个回环检测算法的优略，取决于它TP和TN的概率。所以对于特定的算法，可以在数据集中测试他的TP，TN，FP，FN的出现次数，并计算两个统计量：准确率和召回率（Percision&Recall）。

准确率描述的是回环检测算法中，检测为回环并且真是回环的概率，而召回率则是所有正确检测的回环中，回环被正确检测出的概率。通常来说，这两个量存在矛盾性。

当我们把回环检测的阈值增大时，算法会变得更严格，这样的准确率会提高，但是同时检测的数量会减少，许多原本是回环的地方就被漏掉了，导致召回率降低。反之，我们减小阈值，检测出的回环数量会增多，但是准确率会降低。所以一个好的回环检测算法应该是综合考虑这两个指标，做出的Precision-Recall曲线，召回率是横轴，准确率是纵轴，回环检测问题会倾向于让曲线整体偏向右上方的程度，百分百准确率下的召回率，或则50%召回率下的准确率作为算法的评价指标。通常情况下，算法的好坏不能一概而论，。比如A在准确率高的时候有较高的召回率，而B在70%召回率的情况下还能保证较好的准确率。

在SLAM问题中，我么更倾向于关注准确率，而对召回率相对宽容一些。因为准确率低的时候，在后端Pose Graph中会引入错误的约束，导致整个优化算法效果变差甚至不收敛得到完全错误的结果。而相比之下，召回率低的时候，最多是存在回环的地方没有被检测到，地图可能受 $w_1,w_2,w_3$ 到一些累计误差的影响——然而仅需要一两次回环就可以消除了。所以一般在SLAM问题中倾向于把阈值设置的更严格一些，或则在检测后再添加回环验证的步骤。

1.4 词袋模型

回到相似度计算的问题中，直接使用图像灰度信息存在光照，相机角度等问题。那么可以考虑使用特征点来做回环检测，对两个图像的特征点进行匹配，只要匹配数量大于一定值，就认为存在回环。而且，根据特征匹配的结果，还可以计算两个图像之间的运动关系。这种做法同样存在缺点，如特征计算本身就比较费时；当光照条件改变时，特征描述可能不稳定。

词袋（Bag-of-Word）模型是一种描述图像中信息的方法，它描述图像中有哪几种特征。例如图像中有一个人和一辆车；或则另一个图像中有一只狗和一个人。根据图像中特征的类型和数量，就可以用来计算两个图像的相似度。具体的做法就是：

确定BoW的单词（word）种类，比如上面提到的人，狗等，就算一个类型的单词，许多单词组合在一起，就是一个字典；
确定每一张图像中的单词，用这些单词及它出现的次数来描述整个图像。即将一个图像转化为一个向量；
比较不同图像之间的相似度。

举例来说明，加入上面的提到的”人“， ”狗“ 和”车“构成了一本”字典“，分别记作 $w_1, w_2, w_3$ 。那么对于一个由人和狗组成的图像A就可以记作向量： $A = 1*w_1 + 1*w_2 + 0* w_3$ 或则更简洁的形式 $[1,1,0] ^T$ 。对于图像中出现某个特征多次，例如出现了一个人和两辆车，这时候的描述向量就可以是 $[1,0,2]^T$ 。这时候就把一张图像根据它含有哪些特征定义成了一个向量，比单纯的比较灰度更加稳定。

但是描述向量表达的是某个特征”是否出现“，而不关心”在哪里出现“，所以物体的空间位置与排列顺序无关。相机发生少量运动时，只要物体还在视野中，描述向量就是不变的。

接下来要考虑的就是如何根据描述向量计算不同图像之间的相似度。对于相似度的计算，有不同的设计方法，例如可以定义如下的相似度得分：

式中取了描述向量a和b相减后的 $L_1$ 范数，取a-b的所有元素绝对值之和。当两个向量完全相同时，会得到1；反之完全相反则是0。

1.5 字典

字典由单词组成，而单词代表了一个概念，在图像中是一类特征的组合，所以字典的生成需要将特征点根据特征类型分为多类，这就是一个聚类（Clustering）问题。聚类问题是一个无监督机器学习（Unsupervised ML）问题，本质上让计算机自己寻找规律，来对数据进行分类。经典的聚类算法有K-means[92]，K-means++[93]，DBSCAN，GMM等。

14讲书中使用的是K-means来进行聚类，假如我们有N个数据，想要归成k个类，就可以按照下面的步骤来进行：

随机选取k个中心点： $c_1, ..., c_k$ ;
对每一个样本，计算与每个中心点之间的距离，取最小的作为它的归类；
重新计算中心点；
如果每个中心点都变化很小，则算法收敛，退出；否则返回2。

这样，就可以把特征点聚类成一个含有k个单词的字典了。接下来考虑的就是如何根据图像的特征点，查找字典中对应的单词？如果是一个一个的对比，算法复杂度就是 $O(n)$ ，随着单词的增加，算法使用的会线性增加。所以可以考虑在建字典的时候就对字典排序，这样搜寻的时候就变成了二分查找，算法复杂度变成了 $O(logn)$ 。排序过的字典本质上就树结构，在实践中，可以使用更复杂的结构来加速算法，例如[94][95][96]中的Chou-Liu tree[97]。SLAM书中讲述的k叉树[98]，如下图所示。

K叉树的原理类似于层次聚类，是k-means的直接扩展，它会把数据构建成一个深度为d，每次分叉为k的树，具体做法如下：

在根节点，用k-means把所有的样本聚成k类，构成树结构的第一层；
对第一层的每个节点，把每个属于该节点再聚类成k类，得到下一层；
依次类推，最后得到叶子层。每个子叶代表一个单词。

这样就生成了一个有 $K^d$ 个叶节点的树，每个叶子层构建了单词，而树结构中的中间节点仅供快速查找时使用。在查找某个给定特征对应的单词时，只需将它与每个中间节点的聚类中心对比，一共对比d次，即可找到对应的单词。

一般在前端部分会使用到ORB特征，所以SLAM书中给出了建立ORB字典的具体代码实践。代码需要使用一个开源BoW库，使用的是DBoW3。使用起来只需要调用DBoW3的字典生成接口，把ORB特征放在一个vector中。代码如下：

#include "DBoW3/DBoW3.h"
#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;

/***************************************************
 * 本节演示了如何根据data/目录下的十张图训练字典
 * ************************************************/

int main( int argc, char** argv )
{
    // read the image 
    cout<<"reading images... "<<endl;
    vector<Mat> images; 
    for ( int i=0; i<10; i++ )
    {
        string path = "./data/"+to_string(i+1)+".png";
        images.push_back( imread(path) );
    }
    // detect ORB features
    cout<<"detecting ORB features ... "<<endl;
    Ptr< Feature2D > detector = ORB::create();
    vector<Mat> descriptors;
    for ( Mat& image:images )
    {
        vector<KeyPoint> keypoints; 
        Mat descriptor;
        detector->detectAndCompute( 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;
}

1.6 相似度计算

在建立字典后，对图像中提取出的某一个特征点 $f_j$ ，都可以根据字典查找一个与之对应的单词 $w_j$ 对一张图像的所有N个特征点进行查找操作后，就可以得到一个该图像在单词列表中的分布。

但是这样的做的问题是，字典中可能存在一些重复性很高的特征点，这样的特征点是辨识度很低的单词；同样也会存在一些出现次数很少的特征点，这样的特征点的辨识度高些。所以可以对每个单词添加一个权重，辨识度越高，权重越大。

书中介绍的是TF-IDF（Term Frequency-Inverse Document Frenquency）[100] [101]。TF认为当一个单词在某一图像中出现的频率越高时，这个单词的区分度就越高。而IDF认为，某单词在单词中出现的频率越低，他的区分度越高。

对于IDF部分，可以在建立词袋模型时定义，统计每个叶子节点中特征点的个数与所有特征数量的比例，来作为IDF部分。假设所有特征数量为n，某个叶子节点的数量为 $n_i$ ，那么这个叶子节点的IDF为：

而对于TF部分，他统计某个单词在单个图像中出现的频率。比如单词 $w_i$ 在图像A中出现了 $n_i$ 次。而图像A一共有n个单词，那么TF的定义为：

于是单词 $w_i$ 在图像A中对应的权重就等于TF跟IDF的乘积：

那么这是图像A对应的词袋向量就是：

在实际情况中，由于相似的特征点回落在同一个类中，这里的 $v_A$ 中会存在大量的0，是一个稀疏向量。向量中的非零部分指示出图像含有哪些单词，这部分的值则是反应了权重值。对于两个图像之间的相似度计算定义，可以采用某种范数，书中给出了一种基于 $L_1$ 范数的误差定义[102]：

下面就是代码实践部分，使用上面生成的字典，生成BoW并比较差异。

#include "DBoW3/DBoW3.h"
#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 images and database  
    cout<<"reading database"<<endl;
    DBoW3::Vocabulary vocab("./vocabulary.yml.gz");
    // DBoW3::Vocabulary vocab("./vocab_larger.yml.gz");  // use large vocab if you want: 
    if ( vocab.empty() )
    {
        cerr<<"Vocabulary does not exist."<<endl;
        return 1;
    }
    cout<<"reading images... "<<endl;
    vector<Mat> images; 
    for ( int i=0; i<10; i++ )
    {
        string path = "./data/"+to_string(i+1)+".png";
        images.push_back( imread(path) );
    }
    
    // NOTE: in this case we are comparing images with a vocabulary generated by themselves, this may leed to overfitting.  
    // detect ORB features
    cout<<"detecting ORB features ... "<<endl;
    Ptr< Feature2D > detector = ORB::create();
    vector<Mat> descriptors;
    for ( Mat& image:images )
    {
        vector<KeyPoint> keypoints; 
        Mat descriptor;
        detector->detectAndCompute( image, Mat(), keypoints, descriptor );
        descriptors.push_back( descriptor );
    }
    
    // we can compare the images directly or we can compare one image to a database 
    // images :
    cout<<"comparing images with images "<<endl;
    for ( int i=0; i<images.size(); i++ )
    {
        DBoW3::BowVector v1;
        vocab.transform( descriptors[i], v1 );
        for ( int j=i; j<images.size(); j++ )
        {
            DBoW3::BowVector v2;
            vocab.transform( descriptors[j], v2 );
            double score = vocab.score(v1, v2);
            cout<<"image "<<i<<" vs image "<<j<<" : "<<score<<endl;
        }
        cout<<endl;
    }
    
    // or with database 
    cout<<"comparing images with database "<<endl;
    DBoW3::Database db( vocab, false, 0);
    for ( int i=0; i<descriptors.size(); i++ )
        db.add(descriptors[i]);
    cout<<"database info: "<<db<<endl;
    for ( int i=0; i<descriptors.size(); i++ )
    {
        DBoW3::QueryResults ret;
        db.query( descriptors[i], ret, 4);      // max result=4
        cout<<"searching for image "<<i<<" returns "<<ret<<endl<<endl;
    }
    cout<<"done."<<endl;
}

在进行相似度计算时，往往会遇到这样的情况：有些环境外观很相似，有些环境外观就差很多。这样的话，盲目计算相似度得到得分会有很大的差别，所以最好对相似度得分进行一次归一化处理。具体的做法是，计算某时刻关键帧图像和上一关键帧的相似度作为一个先验相似度 $s(v_t,v_t -\Delta t)$ ，然后其他的分值按照这个值去进行归一化：

这样，就可以认为，当当前帧与之前某个关键帧相似度超过当前帧与上一关键帧的3倍的时候，就认为可能存在回环。这样设置的相似度计算方式会避免引入绝对的相似度阈值，使得回环检测算法能够适应更多的环境。

上面说到，拿当前关键帧与上一时间点的关键比值作为判断条件，会使得算法的泛化性更好。所以关键帧的提取也是需要考虑的。如果关键帧选取的太近，会导致两个关键帧之间的相似度过高，对于历史中的相似帧检测难度就会变高。所以用于回环检测的帧需要具有一定的稀疏性，保证彼此之间不同，同时又能覆盖整个环境。

根据计算相似度检测到可能存在回环后，还需要进一步判断是否真的遇到了回环。因为有时候会存在外观高度相似的场景，这种情况下只依赖外观类型的词袋模型很容易出现感知偏差。所以在回环检测后，还需要进一步进行验证[80] [103]。验证的方法有设立回环的缓存机制，默认只有检测到一段时间内一直存在回环的可能才是正确的回环。这种方法可能看成是时间上的一致；另外一个办法是根据两帧特征点的运动是否与之前pose graph推导出的结果之间的差异来判断是否是真的回环，可以认为是是空间上的一致。

聪明的笨小子

关注

1
点赞
踩
6

收藏

觉得还不错? 一键收藏
1
评论
VSLAM(9)：回环检测

如下图所示，当前端只给出了相邻帧间的估计，由于当前帧的位姿由上一帧的位姿加上相邻帧的相对位姿决定，所以上一帧位姿的误差就延续到了当前帧。以此类推，误差就会越累计越大，使得结果跟真实轨迹越差越远，满满趋向于不稳定。后端虽然可以在一定程度上缓解误差问题，但是只有相邻帧的时候，对于累计误差也无法消除。这时候就要引入回环检测，来给出除了相邻帧外的，一些时隔更久远的约束。回环检测的目的就是识别相机是否经过同一个地方，采集到了相似的数据。如果成功检测，可以为后端提供额外的约束信息，使得最终的估计是一个。
复制链接

扫一扫

专栏目录