DBoW2应用

图像对应的bag-of-words向量\(v_t\)

假设词典总共有\(W\)个单词，那么每一幅图像能够用一个\(W\)维的向量表示
\(（t_1, t_2, t_3, ..., t_W）\)其中
\[t_i = \frac{n_{id}}{n_{nd}}\log\frac{N}{n_i}\]
其中，\(n_{id}\)是单词i在当前帧图像中出现的次数，\(n_d\)是当前图像中所以单词的数目，\(n_i\)是词汇\(i\)在整个数据库中出现的次数，\(N\)是为所有图像中描述子的数目，\(\frac{n_{id}}{n_{nd}}\)表示\(tf\)，\(\log\frac{N}{n_i}\)表示\(idf\)，在建立视觉词袋的时候已经得到。

反向索引：描述词汇中的每一个单词在出现过的图像列表，能够加速查找具有相同词汇的图像。（用什么数据结构实现的？）存储一系列\(<I_t, v_t^i>\)（其中\(I_t\)为图像的索引，\(v_t^i\)为该单词在图像中的权重）。查询数据库时只需要比较有相同词汇的图像，加速查找，也就是说搜索图像只需要（1）词袋和（2）反向索引
具体流程如下：提取当前帧的描述子，查询字典，得到单词，查找反向索引表，得到所有具有该单词的图像。

直接索引:（存储每一幅图像的特征）对于每一幅图像\(I_t\)，存储其使用的词汇的祖先节点（任何一层l）及每一个节点的局部特征\(f_{tj}\)
直接索引能够加快闭环检测的几何认证，因为只有具有相同的词汇或者在第l层有相同的祖先的关键帧才需要进行几何认证
直接索引存储每一个图像\(I_t\)中词汇的在第\(l\)层（预先给定的）的所在的节点已经所有该图像中属于该节点的描述子。

DBow2的作用：通过视觉词汇将一幅图像转换成稀疏的数字向量（能够对大量的图像进行处理）
视觉词汇是离线建立的，通过将描述子空间划分成W个视觉词汇

代码如下：

#include <iostream>
#include <vector>

// DBoW2
//#include "DBoW2/DBoW2.h"
//#include <DUtils/DUtils.h>
//#include <DUtilsCV/DUtilsCV.h> // defines macros CVXX
//#include <DVision/DVision.h>

#include "Thirdparty/DBoW2/DBoW2/FORB.h"
#include "Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h"
//#include "Thirdparty/DBoW2/DBoW2/FClass.h"

// OpenCV
#include <opencv2/opencv.hpp>
#include "opencv2/core/core.hpp"
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <opencv2/nonfree/features2d.hpp>
//#include <opencv2/features2d/features2d.hpp>

// ROS
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <boost/foreach.hpp>
#include <cv_bridge/cv_bridge.h>

#include "ORBextractor.h"

#include <dirent.h>
#include <string.h>

using namespace DBoW2;
using namespace DUtils;
using namespace std;
using namespace ORB_SLAM;

// - - - - - --- - - - -- - - - - -

/// ORB Vocabulary
typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>
ORBVocabulary;

/// ORB Database
//typedef DBoW2::TemplatedDatabase<DBoW2::FORB::TDescriptor, DBoW2::FORB>
//ORBDatabase;

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

void extractORBFeatures(cv::Mat &image, vector<vector<cv::Mat> > &features, ORBextractor* extractor);
void changeStructureORB( const cv::Mat &descriptor,vector<bool> &mask, vector<cv::Mat> &out);
void isInImage(vector<cv::KeyPoint> &keys, float &cx, float &cy, float &rMin, float &rMax, vector<bool> &mask);
void createVocabularyFile(ORBVocabulary &voc, std::string &fileName, const vector<vector<cv::Mat> > &features);

// ----------------------------------------------------------------------------

int main()
{
    //Extracting ORB features from image folder
    vector<std::string> filenames;
    std::string folder = "/home/saodiseng/FRONTAL/";

    cv::glob(folder, filenames);

    // initialze     ORBextractor
    int nLevels = 5;//6;
    ORBextractor* extractor = new ORBextractor(1000,1.2,nLevels,1,20);
    int nImages = filenames.size();

    vector<vector<cv::Mat > > features;
    features.clear();
    features.reserve(nImages);

    //cv_bridge::CvImageConstPtr cv_ptr;
    cv::Mat image;

    //cout << "> Using bag file: " << bagFile << endl;
    cout << "> Extracting Features from " << nImages << " images..." << endl;
    //BOOST_FOREACH(rosbag::MessageInstance const m, viewTopic)
    for(int i = 0; i < nImages; ++i)
    {
        //sensor_msgs::Image::ConstPtr i = m.instantiate<sensor_msgs::Image>();
        std::cout << "Processing the " << i <<" image " << std::endl;
        cv::Mat src = cv::imread(filenames[i]);
        imshow("View", src);
        cv::waitKey(1);
        if (!src.empty())
        {
            //cv_ptr = cv_bridge::toCvShare(i);
            cv::cvtColor(src, image, CV_RGB2GRAY);
            extractORBFeatures(image, features, extractor);
        }

    }

    //bag.close();

    cout << "... Extraction done!" << endl;

    // Creating the Vocabulary
    // define vocabulary
    const int k = 10; // branching factor
    const WeightingType weight = TF_IDF;
    const ScoringType score = L1_NORM;
    ORBVocabulary voc(k, nLevels, weight, score);

    std::string vociName = "vociOmni.txt";

    createVocabularyFile(voc, vociName, features);

    cout << "--- THE END ---" << endl;
    
    return 0;
}


