add ORBvoc.bin to speed up the voc file loading

This method is from raulmur#21. But it hasn't been accepted and now the branch has conflicts. So I change the code manually.

Examples/js/index.html

@@ -27,7 +27,7 @@
 <script>
 var Module = {
   preRun: [function() {
-    Module.FS_createPreloadedFile('/', 'ORBvoc.txt', 'ORBvoc.txt', true, false);
+    Module.FS_createPreloadedFile('/', 'ORBvoc.bin', 'ORBvoc.bin', true, false);
     Module.FS_createPreloadedFile('/', 'c270.yaml', 'c270.yaml', true, false);
   }],
   _main: function() {opencvIsReady();}

Examples/js/main.js

@@ -16,6 +16,8 @@ let stream = null;
 
 let slam = null;
 
+let trStatus = -2;
+
 let info = document.getElementById('info');
 
 function startCamera() {
@@ -74,14 +76,32 @@ function startVideoProcessing() {
   requestAnimationFrame(processVideo);
 }
 
+let count = 0;
+
 function processVideo() {
   stats.begin();
   canvasInputCtx.drawImage(video, 0, 0, videoWidth, videoHeight);
   let imageData = canvasInputCtx.getImageData(0, 0, videoWidth, videoHeight);
   srcMat.data.set(imageData.data);
 
-  let pose = slam.TrackMonocular(srcMat, performance.now())
-  console.log('Tracking state: ' + slam.GetTrackingState())
+  let pose = slam.TrackMonocular(srcMat, performance.now());
+  console.log('No. ' + ++count);
+  let st = slam.GetTrackingState();
+  if (trStatus != st) {
+    trStatus = st;
+    switch (trStatus) {
+      case -1: console.log('Tracking state: ' + 'system not ready'); break;
+      case 0: console.log('Tracking state: ' + 'no images yet'); break;
+      case 1: console.log('Tracking state: ' + 'not initialized'); break;
+      case 2: console.log('Tracking state: ' + 'ok'); break;
+      case 3: console.log('Tracking state: ' + 'lost'); break;
+    }
+  }
+  if(st == 2) {
+    console.log(pose.data32F);
+    console.log("GetKeyFramesInMap: " + slam.GetKeyFramesInMap());
+    console.log("GetMapPointsInMap: " + slam.GetMapPointsInMap());
+  }
   pose.delete();
 
   canvasOutputCtx.drawImage(canvasInput, 0, 0, videoWidth, videoHeight);
@@ -120,7 +140,7 @@ function opencvIsReady() {
   }
   info.innerHTML = '';
   initUI();
-  slam = new cv.SLAM('ORBvoc.txt', 'c270.yaml', cv.MONOCULAR)
+  slam = new cv.SLAM('ORBvoc.bin', 'c270.yaml', cv.MONOCULAR);
   console.log('SLAM is ready');
-  startCamera();
+   startCamera();
 }

Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h

@@ -246,6 +246,18 @@ class TemplatedVocabulary
    */
   void saveToTextFile(const std::string &filename) const;  
 
+  /**
+   * Loads the vocabulary from a binary file
+   * @param filename
+   */
+  bool loadFromBinaryFile(const std::string &filename);
+
+  /**
+   * Saves the vocabulary into a binary file
+   * @param filename
+   */
+  void saveToBinaryFile(const std::string &filename) const;  
+
   /**
    * Saves the vocabulary into a file
    * @param filename
@@ -1450,6 +1462,78 @@ void TemplatedVocabulary<TDescriptor,F>::saveToTextFile(const std::string &filen
 
 // --------------------------------------------------------------------------
 
+template<class TDescriptor, class F>
+bool TemplatedVocabulary<TDescriptor,F>::loadFromBinaryFile(const std::string &filename) {
+  fstream f;
+  f.open(filename.c_str(), ios_base::in|ios::binary);
+  unsigned int nb_nodes, size_node;
+  f.read((char*)&nb_nodes, sizeof(nb_nodes));
+  f.read((char*)&size_node, sizeof(size_node));
+  f.read((char*)&m_k, sizeof(m_k));
+  f.read((char*)&m_L, sizeof(m_L));
+  f.read((char*)&m_scoring, sizeof(m_scoring));
+  f.read((char*)&m_weighting, sizeof(m_weighting));
+  createScoringObject();
+
+  m_words.clear();
+  m_words.reserve(pow((double)m_k, (double)m_L + 1));
+  m_nodes.clear();
+  m_nodes.resize(nb_nodes+1);
+  m_nodes[0].id = 0;
+  char buf[size_node]; int nid = 1;
+  while (!f.eof()) {
+  f.read(buf, size_node);
+  m_nodes[nid].id = nid;
+  // FIXME
+  const int* ptr=(int*)buf;
+  m_nodes[nid].parent = *ptr;
+  //m_nodes[nid].parent = *(const int*)buf;
+  m_nodes[m_nodes[nid].parent].children.push_back(nid);
+  m_nodes[nid].descriptor = cv::Mat(1, F::L, CV_8U);
+  memcpy(m_nodes[nid].descriptor.data, buf+4, F::L);
+  m_nodes[nid].weight = *(float*)(buf+4+F::L);
+  if (buf[8+F::L]) { // is leaf
+    int wid = m_words.size();
+    m_words.resize(wid+1);
+    m_nodes[nid].word_id = wid;
+    m_words[wid] = &m_nodes[nid];
+  }
+  else
+    m_nodes[nid].children.reserve(m_k);
+  nid+=1;
+  }
+  f.close();
+  return true;
+}
+
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::saveToBinaryFile(const std::string &filename) const {
+  fstream f;
+  f.open(filename.c_str(), ios_base::out|ios::binary);
+  unsigned int nb_nodes = m_nodes.size();
+  float _weight;
+  unsigned int size_node = sizeof(m_nodes[0].parent) + F::L*sizeof(char) + sizeof(_weight) + sizeof(bool);
+  f.write((char*)&nb_nodes, sizeof(nb_nodes));
+  f.write((char*)&size_node, sizeof(size_node));
+  f.write((char*)&m_k, sizeof(m_k));
+  f.write((char*)&m_L, sizeof(m_L));
+  f.write((char*)&m_scoring, sizeof(m_scoring));
+  f.write((char*)&m_weighting, sizeof(m_weighting));
+  for(size_t i=1; i<nb_nodes;i++) {
+  const Node& node = m_nodes[i];
+  f.write((char*)&node.parent, sizeof(node.parent));
+  f.write((char*)node.descriptor.data, F::L);
+  _weight = node.weight; f.write((char*)&_weight, sizeof(_weight));
+  bool is_leaf = node.isLeaf(); f.write((char*)&is_leaf, sizeof(is_leaf)); // i put this one at the end for alignement....
+  }
+  f.close();
+}
+
+// --------------------------------------------------------------------------
+
 template<class TDescriptor, class F>
 void TemplatedVocabulary<TDescriptor,F>::save(const std::string &filename) const
 {

include/System.h

@@ -25,6 +25,7 @@
 #include<string>
 #include<thread>
 #include "opencv2/core.hpp"
+#include "opencv2/imgproc.hpp"
 
 #include "Tracking.h"
 #include "FrameDrawer.h"
@@ -119,6 +120,19 @@ class System
     // Information from most recent processed frame
     // You can call this right after TrackMonocular (or stereo or RGBD)
     int GetTrackingState();
+
+    // Get Map values
+    int GetKeyFramesInMap() {
+        if(mpMap)
+            return mpMap->KeyFramesInMap();
+        return -1;
+    }
+    int GetMapPointsInMap() {
+        if(mpMap)
+            return mpMap->MapPointsInMap();
+        return -1;
+    }
+
     std::vector<MapPoint*> GetTrackedMapPoints();
     std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();
 

src/Frame.cc

@@ -76,11 +76,14 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
     mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
     mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
 
+
+    ExtractORB(0,imLeft);
+    ExtractORB(1,imRight);
     // ORB extraction
-    thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
+/*    thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
     thread threadRight(&Frame::ExtractORB,this,1,imRight);
     threadLeft.join();
-    threadRight.join();
+    threadRight.join();*/
 
     N = mvKeys.size();
 

src/Initializer.cc

@@ -101,12 +101,14 @@ bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatc
     float SH, SF;
     cv::Mat H, F;
 
-    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
+    FindHomography(ref(vbMatchesInliersH), ref(SH), ref(H));
+    FindFundamental(ref(vbMatchesInliersF), ref(SF), ref(F));
+    /*thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
     thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
 
     // Wait until both threads have finished
     threadH.join();
-    threadF.join();
+    threadF.join();*/
 
     // Compute ratio of scores
     float RH = SH/(SH+SF);

src/System.cc

@@ -29,6 +29,12 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <time.h>
+
+bool txt_suffix(const std::string &str) {
+  std::size_t index = str.find(".txt", str.size() - 4);
+  return (index != std::string::npos);
+}
 
 namespace ORB_SLAM2
 {
@@ -64,16 +70,20 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
 
     //Load ORB Vocabulary
     cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
-
+    clock_t tStart = clock();
     mpVocabulary = new ORBVocabulary();
-    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
+    bool bVocLoad = false; // chose loading method based on file extension
+    if (txt_suffix(strVocFile))
+        bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
+    else
+        bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);
     if(!bVocLoad)
     {
         cerr << "Wrong path to vocabulary. " << endl;
-        cerr << "Falied to open at: " << strVocFile << endl;
+        cerr << "Failed to open at: " << strVocFile << endl;
         exit(-1);
     }
-    cout << "Vocabulary loaded!" << endl << endl;
+    printf("Vocabulary loaded in %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
 
     //Create KeyFrame Database
     mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);