#include<iostream>
#include<opencv2/opencv.hpp>
#include "DBoW2/DBoW2/FORB.h"
#include "DBoW2/DBoW2/TemplatedVocabulary.h"
#include<string>
using namespace std;
using namespace cv;
using namespace DBoW2;

typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>
ORBVocabulary;

 
int main()
{
    vector<Mat> images;
    int size = 0;

    vector<cv::String> filenames;

    //for (int i = 0; i < 4; ++i)
    for (int i = 0; i < 3; ++i)
    {
        //cv::String folder = "/home/yangtze/SLAM/dataSet/1280_720/imgs/cam" + to_string(i);
        cv::String folder = "/home/yangtze/SLAM/dataSet/indoor_dynamic/imgs/cam
 
" + to_string(i);
        cv::glob(folder, filenames);
        int nImages = filenames.size();
        size = size + nImages;
        for (int i = 0; i < nImages; ++i)
        {
            images.push_back(imread(filenames[i]));
        }
    }

    Ptr<Feature2D>  detector=ORB::create();

    vector
 
<Mat> descriptors(size);

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

    for(int i = 0; i < size; ++i)
    {
        cv::Mat image = images[i];
        vector<KeyPoint> keypoints;
        detector->detectAndCompute(image,Mat(),keypoints,descriptors[i]); 

    }
    
   // printf("LINE:%d\n",__LINE__);
    for (int i = 0; i < size; ++i)
    {
        //printf("LINE:%d\n",__LINE__);
        cv::Mat tmpdescriptor = descriptors[i];
        features.push_back(vector<cv::Mat>());
        for (int j = 0; j < tmpdescriptor.rows; j++)
        {
            //printf("LINE:%d\n",__LINE__);
            features.back().push_back(tmpdescriptor.row(j));
        }
    
    }
    //printf("LINE:%d\n",__LINE__);
    const int K = 10;   
    const int L = 5;
    const WeightingType weight = TF_IDF;
    const ScoringType score = L1_NORM;

    ORBVocabulary orbVocabulary(K, L, weight, score);
 
    string fileName = "ParkingArea.yaml";
 
    orbVocabulary.create(features);

    orbVocabulary.save(fileName);

    printf("SAVE TO THE FILE\n");
}