// ----------------------------------------------------------------------------

void extractORBFeatures(cv::Mat &image, vector<vector<cv::Mat> > &features, ORBextractor* extractor) {
    vector<cv::KeyPoint> keypoints;
    cv::Mat descriptorORB;
    // extract
    (*extractor)(image, cv::Mat(), keypoints, descriptorORB);

    // reject features outside region of interest
    vector<bool> mask;

    float cx = 0; float cy = 0;
    float rMin = 0; float rMax = 0;
    isInImage(keypoints, cx, cy, rMin, rMax, mask);

    // create descriptor vector for the vocabulary
    features.push_back(vector<cv::Mat>());
    changeStructureORB(descriptorORB, mask, features.back());
    imshow("ORBFeature", features.back().back());
}

// ----------------------------------------------------------------------------

void changeStructureORB( const cv::Mat &descriptor,vector<bool> &mask, vector<cv::Mat> &out) {
    for (int i = 0; i < descriptor.rows; i++) {
        if(mask[i]) {
            out.push_back(descriptor.row(i));
        }
    }
}

// ----------------------------------------------------------------------------

void isInImage(vector<cv::KeyPoint> &keys, float &cx, float &cy, float &rMin, float &rMax, vector<bool> &mask) {
    int N = keys.size();
    mask = vector<bool>(N, false);
    int num = 0;
    for(int i=0; i<N; i++) {
        cv::KeyPoint kp = keys[i];

        float u = kp.pt.x;
        float v = kp.pt.y;

        if(u>20 && u<320-20 && v>20 && v<240-20)
        {
            mask[i] = true;
            num ++;
        }
    }
    std::cout << "In image number " << num << std::endl;

}

// ----------------------------------------------------------------------------

void createVocabularyFile(ORBVocabulary &voc, std::string &fileName, const vector<vector<cv::Mat> > &features)
{

    cout << "> Creating vocabulary. May take some time ..." << endl;
    voc.create(features);
    cout << "... done!" << endl;

    cout << "> Vocabulary information: " << endl
        << voc << endl << endl;

    // save the vocabulary to disk
    cout << endl << "> Saving vocabulary..." << endl;
    voc.saveToTextFile(fileName);
    cout << "... saved to file: " << fileName << endl;
}

基于DBoW2做闭环检测

A.查询数据库
通过数据库存储和检索相似的所有图像。步骤为：首先将图像图像转换成bag-of-words向量\(v_t\)（\(tf-idf\)，开头的公式），然后查找数据库中最相似的bag-of-words向量集，\(s(v_t,v_{ti})\)（多少个？？）->正则化（\(s(v_t,v_{t- \Delta t}\)）很小的情况单独考虑）为\(\eta (v_t, v_{t_j})=\frac{s(v_t,v_{t_j})}{s(v_t, v_{t-\Delta t})}\)->舍弃小于阈值的匹配
计算两个bag-of-word向量（两帧图像）1和v2的相似度
\[s(v_1, v_2)=1-\frac{1}{2}\vert \frac{v_1}{\vert v_1\vert}-\frac{v_2}{\vert v_2\vert}\vert\]

B.匹配聚类
为了防止时间上很近的关键帧之间相互竞争，将检索得到的时间戳相差比价小的帧聚成island并将它们看做一个匹配，一系列匹配可以转换成一个匹配\(<v_t, V_{T_i}>\)，island也根据评分排序，选择最高的。Island的得分为$ H(v_t, V_{T_i})=\sum_{j=n_i}^{m_i}\eta(v_t, v_{t_j})$。

C.Temporal consistency（时间一致性）
检测\(V_{T_i}\)和以前的查询结果\(<v_{t-\Delta t},V_{T_j}>\)的\(T_i,T_j\)时间一致性，\(<v_t,V_{T_i}>\)必须和k个以前的匹配查询结果\(<v_{t-\Delta t},V_{T_j}>\)一致(\(k\)个以前的匹配的island时间\(T_i\)接近重叠)，一旦通过一致性检验，则选取island\(V_{T_i}\)中得分最高的词汇\(v_{t'}\)

D.有效的几何一致性
用RANSAC方法得到\(I_t\)和\(I_t'\)的基础矩阵（至少12个对应点），查找对应的特征点（brute force和k-d tree方法）
使用直接索引近似最近邻（字典树中属于第l层的同一个节点，\(l\)提前设定，是速度和recall的折中）具体做法为：
（1）往数据库中加入图像时，在直接索引中存储节点和一些特征的对；
（2）在得到图像间的对应点时，在直接索引中查找只有在第l层属于同一个节点的描述子，并进行比较。这个方法能够提高对应点计算，l是提前固定的是对应点数目和进行该操作的时间的折中。