第十一讲：回环检测

第十一讲：回环检测

词袋检测的细节应该去阅读源码，后续结合ORB-SLAM2整理。

1 回环检测概述

​ 前端提供特征点的提取和轨迹、地图的初值，而后端负责对所有这些数据进行优化。然而，如果像视觉里程计那样仅考虑相邻时间上的关键帧，那么，之前产生的误差将不可避免地累积到下一个时刻，使得SLAM出现累积误差，长期估计的结果将不可靠，或者说，我们无法构建全局一致的轨迹和地图。

​ 回环检测方法：

​ 1. 基于里程计(Odometry based)的几何关系（矛盾）

​ 2. 基于外观(Appearance based)的几何关系

​ 在基于外观的回环检测算法中，核心问题是如何计算图像间的相似性。例如，对于图像A和图像B,我们要设计一种方法，计算它们之间的相似性评分：s(A,B)

​ 假阳性(False Positive)又称为感知偏差，而假阴性（False Negative)称为感知变异。为方便书写，由于我们希望算法和人类的判断一致，所以希望TP和TN尽量高，而FP和FN尽可能低。所以，对于某种特定算法，我
们可以统计它在某个数据集上的TP、TN、FP、FN的出现次数，并计算两个统计量：准确率Precision和召回率Recall。
$$\text{ Precision }=\mathrm{TP}/(\mathrm{TP}+\mathrm{FP}),\text{ Recall }=\mathrm{TP}/(\mathrm{TP}+\mathrm{FN})\text{. }$$
​ 准确率描述的是算法提取的所有回环中确实是真实回环的概率。而召回率则是指，在所有真实回环中被正确检测出来的概率。为什么取这两个统计量呢？因为它们有一定的代表性，并且通常是一对矛盾。

​ 当提高某个阈值时，算法可能变得更加“严格”它检出更少的回环，使准确率得以提高。同时，由于检出的数量变少了，许多原本是回环的地方就可能被漏掉，导致召回率下降。反之，如果我们选择更加宽松的配置，那么检出的回环数量将增加，得到更高的召回率，但其中可能混杂一些不是回环的情况，于是准确率下降。

2 词袋模型

​ 词袋模型用来描述一幅图像有哪几种特征，比如人，汽车等。我们利用这些特征来比较图像的相似度。

单词：由最基本的特征组成，比如人、汽车

字典：由单词组成

$$A=1\cdot w_1+1\cdot w_2+0\cdot w_3$$

$$s\left(\mathbf{a},\mathbf{b}\right)=1-\frac{1}{W}{\Vert \mathbf{a}-\mathbf{b}\Vert }_1$$

3 字典

3.1 字典结构

字典结构：分支数 = k，深度 = d，容纳$k^d$个单词

成员变量	访问	意义
typedef unsigned int WordId	public	当前单词距离最近的叶子节点id
typedef double WordValue	public	当前单词距离最近的叶子节点对应的权重
std::map<WordId, WordValue>	public	词袋向量
typedef unsigned int NodeId	public	单词所属的节点id，如上图所示
enum LNorm	public	归一化时使用的 L-范数， L1和L2两种范数
enum WeightingType	public	权重类型TF_IDF、TF、IDF、BINARY
enum ScoringType	public	评分类型 L1_NORM、L2_NORM、CHI_SQUARE、KL、BHATTACHARYYA、 DOT_PRODUCT

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;



int main( int argc, char** argv ) {
    // 读取图像
    vector<Mat> images; // 每个数组成员都是变量
    for ( int i=0; i<10; i++ )
    {
        string path = "./data/"+to_string(i+1)+".png";
        images.push_back(imread(path));
    }
    // 特征检测
    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;
}

4 词袋模型注意事项

4.1 相似性评分处理

词袋向量，$w$表示单词，$\eta$表示单词对应的权重

$$A=\{(w_1,{\eta }_1),(w_2,{\eta }_2),\dots ,(w_N,{\eta }_N)\}\stackrel{\mathrm{def}}{=}{\mathbf{v}}_A$$

计算两个词袋向量的相似度方法，eg $L_1$范数

$$s\left({\mathbf{v}}_A-{\mathbf{v}}_B\right)=2\sum _{i=1}^N|{\mathbf{v}}_{Ai}|+|{\mathbf{v}}_{Bi}|-|{\mathbf{v}}_{Ai}-{\mathbf{v}}_{Bi}|$$

4.2 loop_closure.cpp闭环检测

#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) {
    // 读取上面十张图生成的字典database  
    cout << "reading database" << endl;
    DBoW3::Vocabulary vocab("./vocabulary.yml.gz");
    //DBoW3::Vocabulary vocab("../vocab_larger.yml.gz");  增强的字典
    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));
    }

    // 特征检测
    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 << "comparing images with images " << endl;
    for (int i = 0; i < images.size(); i++) {
    // 表示图像的单词矢量
        DBoW3::BowVector v1;
        
    // 因为一张图像中不可能含有字典vocab的全部单词，所以transform函数将第i个图像 每个单词的ID和权重记录在v1中。  元素的值要么是0，表示图像i中没有这个单词；要么是该单词的权重
        vocab.transform(descriptors[i], v1);
        for (int j = i; j < images.size(); j++) {
            DBoW3::BowVector v2;
            vocab.transform(descriptors[j], v2);
            
            // 我们对比两个向量V1和V2，计算两者的相似度，即分数。
            double score = vocab.score(v1, v2);
            cout << "image " << i << " vs image " << j << " : " << score << endl;
        }
        cout << endl;
    }

    // 这里相当于创建了一个数据库，这样子我们只输出与目标图像相近的前几幅图像
    cout << "comparing images with database " << endl;
    
    DBoW3::Database db(vocab, false, 0);  // 创建字典数据库
    for (int i = 0; i < descriptors.size(); i++)
        db.add(descriptors[i]);				// 把i图的描述子 与 数据库对比
    cout << "database info: " << db << endl;
    for (int i = 0; i < descriptors.size(); i++) {
        
        // 定义每个图像查询结果
        DBoW3::QueryResults ret;
        db.query(descriptors[i], ret, 4);      // 描述子，输出结果，结果数量
        cout << "searching for image " << i << " returns " << ret << endl << endl;
    }
    cout << "done." << endl;
}

