success vo

.clang-format

@@ -1,7 +1,7 @@
 ---
 Language:        Cpp
 # BasedOnStyle:  Google
-AccessModifierOffset: -1
+AccessModifierOffset: -2
 AlignAfterOpenBracket: Align
 AlignConsecutiveMacros: false
 AlignConsecutiveAssignments: false
@@ -154,7 +154,7 @@ SpacesInAngles:  false
 SpacesInConditionalStatement: false
 SpacesInContainerLiterals: true
 SpacesInCStyleCastParentheses: false
-SpacesInParentheses: false
+SpacesInParentheses: true
 SpacesInSquareBrackets: false
 SpaceBeforeSquareBrackets: false
 Standard:        Auto

.gitignore

@@ -34,4 +34,5 @@
 build/
 .vscode/
 .idea/
-out/
+out/
+thirdparty/Vocabulary/ORBvoc.txt

CMakeLists.txt

@@ -12,10 +12,12 @@ include_directories(
         ${PROJECT_SOURCE_DIR}/tiny_ceres_solver/include
         ${PROJECT_SOURCE_DIR}/tiny_ceres_solver/internal
         ${PROJECT_SOURCE_DIR}/thirdparty/Sophus
+        ${PROJECT_SOURCE_DIR}/thirdparty/DBoW2
         ${PROJECT_SOURCE_DIR}
 )
 
 add_subdirectory(${PROJECT_SOURCE_DIR}/thirdparty/backward-cpp)
+add_subdirectory(${PROJECT_SOURCE_DIR}/thirdparty/DBoW2)
 
 find_package(OpenCV 4 REQUIRED)
 include_directories(${OpenCV_INCLUDE_DIRS})

thirdparty/DBoW2/CMakeLists.txt

@@ -0,0 +1,45 @@
+#cmake_minimum_required(VERSION 2.8)
+#project(DBoW2)
+#
+#if(NOT CMAKE_BUILD_TYPE)
+#  set(CMAKE_BUILD_TYPE Release)
+#endif()
+#
+#set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
+#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
+
+set(HDRS_DBOW2
+  DBoW2/BowVector.h
+  DBoW2/FORB.h 
+  DBoW2/FClass.h       
+  DBoW2/FeatureVector.h
+  DBoW2/ScoringObject.h   
+  DBoW2/TemplatedVocabulary.h)
+set(SRCS_DBOW2
+  DBoW2/BowVector.cpp
+  DBoW2/FORB.cpp      
+  DBoW2/FeatureVector.cpp
+  DBoW2/ScoringObject.cpp)
+
+set(HDRS_DUTILS
+  DUtils/Random.h
+  DUtils/Timestamp.h)
+set(SRCS_DUTILS
+  DUtils/Random.cpp
+  DUtils/Timestamp.cpp)
+
+
+find_package(OpenCV 4 QUIET)
+if(NOT OpenCV_FOUND)
+   find_package(OpenCV 3.0 QUIET)
+   if(NOT OpenCV_FOUND)
+      message(FATAL_ERROR "OpenCV > 3.0 not found.")
+   endif()
+endif()
+
+# set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+include_directories(${OpenCV_INCLUDE_DIRS})
+add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS})
+target_link_libraries(DBoW2 ${OpenCV_LIBS})
+

thirdparty/DBoW2/DBoW2/BowVector.cpp

