test binary read from raulmur/ORB_SLAM2#21

Build.sh

@@ -15,6 +15,5 @@ then
 fi
 
 ./Scripts/Build_ThirdParty.sh ${build_type}
-./Scripts/Build_Resources.sh ${build_type}
 ./Scripts/Build_Source.sh ${build_type}
-
+./Scripts/Build_Resources.sh ${build_type}

Scripts/Build_Resources.sh

@@ -29,3 +29,6 @@ pushd Build/${build_type}/Source/Resources > /dev/null
 cmake ${root_dir}/Source/Resources -DCMAKE_BUILD_TYPE=${build_type}
 make -j${num_jobs} install
 popd
+
+echo "Converting vocabulary to binary"
+./Tools/bin_vocabulary

Source/Libraries/ORB_SLAM2/include/ORBVocabulary.h

@@ -41,7 +41,22 @@ namespace ORB_SLAM2
 
 	void saveToTextFile(const std::string &filename) const;
 
-    private:
+
+//  https://github.com/raulmur/ORB_SLAM2/pull/21
+	/**
+   * 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;
+
+
+  private:
 
 	using F = DBoW2::FORB;
     };

Source/Libraries/ORB_SLAM2/src/ORBVocabulary.cc

@@ -122,5 +122,79 @@ bool ORBVocabulary::loadFromTextFile(const string &filename)
 
     f.close();
 }
-
+
+
+
+
+
+    bool ORBVocabulary::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;
+    }
+
+
+// --------------------------------------------------------------------------
+
+    void ORBVocabulary::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();
+    }
+
+// --------------------------------------------------------------------------
+
 }

Source/Libraries/ORB_SLAM2/src/System.cc

@@ -26,9 +26,18 @@
 #include <pangolin/pangolin.h>
 #include <iomanip>
 #include <chrono>
-
+#include <time.h>
 using namespace ::std;
 
+
+
+
+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
 {
 
@@ -63,17 +72,25 @@ 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 = 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;
         exit(-1);
     }
-    cout << "Vocabulary loaded!" << endl << endl;
-
+//    cout << "Vocabulary loaded!" << endl << endl;
+    printf("Vocabulary loaded in %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
     //Create KeyFrame Database
     mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
 

Source/Resources/CMakeLists.txt

@@ -5,3 +5,13 @@ project(Resources)
 set(BUILD_INSTALL_PREFIX ${Resources_SOURCE_DIR}/../../Develop/${CMAKE_BUILD_TYPE})
 
 add_subdirectory(Vocabulary)
+
+
+# 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} ${DBoW2_LIBS})
+
+target_include_directories(bin_vocabulary PUBLIC ${Resources_SOURCE_DIR}/../../Source/Libraries/ORB_SLAM2/include/)