4.3 相似性评分的处理

​ 对任意两幅图像，我们都能给出一个相似性评分，但是只利用这个分值的绝对大小对我们并不一定有很好的帮助。例如，有些环境的外观本来就很相似，像办公室往往有很多同款式的桌椅一样；另一些环境则各个地方都有很大的不同。考虑到这种情况，我们会取一个先验相似度$s\left({\mathbf{v}}_t,{\mathbf{v}}_{t-\Delta t}\right)$,它表示某时刻关键帧图像与上一时刻的关键帧的相似性。然后，其他的分值都参照这个值进行归一化：
$$s{\left({\mathbf{v}}_t,{\mathbf{v}}_{t_j}\right)}^{\prime }=s\left({\mathbf{v}}_t,{\mathbf{v}}_{t_j}\right)/s\left({\mathbf{v}}_t,{\mathbf{v}}_{t-\Delta t}\right)$$

4.4 关键帧处理

​ 我们再检测回环的时候，必须考虑关键帧的选取。一般来讲，当前帧和其上一帧是最相似的，所以再检测回环的时候，我们应该避开选择当前帧相邻的关键帧。

​ 建立回环后，后续相邻的关键帧也会和闭环帧有极大的相似程度，这些我们都应该选择性避开或聚类。

5 关于安装与编译

• 安装DBoW库

# 仍然是到3dr库安装  或者到下面网站下载安装包https://github.com/rmsalinas/DBow3/tree/c5ae539abddcef43ef64fa130555e2d521098369
cd DBoW3
mkdir build && cd build
cmake ..
make -j4
sudo make install
sudo ldconfig

• 环境编译错误解决libDBoW3.a

# 编译错误
Consolidate compiler generated dependencies of target feature_training
make[2]: *** 没有规则可制作目标“/usr/local/lib/libDBoW3.a”，由“feature_training” 需求。 停止。
CMakeFiles/Makefile2:86: recipe for target 'CMakeFiles/feature_training.dir/all' failed
make[1]: *** [CMakeFiles/feature_training.dir/all] Error 2
Makefile:90: recipe for target 'all' failed

make[2]: *** Waiting for unfinished jobs....
make[2]: *** No rule to make target '/usr/local/lib/libDBoW3.a', needed by 'loop_closure'.  Stop.
make[2]: *** No rule to make target '/usr/local/lib/libDBoW3.a', needed by 'gen_vocab'.  Stop.
make[2]: *** Waiting for unfinished jobs....
make[2]: *** Waiting for unfinished jobs....


#上面意思就是  libDBoW3.a 没找到  就是我们安装的是动态库，CMakeLists.txt中找的是静态库
这是CMakeList.txt文件中指令改一下
set( DBoW3_INCLUDE_DIRS "/usr/local/include" )   # 找头文件   存在
#set( DBoW3_LIBS "/usr/local/lib/libDBoW3.a" )	 # 找源文件   .a文件不存在
set( DBoW3_LIBS "/usr/local/lib/libDBoW3.so" )	 # 改为.so文件


# 这是装dbw时候sudo make install添加路径，确实没有.a文件
pj@p: ~/slambook/slambook2/3rdparty/DBoW3/build$ sudo make install
[sudo] pj 的密码： 
[ 60%] Built target DBoW3
[ 73%] Built target demo_general
[ 86%] Built target create_voc_step0
[100%] Built target create_voc_step1
Install the project...
-- Install configuration: "Release"
-- Up-to-date: /usr/local/lib/cmake/FindDBoW3.cmake
-- Up-to-date: /usr/local/lib/cmake/DBoW3/DBoW3Config.cmake
-- Up-to-date: /usr/local/lib/libDBoW3.so.0.0.1
-- Up-to-date: /usr/local/lib/libDBoW3.so.0.0
-- Up-to-date: /usr/local/lib/libDBoW3.so
-- Up-to-date: /usr/local/include/DBoW3/BowVector.h
-- Up-to-date: /usr/local/include/DBoW3/DBoW3.h
-- Up-to-date: /usr/local/include/DBoW3/Database.h
-- Up-to-date: /usr/local/include/DBoW3/DescManip.h
-- Up-to-date: /usr/local/include/DBoW3/FeatureVector.h
-- Up-to-date: /usr/local/include/DBoW3/QueryResults.h
-- Up-to-date: /usr/local/include/DBoW3/ScoringObject.h
-- Up-to-date: /usr/local/include/DBoW3/Vocabulary.h
-- Up-to-date: /usr/local/include/DBoW3/exports.h
-- Up-to-date: /usr/local/include/DBoW3/quicklz.h
-- Up-to-date: /usr/local/include/DBoW3/timers.h
-- Up-to-date: /usr/local/bin/demo_general
-- Up-to-date: /usr/local/bin/create_voc_step0
-- Up-to-date: /usr/local/bin/create_voc_step1