@@ -0,0 +1,130 @@
+/**
+ * File: BowVector.cpp
+ * Date: March 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: bag of words vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <algorithm>
+#include <cmath>
+
+#include "BowVector.h"
+
+namespace DBoW2 {
+
+// --------------------------------------------------------------------------
+
+BowVector::BowVector(void)
+{
+}
+
+// --------------------------------------------------------------------------
+
+BowVector::~BowVector(void)
+{
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::addWeight(WordId id, WordValue v)
+{
+  BowVector::iterator vit = this->lower_bound(id);
+
+  if(vit != this->end() && !(this->key_comp()(id, vit->first)))
+  {
+    vit->second += v;
+  }
+  else
+  {
+    this->insert(vit, BowVector::value_type(id, v));
+  }
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::addIfNotExist(WordId id, WordValue v)
+{
+  BowVector::iterator vit = this->lower_bound(id);
+
+  if(vit == this->end() || (this->key_comp()(id, vit->first)))
+  {
+    this->insert(vit, BowVector::value_type(id, v));
+  }
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::normalize(LNorm norm_type)
+{
+  double norm = 0.0; 
+  BowVector::iterator it;
+
+  if(norm_type == DBoW2::L1)
+  {
+    for(it = begin(); it != end(); ++it)
+      norm += fabs(it->second);
+  }
+  else
+  {
+    for(it = begin(); it != end(); ++it)
+      norm += it->second * it->second;
+		norm = sqrt(norm);  
+  }
+
+  if(norm > 0.0)
+  {
+    for(it = begin(); it != end(); ++it)
+      it->second /= norm;
+  }
+}
+
+// --------------------------------------------------------------------------
+
+std::ostream& operator<< (std::ostream &out, const BowVector &v)
+{
+  BowVector::const_iterator vit;
+  std::vector<unsigned int>::const_iterator iit;
+  unsigned int i = 0; 
+  const unsigned int N = v.size();
+  for(vit = v.begin(); vit != v.end(); ++vit, ++i)
+  {
+    out << "<" << vit->first << ", " << vit->second << ">";
+
+    if(i < N-1) out << ", ";
+  }
+  return out;
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::saveM(const std::string &filename, size_t W) const
+{
+  std::fstream f(filename.c_str(), std::ios::out);
+
+  WordId last = 0;
+  BowVector::const_iterator bit;
+  for(bit = this->begin(); bit != this->end(); ++bit)
+  {
+    for(; last < bit->first; ++last)
+    {
+      f << "0 ";
+    }
+    f << bit->second << " ";
+
+    last = bit->first + 1;
+  }
+  for(; last < (WordId)W; ++last)
+    f << "0 ";
+
+  f.close();
+}
+
+// --------------------------------------------------------------------------
+
+} // namespace DBoW2
+

thirdparty/DBoW2/DBoW2/BowVector.h

@@ -0,0 +1,119 @@
+/**
+ * File: BowVector.h
+ * Date: March 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: bag of words vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_BOW_VECTOR__
+#define __D_T_BOW_VECTOR__
+
+#include <iostream>
+#include <map>
+#include <vector>
+
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/map.hpp>
+
+namespace DBoW2 {
+
+/// Id of words
+typedef unsigned int WordId;
+
+/// Value of a word
+typedef double WordValue;
+
+/// Id of nodes in the vocabulary treee
+typedef unsigned int NodeId;
+
+/// L-norms for normalization
+enum LNorm
+{
+  L1,
+  L2
+};
+
+/// Weighting type
+enum WeightingType
+{
+  TF_IDF,
+  TF,
+  IDF,
+  BINARY
+};
+
+/// Scoring type
+enum ScoringType
+{
+  L1_NORM,
+  L2_NORM,
+  CHI_SQUARE,
+  KL,
+  BHATTACHARYYA,
+  DOT_PRODUCT,
+};
+
+/// Vector of words to represent images
+class BowVector: 
+	public std::map<WordId, WordValue>
+{
+    friend class boost::serialization::access;
+    template<class Archive>
+    void serialize(Archive& ar, const int version)
+    {
+        ar & boost::serialization::base_object<std::map<WordId, WordValue> >(*this);
+    }
+
+public:
+
+	/** 
+	 * Constructor
+	 */
+	BowVector(void);
+
+	/**
+	 * Destructor
+	 */
+	~BowVector(void);
+
+	/**
+	 * Adds a value to a word value existing in the vector, or creates a new
+	 * word with the given value
+	 * @param id word id to look for
+	 * @param v value to create the word with, or to add to existing word
+	 */
+	void addWeight(WordId id, WordValue v);
+
+	/**
+	 * Adds a word with a value to the vector only if this does not exist yet
+	 * @param id word id to look for
+	 * @param v value to give to the word if this does not exist
+	 */
+	void addIfNotExist(WordId id, WordValue v);
+
+	/**
+	 * L1-Normalizes the values in the vector 
+	 * @param norm_type norm used
+	 */
+	void normalize(LNorm norm_type);
+
+	/**
+	 * Prints the content of the bow vector
+	 * @param out stream
+	 * @param v
+	 */
+	friend std::ostream& operator<<(std::ostream &out, const BowVector &v);
+
+	/**
+	 * Saves the bow vector as a vector in a matlab file
+	 * @param filename
+	 * @param W number of words in the vocabulary
+	 */
+	void saveM(const std::string &filename, size_t W) const;
+};
+
+} // namespace DBoW2
+
+#endif