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.實踐

 1 #include <iostream>
 2 #include <vector>
 3 #include <string>
 4 
 5 #include <DBoW3/DBoW3.h>
 6 #include <opencv2/core/core.hpp>
 7 #include <opencv2/highgui/highgui.hpp>
 8 #include <opencv2/features2d/features2d.hpp>
 9 
10 using namespace std;
11 using namespace cv;
12 
13 int main(int argc, char** argv){
14     // 1. load vocabulary
15     cout << "read database ..." << endl;
16     DBoW3::Vocabulary vocab("vocabulary_test.yml.gz");
17     if(vocab.empty()) {
18         cerr << "vocab is empty." << endl;
19         return 0;
20     }
21 
22     // 2. load images
23     cout << " load images ..." << endl;
24     vector<cv::Mat> images;
25     for(int i = 0; i < 10; i++ )
26     {
27         string imgPath = "../data/" + std::to_string(i+1) + ".png";
28         images.push_back(imread(imgPath));
29     }
30 
31     // 3. extract features with descriptors
32     cout << "extract features with descriptors...." << endl;
33 
34     cv::Ptr<cv::ORB> orb = cv::ORB::create();
35     vector<cv::Mat> descriptors;
36 
37     for(auto& image:images) {
38         vector<cv::KeyPoint> kp;
39         cv::Mat desp;
40 
41         orb->detectAndCompute(image, cv::Mat(), kp, desp);
42         descriptors.push_back(desp);
43     }
44 
45     // 4. loop closure detection
46     cout <<"loop closure detection ..." << endl;
47     DBoW3::Database db(vocab, false, 0);
48     for(auto& desp:descriptors)
49         db.add(desp);
50     cout<<"database info: "<<db<<endl;
51 
52     int i = 0;
53     for(auto& desp:descriptors) {
54         DBoW3::QueryResults rest;
55         db.query(desp, rest, 4);
56         cout<<"searching for image "<< i <<" returns "<<rest<<endl<<endl;
57         i++;
58     }
59 
60     cout << " done." << endl;
61 
62     return 0;
63 }