SLAM高翔十四讲（十一）第十一讲 回环检测

一、回环检测

1.1 问题引入

1. 相机每次经过同一个位置，优化轨迹应和实际位置一致。添加回环检测后的位姿消除累积误差，回环检测的关键是检测出相机经过同一个地方。
2. 意义：
   1）回环监测使我们估计的地图和轨迹在长时间下保持正确性；
   2）其提供当前数据与历史数据的关联，可以利用回环检测进行重定位。

1.2 回环检测的方法

1. 最简单的方法对两幅图进行特征匹配，根据正确匹配的数量确定其关联性；
2. 基于里程计的几何关系，当相机运动到之前某个位置的附近时，检测它们有没有回环关系；
3. 基于外观的回环检测，根据两幅图的相似性确定其回环关系；
4. 从工程角度可以利用GPS信息

在本章中我们采用基于外观的回环检测算法，核心问题是如何计算图像间的相似性。在确定相似性前，我们需要了解准确率和召回率、词袋模型、字典等概念。

二、相关概念

2.1 准确率和召回率

准确率：描述的是算法提取的所有回环中是回环的概率；Precision=TP/(TP+FP)
召回率：是指所有真实回环被正确检测出俩的概率。Recall=TP/(TP+FN)

算法/事实	是回环	不是回环
是回环	真阳性TP	假阳性FP
不是回环	假阴性FN	真阴性TN

在SLAM系统中我们对准确率要求高一些，召回率相对宽松

2.2 词袋模型

词袋(BoW)，目的是用"图像上有哪几种特征"来描述一幅图像。根据这样来度量两幅图的相似性。

2.3 字典

1. 字典由很多单词构成，每个单词代表一个概念。
2. 字典生成问题类似于一个聚类问题。我们本章使用K叉树结合K-means聚类构建一个字典。

三、回环检测相似度计算

3.1 CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( loop_closure )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# opencv 
find_package(OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS} )

# dbow3 
# dbow3 is a simple lib so I assume you installed it in default directory 
set( DBoW3_INCLUDE_DIRS "/usr/local/include" )
set( DBoW3_LIBS "/usr/local/lib/libDBoW3.so" )

add_executable( feature_training feature_training.cpp )
target_link_libraries( feature_training ${OpenCV_LIBS} ${DBoW3_LIBS} )

add_executable( loop_closure loop_closure.cpp )
target_link_libraries( loop_closure ${OpenCV_LIBS} ${DBoW3_LIBS} )

add_executable( gen_vocab gen_vocab_large.cpp )
target_link_libraries( gen_vocab ${OpenCV_LIBS} ${DBoW3_LIBS} )

3.2 创建字典

#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 = "/home/robot/桌面/slambook2-master/ch11/data/"+to_string(i+1)+".png";
        images.push_back( imread(path) );
    }
    // 提取ORB特征
    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 );
    }
    
    // 创建字典
    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;
}

结果显示：分支数量10，树的深度5，单词数4972

reading images... 
detecting ORB features ... 
creating vocabulary ... 
vocabulary info: Vocabulary: k = 10, L = 5, Weighting = tf-idf, Scoring = L1-norm, Number of words = 4972
done

3.3 利用字典进行相似度计算

#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("/home/robot/桌面/slambook2-master/ch11/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 = "/home/robot/桌面/slambook2-master/ch11/data/" + to_string(i + 1) + ".png";
        images.push_back(imread(path));
    }

    // 注意：在这种情况下，我们将图像与自己生成的词汇表进行比较，这可能会导致过度拟合。
    // 提取ORB特征
    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);
    }

    // 我们可以直接比较图像，也可以将一张图像与图像字典进行比较
    // 1. 直接比较两幅图像
    cout << "（1）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;
    }

    // 2. 图像与图像字典进行比较
    cout << "（2）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;
}

3.4 结果

reading database
reading images... 
detecting ORB features ... 
（1）comparing images with images 
image 0 vs image 0 : 1
image 0 vs image 1 : 0.237894
image 0 vs image 2 : 0.254795
image 0 vs image 3 : 0.264314
image 0 vs image 4 : 0.257389
image 0 vs image 5 : 0.262672
image 0 vs image 6 : 0.285965
image 0 vs image 7 : 0.228053
image 0 vs image 8 : 0.239704
image 0 vs image 9 : 0.238567

image 1 vs image 1 : 1
image 1 vs image 2 : 0.27165
image 1 vs image 3 : 0.256173
image 1 vs image 4 : 0.261168
image 1 vs image 5 : 0.239754
image 1 vs image 6 : 0.237091
image 1 vs image 7 : 0.237345
image 1 vs image 8 : 0.247675
image 1 vs image 9 : 0.22602

