回环检测

一、回环检测的意义

SLAM系统有了前端的视觉里程计，有了后端优化，似乎已经比较好用了。但事情还是没有我们想象的那么简单，由于上一篇文章我们刚刚把后端优化做了一次精简，在提高实时性的同时降低了精度。一旦精度降低，又会面临长时间累计误差的问题，特别是像ORB-SLAM那样只做局部地图优化的方案。我们怎样能够平衡这个矛盾呢，有没有更好的解决方案，这是本文将要讨论的问题。

回环检测为解决以上问题提供了很好的思路。我们不妨思考一下人是如何建立环境地图的。在局部区域，人不断移动从而在脑海中建造增量式地图，但时间长了人也会分不清现在到底朝向哪边，与起始点的关系如何。假如人恰好在某一时刻回到了之前路过的位置，如果这个人对环境足够敏感，他就能发现这一事实，从而修正自己之前对方位的判断。我们说，此时检测到了一个回环。显然，人是通过看面前的景物并与脑海中残存的印象比对从而检测到回环的。对于SLAM来说，也可以这样做，通过比对当前帧与过去的关键帧，相似度超过某一阈值时就可以认为检测到回环。

现在，问题的关键就在于如何判断两帧图片的相似度。最直观的做法是特征匹配，比较匹配的数量是否足够多。但由于特征匹配非常耗时，回环检测需要与过去所有关键帧匹配，这个运算量是绝对无法承受的。因此，有人提出了词袋模型，用来加速特征匹配。

二、词袋模型

词袋模型（Bag-of-Words，BoW）把特征当成一个个单词，通过比较两张图片中出现的单词是否一致，来判断这两张图片是否是同一场景。

为了能够把特征归类为单词，我们需要训练一个字典。所谓的字典就是包含了所有可能的单词的集合，为了提高通用性，需要使用海量的数据训练。

字典的训练其实是一个聚类的过程。假设所有图片中共提取了10,000,000个特征，可以使用K-means方法把它们聚成100,000个单词。但是，如果只是用这100,000个单词来匹配的话效率还是太低，因为每个特征需要比较100,000次才能找到自己对应的单词。为了提高效率，字典在训练的过程中构建了一个k个分支，深度为d的树，如下图所示。直观上看，上层结点提供了粗分类，下层结点提供了细分类，直到叶子结点。利用这个树，就可以将时间复杂度降低到对数级别，大大加速了特征匹配。

K叉树字典

三、使用DBoW3库

DBoW3库为我们提供了非常方便的训练词典和使用词典的方法。

训练词典时，只需要把所有训练用的图片的描述符传给DBoW3::Vocabulary的create方法就可以了。训练好的词袋模型保存在vocabulary.yml.gz文件中。

/***************************************************
 * 本节演示了如何根据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;
}

接下来，使用训练好的词袋模型对图片计算相似性评分。DBoW3为我们提供了两种计算相似性的方式，第一种是直接对两张图片比较；第二种是把图片集构造成一个数据库，再与另一张图片比较。

/***************************************************
 * 本节演示了如何根据前面训练的字典计算相似性评分
 * ************************************************/
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;
}

输出结果如下：

reading database
reading images...
detecting ORB features ...
comparing images with images
image 0 vs image 0 : 1
image 0 vs image 1 : 0.00399443
image 0 vs image 2 : 0.00608589
image 0 vs image 3 : 0.00409821
image 0 vs image 4 : 0.0066729
image 0 vs image 5 : 0.00374513
image 0 vs image 6 : 0.00448191
image 0 vs image 7 : -0
image 0 vs image 8 : 0.0103268
image 0 vs image 9 : 0.0320906

image 1 vs image 1 : 1
image 1 vs image 2 : 0.0238409
image 1 vs image 3 : 0.00697527
image 1 vs image 4 : 0.00517708
image 1 vs image 5 : 0.00556919
image 1 vs image 6 : 0.00487344
image 1 vs image 7 : 0.00538609
image 1 vs image 8 : 0.00814409
image 1 vs image 9 : 0.00587605

......

可以看到，图片越相似，评分越接近1。我们可以根据这个评分来判断两张图片是否是同一场景。但是直接给定一个绝对的阈值并不合适。通常，如果当前帧与之前某帧的相似度超过当前帧与上一个关键帧相似度的3倍，就认为可能存在回环。不过，这种做法要求关键帧之间的相似性不能太高，否则无法检测出回环。

代码下载地址：https://github.com/gaoxiang12/slambook/tree/master/ch12

四、参考资料

《视觉SLAM十四讲》第12讲 回环检测 高翔