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;
}