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 }