SLAM 1 : 回环检测(Loop Closure Detection)

这个系列文章主要是记录自己在学习SLAM中的一些总结。

 

在这里，首先我们介绍以下SLAM一个相对独立的单元-回环检测(Loop closure detection). 先介绍这部分的原因是: 1) 它是相对独立的一块，可以和整个slam系统分离开，有些slam(如激光slam)就没有这个单元， SVO也没有; 2) 如果要学习ORB-SLAM， 前端和后端代码中都有提到这个，有助于理解。

1. 加入回环检测的目的

加入回环检测的目的是为了通过找到已经走过的地方然后构建一个约束，从而达到减小累积误差。

2. 方法

目前常用的方法是Visual Bag-of-Word。 DBoW是通过用图像上的几种特征来描述一幅图。 简单的说: 首先通过一个大的样本数据库，对图像提取其特征点然后训练为不同的words。 比如说数据库中提取了N个特征点，需要分成k个单词的字典。我们可以先随机的选取k个中心点； 然后计算最近的点归入此类; 然后对每一类在重新计算中心点; 如果中心点收敛了，则退出，否则继续迭代。 这就是k-means的思路。

当然，k-means有固有的问题，比如随机选取点的话可能导致每次的聚类结果不同 以及 对k的值的选取问题。 （k-means++)

有了数据库后，我们可以根据图像中的特征点来查找单词。 比较naive的方法是说通过frame-to-frame的对比，很明显效率低下。 虽然我们也可以先对其排好序，然后通过二分法来查找，效率也不高。 因此， 常用的方法是k-d tree. (数据结构的重要性!!!)

如上图就是一个k 叉树结构 （2叉树实际上）， 从而查询结果只和深度有关了。

 

3.实践

#include <iostream>
#include <vector>
#include <string>

#include <DBoW3/DBoW3.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

using namespace std;
using namespace cv;

int main(int argc, char** argv){
    // 1. load vocabulary
    cout << "read database ..." << endl;
    DBoW3::Vocabulary vocab("vocabulary_test.yml.gz");
    if(vocab.empty()) {
        cerr << "vocab is empty." << endl;
        return 0;
    }

    // 2. load images
    cout << " load images ..." << endl;
    vector<cv::Mat> images;
    for(int i = 0; i < 10; i++ )
    {
        string imgPath = "../data/" + std::to_string(i+1) + ".png";
        images.push_back(imread(imgPath));
    }

    // 3. extract features with descriptors
    cout << "extract features with descriptors...." << endl;

    cv::Ptr<cv::ORB> orb = cv::ORB::create();
    vector<cv::Mat> descriptors;

    for(auto& image:images) {
        vector<cv::KeyPoint> kp;
        cv::Mat desp;

        orb->detectAndCompute(image, cv::Mat(), kp, desp);
        descriptors.push_back(desp);
    }

    // 4. loop closure detection
    cout <<"loop closure detection ..." << endl;
    DBoW3::Database db(vocab, false, 0);
    for(auto& desp:descriptors)
        db.add(desp);
    cout<<"database info: "<<db<<endl;

    int i = 0;
    for(auto& desp:descriptors) {
        DBoW3::QueryResults rest;
        db.query(desp, rest, 4);
        cout<<"searching for image "<< i <<" returns "<<rest<<endl<<endl;
        i++;
    }

    cout << " done." << endl;

    return 0;
}