added binary version of vocabulary to speedup ORB_SLAM2 start

CMakeLists.txt

@@ -95,3 +95,8 @@ add_executable(mono_kitti
 Examples/Monocular/mono_kitti.cc)
 target_link_libraries(mono_kitti ${PROJECT_NAME})
 
+# Build tools
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/tools)
+add_executable(bin_vocabulary
+tools/bin_vocabulary.cc)
+target_link_libraries(bin_vocabulary ${PROJECT_NAME})

Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h

@@ -246,6 +246,19 @@ 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 +1463,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
 {

build.sh

@@ -29,3 +29,7 @@ mkdir build
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
 make -j
+cd ..
+
+echo "Converting vocabulary to binary"
+./tools/bin_vocabulary

src/PnPsolver.cc

@@ -196,7 +196,7 @@ cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInlie
 
             add_correspondence(mvP3Dw[idx].x,mvP3Dw[idx].y,mvP3Dw[idx].z,mvP2D[idx].x,mvP2D[idx].y);
 
-            vAvailableIndices[idx] = vAvailableIndices.back();
+            vAvailableIndices[randi] = vAvailableIndices.back();
             vAvailableIndices.pop_back();
         }
 

src/System.cc

@@ -25,6 +25,12 @@
 #include <thread>
 #include <pangolin/pangolin.h>
 #include <iomanip>
+#include <time.h>
+
+bool has_suffix(const std::string &str, const std::string &suffix) {
+  std::size_t index = str.find(suffix, str.size() - suffix.size());
+  return (index != std::string::npos);
+}
 
 namespace ORB_SLAM2
 {
@@ -60,16 +66,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 (has_suffix(strVocFile, ".txt"))
+	  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);

tools/bin_vocabulary.cc

@@ -0,0 +1,52 @@
+#include <time.h>
+
+#include "ORBVocabulary.h"
+using namespace std;
+
+bool load_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) {
+  clock_t tStart = clock();
+  bool res = voc->loadFromTextFile(infile);
+  printf("Loading fom text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
+  return res;
+}
+
+void load_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) {
+  clock_t tStart = clock();
+  voc->load(infile);
+  printf("Loading fom xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
+}
+
+void load_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string infile) {
+  clock_t tStart = clock();
+  voc->loadFromBinaryFile(infile);
+  printf("Loading fom binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
+}
+
+void save_as_xml(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) {
+  clock_t tStart = clock();
+  voc->save(outfile);
+  printf("Saving as xml: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
+}
+
+void save_as_text(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) {
+  clock_t tStart = clock();
+  voc->saveToTextFile(outfile);
+  printf("Saving as text: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
+}
+
+void save_as_binary(ORB_SLAM2::ORBVocabulary* voc, const std::string outfile) {
+  clock_t tStart = clock();
+  voc->saveToBinaryFile(outfile);
+  printf("Saving as binary: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
+}
+
+
+int main(int argc, char **argv) {
+  cout << "BoW load/save benchmark" << endl;
+  ORB_SLAM2::ORBVocabulary* voc = new ORB_SLAM2::ORBVocabulary();
+
+  load_as_text(voc, "Vocabulary/ORBvoc.txt");
+  save_as_binary(voc, "Vocabulary/ORBvoc.bin");
+
+  return 0;
+}