image 2 vs image 2 : 1
image 2 vs image 3 : 0.220252
image 2 vs image 4 : 0.280244
image 2 vs image 5 : 0.234565
image 2 vs image 6 : 0.242935
image 2 vs image 7 : 0.216532
image 2 vs image 8 : 0.234954
image 2 vs image 9 : 0.219037

image 3 vs image 3 : 1
image 3 vs image 4 : 0.257467
image 3 vs image 5 : 0.238607
image 3 vs image 6 : 0.265279
image 3 vs image 7 : 0.211756
image 3 vs image 8 : 0.211692
image 3 vs image 9 : 0.235386

image 4 vs image 4 : 1
image 4 vs image 5 : 0.288339
image 4 vs image 6 : 0.258294
image 4 vs image 7 : 0.233924
image 4 vs image 8 : 0.20542
image 4 vs image 9 : 0.236427

image 5 vs image 5 : 1
image 5 vs image 6 : 0.249152
image 5 vs image 7 : 0.24961
image 5 vs image 8 : 0.233712
image 5 vs image 9 : 0.242769

image 6 vs image 6 : 1
image 6 vs image 7 : 0.237024
image 6 vs image 8 : 0.215265
image 6 vs image 9 : 0.213725

image 7 vs image 7 : 1
image 7 vs image 8 : 0.195906
image 7 vs image 9 : 0.203093

image 8 vs image 8 : 1
image 8 vs image 9 : 0.198902

image 9 vs image 9 : 1

（2）comparing images with database 
database info: Database: Entries = 10, Using direct index = no. Vocabulary: k = 10, L = 5, Weighting = tf-idf, Scoring = L1-norm, Number of words = 4983
searching for image 0 returns 4 results:
<EntryId: 0, Score: 1>
<EntryId: 6, Score: 0.285965>
<EntryId: 3, Score: 0.264314>
<EntryId: 5, Score: 0.262672>

searching for image 1 returns 4 results:
<EntryId: 1, Score: 1>
<EntryId: 2, Score: 0.27165>
<EntryId: 4, Score: 0.261168>
<EntryId: 3, Score: 0.256173>

searching for image 2 returns 4 results:
<EntryId: 2, Score: 1>
<EntryId: 4, Score: 0.280244>
<EntryId: 1, Score: 0.27165>
<EntryId: 0, Score: 0.254795>

searching for image 3 returns 4 results:
<EntryId: 3, Score: 1>
<EntryId: 6, Score: 0.265279>
<EntryId: 0, Score: 0.264314>
<EntryId: 4, Score: 0.257467>

searching for image 4 returns 4 results:
<EntryId: 4, Score: 1>
<EntryId: 5, Score: 0.288339>
<EntryId: 2, Score: 0.280244>
<EntryId: 1, Score: 0.261168>

searching for image 5 returns 4 results:
<EntryId: 5, Score: 1>
<EntryId: 4, Score: 0.288339>
<EntryId: 0, Score: 0.262672>
<EntryId: 7, Score: 0.24961>

searching for image 6 returns 4 results:
<EntryId: 6, Score: 1>
<EntryId: 0, Score: 0.285965>
<EntryId: 3, Score: 0.265279>
<EntryId: 4, Score: 0.258294>

searching for image 7 returns 4 results:
<EntryId: 7, Score: 1>
<EntryId: 5, Score: 0.24961>
<EntryId: 1, Score: 0.237345>
<EntryId: 6, Score: 0.237024>

searching for image 8 returns 4 results:
<EntryId: 8, Score: 1>
<EntryId: 1, Score: 0.247675>
<EntryId: 0, Score: 0.239704>
<EntryId: 2, Score: 0.234954>

searching for image 9 returns 4 results:
<EntryId: 9, Score: 1>
<EntryId: 5, Score: 0.242769>
<EntryId: 0, Score: 0.238567>
<EntryId: 4, Score: 0.236427>

done.

四、实验分析

4.1 增加字典规模

增加字典规模，无关图像的相似性会变小

4.2 相似性的评分处理

引入一个先验相似度，若当前关键帧与之前某个关键帧的相似度超过与上一个关键帧的相似度的3倍，认为回环。

4.3 关键帧的处理

回环检测的关键帧最好稀疏一些，彼此不同，又能覆盖整个环境。
把"相近"的回环聚成一类，使算法不要反复检测同一类的回环。

4.4 检测后的验证

在回环检测后增加一个验证步骤：1）设立回环的缓存机制；2）空间上一致性检测

4.5 联系机器学习