fixed vector size mismatch between matched and status vectors

loop_fusion/src/ThirdParty/DBoW/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
+

loop_fusion/src/ThirdParty/DBoW/BowVector.h

@@ -0,0 +1,109 @@
+/**
+ * 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>
+
+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>
+{
+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

loop_fusion/src/ThirdParty/DBoW/DBoW2.h

@@ -0,0 +1,79 @@
+/*
+ * File: DBoW2.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: Generic include file for the DBoW2 classes and
+ *   the specialized vocabularies and databases
+ * License: see the LICENSE.txt file
+ *
+ */
+
+/*! \mainpage DBoW2 Library
+ *
+ * DBoW2 library for C++:
+ * Bag-of-word image database for image retrieval.
+ *
+ * Written by Dorian Galvez-Lopez,
+ * University of Zaragoza
+ * 
+ * Check my website to obtain updates: http://doriangalvez.com
+ *
+ * \section requirements Requirements
+ * This library requires the DUtils, DUtilsCV, DVision and OpenCV libraries,
+ * as well as the boost::dynamic_bitset class.
+ *
+ * \section citation Citation
+ * If you use this software in academic works, please cite:
+ <pre>
+   @@ARTICLE{GalvezTRO12,
+    author={Galvez-Lopez, Dorian and Tardos, J. D.}, 
+    journal={IEEE Transactions on Robotics},
+    title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
+    year={2012},
+    month={October},
+    volume={28},
+    number={5},
+    pages={1188--1197},
+    doi={10.1109/TRO.2012.2197158},
+    ISSN={1552-3098}
+  }
+ </pre>
+ *
+ * \section license License
+ * This file is licensed under a Creative Commons 
+ * Attribution-NonCommercial-ShareAlike 3.0 license. 
+ * This file can be freely used and users can use, download and edit this file 
+ * provided that credit is attributed to the original author. No users are 
+ * permitted to use this file for commercial purposes unless explicit permission
+ * is given by the original author. Derivative works must be licensed using the
+ * same or similar license.
+ * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further
+ * details.
+ *
+ */
+
+#ifndef __D_T_DBOW2__
+#define __D_T_DBOW2__
+
+/// Includes all the data structures to manage vocabularies and image databases
+namespace DBoW2
+{
+}
+
+#include "TemplatedVocabulary.h"
+#include "TemplatedDatabase.h"
+#include "BowVector.h"
+#include "FeatureVector.h"
+#include "QueryResults.h"
+#include "FBrief.h"
+
+/// BRIEF Vocabulary
+typedef DBoW2::TemplatedVocabulary<DBoW2::FBrief::TDescriptor, DBoW2::FBrief> 
+  BriefVocabulary;
+
+/// BRIEF Database
+typedef DBoW2::TemplatedDatabase<DBoW2::FBrief::TDescriptor, DBoW2::FBrief> 
+  BriefDatabase;
+
+#endif
+

loop_fusion/src/ThirdParty/DBoW/FBrief.cpp

@@ -0,0 +1,108 @@
+/**
+ * File: FBrief.cpp
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: functions for BRIEF descriptors
+ * License: see the LICENSE.txt file
+ *
+ */
+ 
+#include <vector>
+#include <string>
+#include <sstream>
+
+#include "FBrief.h"
+
+using namespace std;
+
+namespace DBoW2 {
+
+// --------------------------------------------------------------------------
+
+void FBrief::meanValue(const std::vector<FBrief::pDescriptor> &descriptors, 
+  FBrief::TDescriptor &mean)
+{
+  mean.reset();
+  
+  if(descriptors.empty()) return;
+  
+  const int N2 = descriptors.size() / 2;
+  const int L = descriptors[0]->size();
+  
+  vector<int> counters(L, 0);
+
+  vector<FBrief::pDescriptor>::const_iterator it;
+  for(it = descriptors.begin(); it != descriptors.end(); ++it)
+  {
+    const FBrief::TDescriptor &desc = **it;
+    for(int i = 0; i < L; ++i)
+    {
+      if(desc[i]) counters[i]++;
+    }
+  }
+  
+  for(int i = 0; i < L; ++i)
+  {
+    if(counters[i] > N2) mean.set(i);
+  }
+  
+}
+
+// --------------------------------------------------------------------------
+  
+double FBrief::distance(const FBrief::TDescriptor &a, 
+  const FBrief::TDescriptor &b)
+{
+  return (double)DVision::BRIEF::distance(a, b);
+}
+
+// --------------------------------------------------------------------------
+  
+std::string FBrief::toString(const FBrief::TDescriptor &a)
+{
+  // from boost::bitset
+  string s;
+  to_string(a, s); // reversed
+  return s;
+}
+
+// --------------------------------------------------------------------------
+  
+void FBrief::fromString(FBrief::TDescriptor &a, const std::string &s)
+{
+  // from boost::bitset
+  stringstream ss(s);
+  ss >> a;
+}
+
+// --------------------------------------------------------------------------
+
+void FBrief::toMat32F(const std::vector<TDescriptor> &descriptors, 
+  cv::Mat &mat)
+{
+  if(descriptors.empty())
+  {
+    mat.release();
+    return;
+  }
+  
+  const int N = descriptors.size();
+  const int L = descriptors[0].size();
+  
+  mat.create(N, L, CV_32F);
+  
+  for(int i = 0; i < N; ++i)
+  {
+    const TDescriptor& desc = descriptors[i];
+    float *p = mat.ptr<float>(i);
+    for(int j = 0; j < L; ++j, ++p)
+    {
+      *p = (desc[j] ? 1 : 0);
+    }
+  } 
+}
+
+// --------------------------------------------------------------------------
+
+} // namespace DBoW2
+

loop_fusion/src/ThirdParty/DBoW/FBrief.h

@@ -0,0 +1,73 @@
+/**
+ * File: FBrief.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: functions for BRIEF descriptors
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_F_BRIEF__
+#define __D_T_F_BRIEF__
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <string>
+
+#include "FClass.h"
+#include "../DVision/DVision.h"
+
+namespace DBoW2 {
+
+/// Functions to manipulate BRIEF descriptors
+class FBrief: protected FClass
+{
+public:
+
+  typedef DVision::BRIEF::bitset TDescriptor;
+  typedef const TDescriptor *pDescriptor;
+
+  /**
+   * Calculates the mean value of a set of descriptors
+   * @param descriptors
+   * @param mean mean descriptor
+   */
+  static void meanValue(const std::vector<pDescriptor> &descriptors, 
+    TDescriptor &mean);
+  
+  /**
+   * Calculates the distance between two descriptors
+   * @param a
+   * @param b
+   * @return distance
+   */
+  static double distance(const TDescriptor &a, const TDescriptor &b);
+  
+  /**
+   * Returns a string version of the descriptor
+   * @param a descriptor
+   * @return string version
+   */
+  static std::string toString(const TDescriptor &a);
+  
+  /**
+   * Returns a descriptor from a string
+   * @param a descriptor
+   * @param s string version
+   */
+  static void fromString(TDescriptor &a, const std::string &s);
+  
+  /**
+   * Returns a mat with the descriptors in float format
+   * @param descriptors
+   * @param mat (out) NxL 32F matrix
+   */
+  static void toMat32F(const std::vector<TDescriptor> &descriptors, 
+    cv::Mat &mat);
+
+};
+
+} // namespace DBoW2
+
+#endif
+

loop_fusion/src/ThirdParty/DBoW/FClass.h

@@ -0,0 +1,71 @@
+/**
+ * File: FClass.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: generic FClass to instantiate templated classes
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_FCLASS__
+#define __D_T_FCLASS__
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <string>
+
+namespace DBoW2 {
+
+/// Generic class to encapsulate functions to manage descriptors.
+/**
+ * This class must be inherited. Derived classes can be used as the
+ * parameter F when creating Templated structures
+ * (TemplatedVocabulary, TemplatedDatabase, ...)
+ */
+class FClass
+{
+  class TDescriptor;
+  typedef const TDescriptor *pDescriptor;
+  
+  /**
+   * Calculates the mean value of a set of descriptors
+   * @param descriptors
+   * @param mean mean descriptor
+   */
+  virtual void meanValue(const std::vector<pDescriptor> &descriptors, 
+    TDescriptor &mean) = 0;
+  
+  /**
+   * Calculates the distance between two descriptors
+   * @param a
+   * @param b
+   * @return distance
+   */
+  static double distance(const TDescriptor &a, const TDescriptor &b);
+  
+  /**
+   * Returns a string version of the descriptor
+   * @param a descriptor
+   * @return string version
+   */
+  static std::string toString(const TDescriptor &a);
+  
+  /**
+   * Returns a descriptor from a string
+   * @param a descriptor
+   * @param s string version
+   */
+  static void fromString(TDescriptor &a, const std::string &s);
+
+  /**
+   * Returns a mat with the descriptors in float format
+   * @param descriptors
+   * @param mat (out) NxL 32F matrix
+   */
+  static void toMat32F(const std::vector<TDescriptor> &descriptors, 
+    cv::Mat &mat);
+};
+
+} // namespace DBoW2
+
+#endif

loop_fusion/src/ThirdParty/DBoW/FeatureVector.cpp

@@ -0,0 +1,85 @@
+/**
+ * File: FeatureVector.cpp
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: feature vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include "FeatureVector.h"
+#include <map>
+#include <vector>
+#include <iostream>
+
+namespace DBoW2 {
+
+// ---------------------------------------------------------------------------
+
+FeatureVector::FeatureVector(void)
+{
+}
+
+// ---------------------------------------------------------------------------
+
+FeatureVector::~FeatureVector(void)
+{
+}
+
+// ---------------------------------------------------------------------------
+
+void FeatureVector::addFeature(NodeId id, unsigned int i_feature)
+{
+  FeatureVector::iterator vit = this->lower_bound(id);
+  
+  if(vit != this->end() && vit->first == id)
+  {
+    vit->second.push_back(i_feature);
+  }
+  else
+  {
+    vit = this->insert(vit, FeatureVector::value_type(id, 
+      std::vector<unsigned int>() ));
+    vit->second.push_back(i_feature);
+  }
+}
+
+// ---------------------------------------------------------------------------
+
+std::ostream& operator<<(std::ostream &out, 
+  const FeatureVector &v)
+{
+  if(!v.empty())
+  {
+    FeatureVector::const_iterator vit = v.begin();
+    
+    const std::vector<unsigned int>* f = &vit->second;
+
+    out << "<" << vit->first << ": [";
+    if(!f->empty()) out << (*f)[0];
+    for(unsigned int i = 1; i < f->size(); ++i)
+    {
+      out << ", " << (*f)[i];
+    }
+    out << "]>";
+    
+    for(++vit; vit != v.end(); ++vit)
+    {
+      f = &vit->second;
+      
+      out << ", <" << vit->first << ": [";
+      if(!f->empty()) out << (*f)[0];
+      for(unsigned int i = 1; i < f->size(); ++i)
+      {
+        out << ", " << (*f)[i];
+      }
+      out << "]>";
+    }
+  }
+  
+  return out;  
+}
+
+// ---------------------------------------------------------------------------
+
+} // namespace DBoW2

loop_fusion/src/ThirdParty/DBoW/FeatureVector.h

@@ -0,0 +1,56 @@
+/**
+ * File: FeatureVector.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: feature vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_FEATURE_VECTOR__
+#define __D_T_FEATURE_VECTOR__
+
+#include "BowVector.h"
+#include <map>
+#include <vector>
+#include <iostream>
+
+namespace DBoW2 {
+
+/// Vector of nodes with indexes of local features
+class FeatureVector: 
+  public std::map<NodeId, std::vector<unsigned int> >
+{
+public:
+
+  /**
+   * Constructor
+   */
+  FeatureVector(void);
+  
+  /**
+   * Destructor
+   */
+  ~FeatureVector(void);
+  
+  /**
+   * Adds a feature to an existing node, or adds a new node with an initial
+   * feature
+   * @param id node id to add or to modify
+   * @param i_feature index of feature to add to the given node
+   */
+  void addFeature(NodeId id, unsigned int i_feature);
+
+  /**
+   * Sends a string versions of the feature vector through the stream
+   * @param out stream
+   * @param v feature vector
+   */
+  friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v);
+    
+};
+
+} // namespace DBoW2
+
+#endif
+

loop_fusion/src/ThirdParty/DBoW/QueryResults.cpp

@@ -0,0 +1,63 @@
+/**
+ * File: QueryResults.cpp
+ * Date: March, November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: structure to store results of database queries
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <iostream>
+#include <fstream>
+#include "QueryResults.h"
+
+using namespace std;
+
+namespace DBoW2
+{
+
+// ---------------------------------------------------------------------------
+
+ostream & operator<<(ostream& os, const Result& ret )
+{
+  os << "<EntryId: " << ret.Id << ", Score: " << ret.Score << ">";
+  return os;
+}
+
+// ---------------------------------------------------------------------------
+
+ostream & operator<<(ostream& os, const QueryResults& ret )
+{
+  if(ret.size() == 1)
+    os << "1 result:" << endl;
+  else
+    os << ret.size() << " results:" << endl;
+    
+  QueryResults::const_iterator rit;
+  for(rit = ret.begin(); rit != ret.end(); ++rit)
+  {
+    os << *rit;
+    if(rit + 1 != ret.end()) os << endl;
+  }
+  return os;
+}
+
+// ---------------------------------------------------------------------------
+
+void QueryResults::saveM(const std::string &filename) const
+{
+  fstream f(filename.c_str(), ios::out);
+  
+  QueryResults::const_iterator qit;
+  for(qit = begin(); qit != end(); ++qit)
+  {
+    f << qit->Id << " " << qit->Score << endl;
+  }
+  
+  f.close();
+}
+
+// ---------------------------------------------------------------------------
+
+} // namespace DBoW2
+

loop_fusion/src/ThirdParty/DBoW/QueryResults.h

@@ -0,0 +1,205 @@
+/**
+ * File: QueryResults.h
+ * Date: March, November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: structure to store results of database queries
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_QUERY_RESULTS__
+#define __D_T_QUERY_RESULTS__
+
+#include <vector>
+
+namespace DBoW2 {
+
+/// Id of entries of the database
+typedef unsigned int EntryId;
+
+/// Single result of a query
+class Result
+{
+public:
+  
+  /// Entry id
+  EntryId Id;
+  
+  /// Score obtained
+  double Score;
+  
+  /// debug
+  int nWords; // words in common
+  // !!! this is filled only by Bhatt score!
+  // (and for BCMatching, BCThresholding then)
+  
+  double bhatScore, chiScore;
+  /// debug
+  
+  // only done by ChiSq and BCThresholding 
+  double sumCommonVi;
+  double sumCommonWi;
+  double expectedChiScore;
+  /// debug
+
+  /**
+   * Empty constructors
+   */
+  inline Result(){}
+  
+  /**
+   * Creates a result with the given data
+   * @param _id entry id
+   * @param _score score
+   */
+  inline Result(EntryId _id, double _score): Id(_id), Score(_score){}
+
+  /**
+   * Compares the scores of two results
+   * @return true iff this.score < r.score
+   */
+  inline bool operator<(const Result &r) const
+  {
+    return this->Score < r.Score;
+  }
+
+  /**
+   * Compares the scores of two results
+   * @return true iff this.score > r.score
+   */
+  inline bool operator>(const Result &r) const
+  {
+    return this->Score > r.Score;
+  }
+
+  /**
+   * Compares the entry id of the result
+   * @return true iff this.id == id
+   */
+  inline bool operator==(EntryId id) const
+  {
+    return this->Id == id;
+  }
+  
+  /**
+   * Compares the score of this entry with a given one
+   * @param s score to compare with
+   * @return true iff this score < s
+   */
+  inline bool operator<(double s) const
+  {
+    return this->Score < s;
+  }
+  
+  /**
+   * Compares the score of this entry with a given one
+   * @param s score to compare with
+   * @return true iff this score > s
+   */
+  inline bool operator>(double s) const
+  {
+    return this->Score > s;
+  }
+  
+  /**
+   * Compares the score of two results
+   * @param a
+   * @param b
+   * @return true iff a.Score > b.Score
+   */
+  static inline bool gt(const Result &a, const Result &b)
+  {
+    return a.Score > b.Score;
+  }
+  
+  /**
+   * Compares the scores of two results
+   * @return true iff a.Score > b.Score
+   */
+  inline static bool ge(const Result &a, const Result &b)
+  {
+    return a.Score > b.Score;
+  }
+  
+  /**
+   * Returns true iff a.Score >= b.Score
+   * @param a
+   * @param b
+   * @return true iff a.Score >= b.Score
+   */
+  static inline bool geq(const Result &a, const Result &b)
+  {
+    return a.Score >= b.Score;
+  }
+  
+  /**
+   * Returns true iff a.Score >= s
+   * @param a
+   * @param s
+   * @return true iff a.Score >= s
+   */
+  static inline bool geqv(const Result &a, double s)
+  {
+    return a.Score >= s;
+  }
+  
+  
+  /**
+   * Returns true iff a.Id < b.Id
+   * @param a
+   * @param b
+   * @return true iff a.Id < b.Id
+   */
+  static inline bool ltId(const Result &a, const Result &b)
+  {
+    return a.Id < b.Id;
+  }
+  
+  /**
+   * Prints a string version of the result
+   * @param os ostream
+   * @param ret Result to print
+   */
+  friend std::ostream & operator<<(std::ostream& os, const Result& ret );
+};
+
+/// Multiple results from a query
+class QueryResults: public std::vector<Result>
+{
+public:
+
+  /** 
+   * Multiplies all the scores in the vector by factor
+   * @param factor
+   */
+  inline void scaleScores(double factor);
+  
+  /**
+   * Prints a string version of the results
+   * @param os ostream
+   * @param ret QueryResults to print
+   */
+  friend std::ostream & operator<<(std::ostream& os, const QueryResults& ret );
+  
+  /**
+   * Saves a matlab file with the results 
+   * @param filename 
+   */
+  void saveM(const std::string &filename) const;
+  
+};
+
+// --------------------------------------------------------------------------
+
+inline void QueryResults::scaleScores(double factor)
+{
+  for(QueryResults::iterator qit = begin(); qit != end(); ++qit) 
+    qit->Score *= factor;
+}
+
+// --------------------------------------------------------------------------
+
+} // namespace TemplatedBoW
+  
+#endif
+

loop_fusion/src/ThirdParty/DBoW/ScoringObject.cpp

@@ -0,0 +1,315 @@
+/**
+ * File: ScoringObject.cpp
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: functions to compute bow scores 
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <cfloat>
+#include "TemplatedVocabulary.h"
+#include "BowVector.h"
+
+using namespace DBoW2;
+
+// If you change the type of WordValue, make sure you change also the
+// epsilon value (this is needed by the KL method)
+const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double L1Scoring::score(const BowVector &v1, const BowVector &v2) const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      score += fabs(vi - wi) - fabs(vi) - fabs(wi);
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+      // v1_it = (first element >= v2_it.id)
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+  
+  // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) 
+  //		for all i | v_i != 0 and w_i != 0 
+  // (Nister, 2006)
+  // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1}
+  score = -score/2.0;
+
+  return score; // [0..1]
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double L2Scoring::score(const BowVector &v1, const BowVector &v2) const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      score += vi * wi;
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+      // v1_it = (first element >= v2_it.id)
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+  
+  // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) )
+	//		for all i | v_i != 0 and w_i != 0 )
+	// (Nister, 2006)
+	if(score >= 1) // rounding errors
+	  score = 1.0;
+	else
+    score = 1.0 - sqrt(1.0 - score); // [0..1]
+
+  return score;
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) 
+  const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  // all the items are taken into account
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      // (v-w)^2/(v+w) - v - w = -4 vw/(v+w)
+      // we move the -4 out
+      if(vi + wi != 0.0) score += vi * wi / (vi + wi);
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+    }
+  }
+    
+  // this takes the -4 into account
+  score = 2. * score; // [0..1]
+
+  return score;
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double KLScoring::score(const BowVector &v1, const BowVector &v2) const
+{ 
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  // all the items or v are taken into account
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      if(vi != 0 && wi != 0) score += vi * log(vi/wi);
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      score += vi * (log(vi) - LOG_EPS);
+      ++v1_it;
+    }
+    else
+    {
+      // move v2_it forward, do not add any score
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+  
+  // sum rest of items of v
+  for(; v1_it != v1_end; ++v1_it) 
+    if(v1_it->second != 0)
+      score += v1_it->second * (log(v1_it->second) - LOG_EPS);
+  
+  return score; // cannot be scaled
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double BhattacharyyaScoring::score(const BowVector &v1, 
+  const BowVector &v2) const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      score += sqrt(vi * wi);
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+      // v1_it = (first element >= v2_it.id)
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+
+  return score; // already scaled
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double DotProductScoring::score(const BowVector &v1, 
+  const BowVector &v2) const
+{
+  BowVector::const_iterator v1_it, v2_it;
+  const BowVector::const_iterator v1_end = v1.end();
+  const BowVector::const_iterator v2_end = v2.end();
+  
+  v1_it = v1.begin();
+  v2_it = v2.begin();
+  
+  double score = 0;
+  
+  while(v1_it != v1_end && v2_it != v2_end)
+  {
+    const WordValue& vi = v1_it->second;
+    const WordValue& wi = v2_it->second;
+    
+    if(v1_it->first == v2_it->first)
+    {
+      score += vi * wi;
+      
+      // move v1 and v2 forward
+      ++v1_it;
+      ++v2_it;
+    }
+    else if(v1_it->first < v2_it->first)
+    {
+      // move v1 forward
+      v1_it = v1.lower_bound(v2_it->first);
+      // v1_it = (first element >= v2_it.id)
+    }
+    else
+    {
+      // move v2 forward
+      v2_it = v2.lower_bound(v1_it->first);
+      // v2_it = (first element >= v1_it.id)
+    }
+  }
+
+  return score; // cannot scale
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+

loop_fusion/src/ThirdParty/DBoW/ScoringObject.h

@@ -0,0 +1,95 @@
+/**
+ * File: ScoringObject.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: functions to compute bow scores 
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_SCORING_OBJECT__
+#define __D_T_SCORING_OBJECT__
+
+#include "BowVector.h"
+
+namespace DBoW2 {
+
+/// Base class of scoring functions
+class GeneralScoring
+{
+public:
+  /**
+   * Computes the score between two vectors. Vectors must be sorted and 
+   * normalized if necessary
+   * @param v (in/out)
+   * @param w (in/out)
+   * @return score
+   */
+  virtual double score(const BowVector &v, const BowVector &w) const = 0;
+
+  /**
+   * Returns whether a vector must be normalized before scoring according
+   * to the scoring scheme
+   * @param norm norm to use
+   * @return true iff must normalize
+   */
+  virtual bool mustNormalize(LNorm &norm) const = 0;
+
+  /// Log of epsilon
+	static const double LOG_EPS; 
+  // If you change the type of WordValue, make sure you change also the
+	// epsilon value (this is needed by the KL method)
+	
+  virtual ~GeneralScoring() {} //!< Required for virtual base classes	
+};
+
+/** 
+ * Macro for defining Scoring classes
+ * @param NAME name of class
+ * @param MUSTNORMALIZE if vectors must be normalized to compute the score
+ * @param NORM type of norm to use when MUSTNORMALIZE
+ */
+#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \
+  NAME: public GeneralScoring \
+  { public: \
+    /** \
+     * Computes score between two vectors \
+     * @param v \
+     * @param w \
+     * @return score between v and w \
+     */ \
+    virtual double score(const BowVector &v, const BowVector &w) const; \
+    \
+    /** \
+     * Says if a vector must be normalized according to the scoring function \
+     * @param norm (out) if true, norm to use
+     * @return true iff vectors must be normalized \
+     */ \
+    virtual inline bool mustNormalize(LNorm &norm) const  \
+      { norm = NORM; return MUSTNORMALIZE; } \
+  }
+  
+/// L1 Scoring object
+class __SCORING_CLASS(L1Scoring, true, L1);
+
+/// L2 Scoring object
+class __SCORING_CLASS(L2Scoring, true, L2);
+
+/// Chi square Scoring object
+class __SCORING_CLASS(ChiSquareScoring, true, L1);
+
+/// KL divergence Scoring object
+class __SCORING_CLASS(KLScoring, true, L1);
+
+/// Bhattacharyya Scoring object
+class __SCORING_CLASS(BhattacharyyaScoring, true, L1);
+
+/// Dot product Scoring object
+class __SCORING_CLASS(DotProductScoring, false, L1);
+
+#undef __SCORING_CLASS
+  
+} // namespace DBoW2
+
+#endif
+

loop_fusion/src/ThirdParty/DUtils/DException.h

@@ -0,0 +1,65 @@
+/*	
+ * File: DException.h
+ * Project: DUtils library
+ * Author: Dorian Galvez-Lopez
+ * Date: October 6, 2009
+ * Description: general exception of the library
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#pragma once
+
+#ifndef __D_EXCEPTION__
+#define __D_EXCEPTION__
+
+#include <stdexcept>
+#include <string>
+using namespace std;
+
+namespace DUtils {
+
+/// General exception
+class DException :
+	public exception
+{
+public:
+	/**
+	 * Creates an exception with a general error message
+	 */
+	DException(void) throw(): m_message("DUtils exception"){}
+
+	/**
+	 * Creates an exception with a custom error message
+	 * @param msg: message
+	 */
+	DException(const char *msg) throw(): m_message(msg){}
+	
+	/**
+	 * Creates an exception with a custom error message
+	 * @param msg: message
+	 */
+	DException(const string &msg) throw(): m_message(msg){}
+
+  /**
+	 * Destructor
+	 */
+	virtual ~DException(void) throw(){}
+
+	/**
+	 * Returns the exception message
+	 */
+	virtual const char* what() const throw()
+	{
+		return m_message.c_str();
+	}
+
+protected:
+  /// Error message
+	string m_message;
+};
+
+}
+
+#endif
+

loop_fusion/src/ThirdParty/DUtils/DUtils.h

@@ -0,0 +1,48 @@
+/*
+ * File: DUtils.h
+ * Project: DUtils library
+ * Author: Dorian Galvez-Lopez
+ * Date: October 6, 2009
+ * Description: include file for including all the library functionalities
+ * License: see the LICENSE.txt file
+ *
+ */
+
+/*! \mainpage DUtils Library
+ *
+ * DUtils library for C++:
+ * Collection of classes with general utilities for C++ applications.
+ *
+ * Written by Dorian Galvez-Lopez,
+ * University of Zaragoza
+ * 
+ * Check my website to obtain updates: http://webdiis.unizar.es/~dorian
+ *
+ * \section license License
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License (LGPL) as 
+ * published by the Free Software Foundation, either version 3 of the License, 
+ * or any later version.
+ *
+ */
+
+
+#pragma once
+
+#ifndef __D_UTILS__
+#define __D_UTILS__
+
+/// Several utilities for C++ programs
+namespace DUtils
+{
+}
+
+// Exception
+#include "DException.h"
+
+#include "Timestamp.h"
+// Random numbers
+#include "Random.h"
+
+
+#endif

loop_fusion/src/ThirdParty/DUtils/Random.cpp

@@ -0,0 +1,129 @@
+/*	
+ * File: Random.cpp
+ * Project: DUtils library
+ * Author: Dorian Galvez-Lopez
+ * Date: April 2010
+ * Description: manages pseudo-random numbers
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include "Random.h"
+#include "Timestamp.h"
+#include <cstdlib>
+using namespace std;
+
+bool DUtils::Random::m_already_seeded = false;
+
+void DUtils::Random::SeedRand(){
+	Timestamp time;
+	time.setToCurrentTime();
+	srand((unsigned)time.getFloatTime()); 
+}
+
+void DUtils::Random::SeedRandOnce()
+{
+  if(!m_already_seeded)
+  {
+    DUtils::Random::SeedRand();
+    m_already_seeded = true;
+  }
+}
+
+void DUtils::Random::SeedRand(int seed)
+{
+	srand(seed); 
+}
+
+void DUtils::Random::SeedRandOnce(int seed)
+{
+  if(!m_already_seeded)
+  {
+    DUtils::Random::SeedRand(seed);
+    m_already_seeded = true;
+  }
+}
+
+int DUtils::Random::RandomInt(int min, int max){
+	int d = max - min + 1;
+	return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min;
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max)
+{
+  if(min <= max)
+  {
+    m_min = min;
+    m_max = max;
+  }
+  else
+  {
+    m_min = max;
+    m_max = min;
+  }
+
+  createValues();
+}
+
+// ---------------------------------------------------------------------------
+
+DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer
+  (const DUtils::Random::UnrepeatedRandomizer& rnd)
+{
+  *this = rnd;
+}
+
+// ---------------------------------------------------------------------------
+
+int DUtils::Random::UnrepeatedRandomizer::get()
+{
+  if(empty()) createValues();
+  
+  DUtils::Random::SeedRandOnce();
+  
+  int k = DUtils::Random::RandomInt(0, m_values.size()-1);
+  int ret = m_values[k];
+  m_values[k] = m_values.back();
+  m_values.pop_back();
+  
+  return ret;
+}
+
+// ---------------------------------------------------------------------------
+
+void DUtils::Random::UnrepeatedRandomizer::createValues()
+{
+  int n = m_max - m_min + 1;
+  
+  m_values.resize(n);
+  for(int i = 0; i < n; ++i) m_values[i] = m_min + i;
+}
+
+// ---------------------------------------------------------------------------
+
+void DUtils::Random::UnrepeatedRandomizer::reset()
+{
+  if((int)m_values.size() != m_max - m_min + 1) createValues();
+}
+
+// ---------------------------------------------------------------------------
+
+DUtils::Random::UnrepeatedRandomizer& 
+DUtils::Random::UnrepeatedRandomizer::operator=
+  (const DUtils::Random::UnrepeatedRandomizer& rnd)
+{
+  if(this != &rnd)
+  {
+    this->m_min = rnd.m_min;
+    this->m_max = rnd.m_max;
+    this->m_values = rnd.m_values;
+  }
+  return *this;
+}
+
+// ---------------------------------------------------------------------------
+
+

loop_fusion/src/ThirdParty/DUtils/Random.h

@@ -0,0 +1,184 @@
+/*	
+ * File: Random.h
+ * Project: DUtils library
+ * Author: Dorian Galvez-Lopez
+ * Date: April 2010, November 2011
+ * Description: manages pseudo-random numbers
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#pragma once
+#ifndef __D_RANDOM__
+#define __D_RANDOM__
+
+#include <cstdlib>
+#include <vector>
+
+namespace DUtils {
+
+/// Functions to generate pseudo-random numbers
+class Random
+{
+public:
+  class UnrepeatedRandomizer;
+  
+public:
+	/**
+	 * Sets the random number seed to the current time
+	 */
+	static void SeedRand();
+	
+	/**
+	 * Sets the random number seed to the current time only the first
+	 * time this function is called
+	 */
+	static void SeedRandOnce();
+
+	/** 
+	 * Sets the given random number seed
+	 * @param seed
+	 */
+	static void SeedRand(int seed);
+
+	/** 
+	 * Sets the given random number seed only the first time this function 
+	 * is called
+	 * @param seed
+	 */
+	static void SeedRandOnce(int seed);
+
+	/**
+	 * Returns a random number in the range [0..1]
+	 * @return random T number in [0..1]
+	 */
+	template <class T>
+	static T RandomValue(){
+		return (T)rand()/(T)RAND_MAX;
+	}
+
+	/**
+	 * Returns a random number in the range [min..max]
+	 * @param min
+	 * @param max
+	 * @return random T number in [min..max]
+	 */
+	template <class T>
+	static T RandomValue(T min, T max){
+		return Random::RandomValue<T>() * (max - min) + min;
+	}
+
+	/**
+	 * Returns a random int in the range [min..max]
+	 * @param min
+	 * @param max
+	 * @return random int in [min..max]
+	 */
+	static int RandomInt(int min, int max);
+	
+	/** 
+	 * Returns a random number from a gaussian distribution
+	 * @param mean
+	 * @param sigma standard deviation
+	 */
+	template <class T>
+	static T RandomGaussianValue(T mean, T sigma)
+	{
+    // Box-Muller transformation
+    T x1, x2, w, y1;
+
+    do {
+      x1 = (T)2. * RandomValue<T>() - (T)1.;
+      x2 = (T)2. * RandomValue<T>() - (T)1.;
+      w = x1 * x1 + x2 * x2;
+    } while ( w >= (T)1. || w == (T)0. );
+
+    w = sqrt( ((T)-2.0 * log( w ) ) / w );
+    y1 = x1 * w;
+
+    return( mean + y1 * sigma );
+	}
+
+private:
+
+  /// If SeedRandOnce() or SeedRandOnce(int) have already been called
+  static bool m_already_seeded;
+  
+};
+
+// ---------------------------------------------------------------------------
+
+/// Provides pseudo-random numbers with no repetitions
+class Random::UnrepeatedRandomizer
+{
+public:
+
+  /** 
+   * Creates a randomizer that returns numbers in the range [min, max]
+   * @param min
+   * @param max
+   */
+  UnrepeatedRandomizer(int min, int max);
+  ~UnrepeatedRandomizer(){}
+  
+  /**
+   * Copies a randomizer
+   * @param rnd
+   */
+  UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd);
+  
+  /**
+   * Copies a randomizer
+   * @param rnd
+   */
+  UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd);
+  
+  /** 
+   * Returns a random number not given before. If all the possible values
+   * were already given, the process starts again
+   * @return unrepeated random number
+   */
+  int get();
+  
+  /**
+   * Returns whether all the possible values between min and max were
+   * already given. If get() is called when empty() is true, the behaviour
+   * is the same than after creating the randomizer
+   * @return true iff all the values were returned
+   */
+  inline bool empty() const { return m_values.empty(); }
+  
+  /**
+   * Returns the number of values still to be returned
+   * @return amount of values to return
+   */
+  inline unsigned int left() const { return m_values.size(); }
+  
+  /**
+   * Resets the randomizer as it were just created
+   */
+  void reset();
+  
+protected:
+
+  /** 
+   * Creates the vector with available values
+   */
+  void createValues();
+
+protected:
+
+  /// Min of range of values
+  int m_min;
+  /// Max of range of values
+  int m_max;
+
+  /// Available values
+  std::vector<int> m_values;
+
+};
+
+}
+
+#endif
+

loop_fusion/src/ThirdParty/DUtils/Timestamp.cpp

@@ -0,0 +1,246 @@
+/*
+ * File: Timestamp.cpp
+ * Author: Dorian Galvez-Lopez
+ * Date: March 2009
+ * Description: timestamping functions
+ * 
+ * Note: in windows, this class has a 1ms resolution
+ *
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <cstdio>
+#include <cstdlib>
+#include <ctime>
+#include <cmath>
+#include <sstream>
+#include <iomanip>
+
+#ifdef _WIN32
+#ifndef WIN32
+#define WIN32
+#endif
+#endif
+
+#ifdef WIN32
+#include <sys/timeb.h>
+#define sprintf sprintf_s
+#else
+#include <sys/time.h>
+#endif
+
+#include "Timestamp.h"
+
+using namespace std;
+
+using namespace DUtils;
+
+Timestamp::Timestamp(Timestamp::tOptions option)
+{
+  if(option & CURRENT_TIME)
+    setToCurrentTime();
+  else if(option & ZERO)
+    setTime(0.);
+}
+
+Timestamp::~Timestamp(void)
+{
+}
+
+bool Timestamp::empty() const
+{
+  return m_secs == 0 && m_usecs == 0;
+}
+
+void Timestamp::setToCurrentTime(){
+	
+#ifdef WIN32
+	struct __timeb32 timebuffer;
+	_ftime32_s( &timebuffer ); // C4996
+	// Note: _ftime is deprecated; consider using _ftime_s instead
+	m_secs = timebuffer.time;
+	m_usecs = timebuffer.millitm * 1000;
+#else
+	struct timeval now;
+	gettimeofday(&now, NULL);
+	m_secs = now.tv_sec;
+	m_usecs = now.tv_usec;
+#endif
+
+}
+
+void Timestamp::setTime(const string &stime){
+	string::size_type p = stime.find('.');
+	if(p == string::npos){
+		m_secs = atol(stime.c_str());
+		m_usecs = 0;
+	}else{
+		m_secs = atol(stime.substr(0, p).c_str());
+		
+		string s_usecs = stime.substr(p+1, 6);
+		m_usecs = atol(stime.substr(p+1).c_str());
+		m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length()));
+	}
+}
+
+void Timestamp::setTime(double s)
+{
+  m_secs = (unsigned long)s;
+  m_usecs = (s - (double)m_secs) * 1e6;
+}
+
+double Timestamp::getFloatTime() const {
+	return double(m_secs) + double(m_usecs)/1000000.0;
+}
+
+string Timestamp::getStringTime() const {
+	char buf[32];
+	sprintf(buf, "%.6lf", this->getFloatTime());
+	return string(buf);
+}
+
+double Timestamp::operator- (const Timestamp &t) const {
+	return this->getFloatTime() - t.getFloatTime();
+}
+
+Timestamp& Timestamp::operator+= (double s)
+{
+  *this = *this + s;
+  return *this;
+}
+
+Timestamp& Timestamp::operator-= (double s)
+{
+  *this = *this - s;
+  return *this;
+}
+
+Timestamp Timestamp::operator+ (double s) const
+{
+	unsigned long secs = (long)floor(s);
+	unsigned long usecs = (long)((s - (double)secs) * 1e6);
+
+  return this->plus(secs, usecs);
+}
+
+Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const
+{
+  Timestamp t;
+
+	const unsigned long max = 1000000ul;
+
+	if(m_usecs + usecs >= max)
+		t.setTime(m_secs + secs + 1, m_usecs + usecs - max);
+	else
+		t.setTime(m_secs + secs, m_usecs + usecs);
+	
+	return t;
+}
+
+Timestamp Timestamp::operator- (double s) const
+{
+	unsigned long secs = (long)floor(s);
+	unsigned long usecs = (long)((s - (double)secs) * 1e6);
+
+	return this->minus(secs, usecs);
+}
+
+Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const
+{
+  Timestamp t;
+
+	const unsigned long max = 1000000ul;
+
+	if(m_usecs < usecs)
+		t.setTime(m_secs - secs - 1, max - (usecs - m_usecs));
+	else
+		t.setTime(m_secs - secs, m_usecs - usecs);
+	
+	return t;
+}
+
+bool Timestamp::operator> (const Timestamp &t) const
+{
+	if(m_secs > t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs > t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator>= (const Timestamp &t) const
+{
+	if(m_secs > t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator< (const Timestamp &t) const
+{
+	if(m_secs < t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs < t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator<= (const Timestamp &t) const
+{
+	if(m_secs < t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator== (const Timestamp &t) const
+{
+	return(m_secs == t.m_secs && m_usecs == t.m_usecs);
+}
+
+
+string Timestamp::Format(bool machine_friendly) const 
+{
+  struct tm tm_time;
+
+  time_t t = (time_t)getFloatTime();
+
+#ifdef WIN32
+  localtime_s(&tm_time, &t);
+#else
+  localtime_r(&t, &tm_time);
+#endif
+  
+  char buffer[128];
+  
+  if(machine_friendly)
+  {
+    strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time);
+  }
+  else
+  {
+    strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001
+  }
+  
+  return string(buffer);
+}
+
+string Timestamp::Format(double s) {
+	int days = int(s / (24. * 3600.0));
+	s -= days * (24. * 3600.0);
+	int hours = int(s / 3600.0);
+	s -= hours * 3600;
+	int minutes = int(s / 60.0);
+	s -= minutes * 60;
+	int seconds = int(s);
+	int ms = int((s - seconds)*1e6);
+
+	stringstream ss;
+	ss.fill('0');
+	bool b;
+	if((b = (days > 0))) ss << days << "d ";
+	if((b = (b || hours > 0))) ss << setw(2) << hours << ":";
+	if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":";
+	if(b) ss << setw(2);
+	ss << seconds;
+	if(!b) ss << "." << setw(6) << ms;
+
+	return ss.str();
+}
+
+

loop_fusion/src/ThirdParty/DUtils/Timestamp.h

@@ -0,0 +1,204 @@
+/*
+ * File: Timestamp.h
+ * Author: Dorian Galvez-Lopez
+ * Date: March 2009
+ * Description: timestamping functions
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_TIMESTAMP__
+#define __D_TIMESTAMP__
+
+#include <iostream>
+using namespace std;
+
+namespace DUtils {
+
+/// Timestamp
+class Timestamp
+{
+public:
+
+  /// Options to initiate a timestamp
+  enum tOptions
+  {
+    NONE = 0,
+    CURRENT_TIME = 0x1,
+    ZERO = 0x2
+  };
+  
+public:
+  
+  /**
+   * Creates a timestamp 
+   * @param option option to set the initial time stamp
+   */
+	Timestamp(Timestamp::tOptions option = NONE);
+	
+	/**
+	 * Destructor
+	 */
+	virtual ~Timestamp(void);
+
+  /**
+   * Says if the timestamp is "empty": seconds and usecs are both 0, as 
+   * when initiated with the ZERO flag
+   * @return true iif secs == usecs == 0
+   */
+  bool empty() const;
+
+	/**
+	 * Sets this instance to the current time
+	 */
+	void setToCurrentTime();
+
+	/**
+	 * Sets the timestamp from seconds and microseconds
+	 * @param secs: seconds
+	 * @param usecs: microseconds
+	 */
+	inline void setTime(unsigned long secs, unsigned long usecs){
+		m_secs = secs;
+		m_usecs = usecs;
+	}
+	
+	/**
+	 * Returns the timestamp in seconds and microseconds
+	 * @param secs seconds
+	 * @param usecs microseconds
+	 */
+	inline void getTime(unsigned long &secs, unsigned long &usecs) const
+	{
+	  secs = m_secs;
+	  usecs = m_usecs;
+	}
+
+	/**
+	 * Sets the timestamp from a string with the time in seconds
+	 * @param stime: string such as "1235603336.036609"
+	 */
+	void setTime(const string &stime);
+	
+	/**
+	 * Sets the timestamp from a number of seconds from the epoch
+	 * @param s seconds from the epoch
+	 */
+	void setTime(double s);
+	
+	/**
+	 * Returns this timestamp as the number of seconds in (long) float format
+	 */
+	double getFloatTime() const;
+
+	/**
+	 * Returns this timestamp as the number of seconds in fixed length string format
+	 */
+	string getStringTime() const;
+
+	/**
+	 * Returns the difference in seconds between this timestamp (greater) and t (smaller)
+	 * If the order is swapped, a negative number is returned
+	 * @param t: timestamp to subtract from this timestamp
+	 * @return difference in seconds
+	 */
+	double operator- (const Timestamp &t) const;
+
+	/** 
+	 * Returns a copy of this timestamp + s seconds + us microseconds
+	 * @param s seconds
+	 * @param us microseconds
+	 */
+	Timestamp plus(unsigned long s, unsigned long us) const;
+
+  /**
+   * Returns a copy of this timestamp - s seconds - us microseconds
+   * @param s seconds
+   * @param us microseconds
+   */
+  Timestamp minus(unsigned long s, unsigned long us) const;
+
+  /**
+   * Adds s seconds to this timestamp and returns a reference to itself
+   * @param s seconds
+   * @return reference to this timestamp
+   */
+  Timestamp& operator+= (double s);
+  
+  /**
+   * Substracts s seconds to this timestamp and returns a reference to itself
+   * @param s seconds
+   * @return reference to this timestamp
+   */
+  Timestamp& operator-= (double s);
+
+	/**
+	 * Returns a copy of this timestamp + s seconds
+	 * @param s: seconds
+	 */
+	Timestamp operator+ (double s) const;
+
+	/**
+	 * Returns a copy of this timestamp - s seconds
+	 * @param s: seconds
+	 */
+	Timestamp operator- (double s) const;
+
+	/**
+	 * Returns whether this timestamp is at the future of t
+	 * @param t
+	 */
+	bool operator> (const Timestamp &t) const;
+
+	/**
+	 * Returns whether this timestamp is at the future of (or is the same as) t
+	 * @param t
+	 */
+	bool operator>= (const Timestamp &t) const;
+
+	/** 
+	 * Returns whether this timestamp and t represent the same instant
+	 * @param t
+	 */
+	bool operator== (const Timestamp &t) const;
+
+	/**
+	 * Returns whether this timestamp is at the past of t
+	 * @param t
+	 */
+	bool operator< (const Timestamp &t) const;
+
+	/**
+	 * Returns whether this timestamp is at the past of (or is the same as) t
+	 * @param t
+	 */
+	bool operator<= (const Timestamp &t) const;
+
+  /**
+   * Returns the timestamp in a human-readable string
+   * @param machine_friendly if true, the returned string is formatted
+   *   to yyyymmdd_hhmmss, without weekday or spaces
+   * @note This has not been tested under Windows
+   * @note The timestamp is truncated to seconds
+   */
+  string Format(bool machine_friendly = false) const;
+
+	/**
+	 * Returns a string version of the elapsed time in seconds, with the format
+	 * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us
+	 * @param s: elapsed seconds (given by getFloatTime) to format
+	 */
+	static string Format(double s);
+	
+
+protected:
+  /// Seconds
+	unsigned long m_secs;	// seconds
+	/// Microseconds
+	unsigned long m_usecs;	// microseconds
+};
+
+}
+
+#endif
+

loop_fusion/src/ThirdParty/DVision/BRIEF.cpp

@@ -0,0 +1,174 @@
+/**
+ * File: BRIEF.cpp
+ * Author: Dorian Galvez
+ * Date: September 2010
+ * Description: implementation of BRIEF (Binary Robust Independent 
+ *   Elementary Features) descriptor by 
+ *   Michael Calonder, Vincent Lepetitand Pascal Fua
+ *   + close binary tests (by Dorian Galvez-Lopez)
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include "BRIEF.h"
+#include "../DUtils/DUtils.h"
+#include <boost/dynamic_bitset.hpp>
+#include <vector>
+
+using namespace std;
+using namespace DVision;
+
+// ----------------------------------------------------------------------------
+
+BRIEF::BRIEF(int nbits, int patch_size, Type type):
+  m_bit_length(nbits), m_patch_size(patch_size), m_type(type)
+{
+  assert(patch_size > 1);
+  assert(nbits > 0);
+  generateTestPoints();
+}
+
+// ----------------------------------------------------------------------------
+
+BRIEF::~BRIEF()
+{
+}
+
+// ---------------------------------------------------------------------------
+
+void BRIEF::compute(const cv::Mat &image, 
+    const std::vector<cv::KeyPoint> &points,
+    vector<bitset> &descriptors,
+    bool treat_image) const
+{
+  const float sigma = 2.f;
+  const cv::Size ksize(9, 9);
+  
+  cv::Mat im;
+  if(treat_image)
+  {
+    cv::Mat aux;
+    if(image.depth() == 3)
+    {
+      cv::cvtColor(image, aux, cv::COLOR_RGB2GRAY);
+    }
+    else
+    {
+      aux = image;
+    }
+
+    cv::GaussianBlur(aux, im, ksize, sigma, sigma);
+    
+  }
+  else
+  {
+    im = image;
+  }
+  
+  assert(im.type() == CV_8UC1);
+  assert(im.isContinuous());
+  
+  // use im now
+  const int W = im.cols;
+  const int H = im.rows;
+  
+  descriptors.resize(points.size());
+  std::vector<bitset>::iterator dit;
+
+  std::vector<cv::KeyPoint>::const_iterator kit;
+  
+  int x1, y1, x2, y2;
+  
+  dit = descriptors.begin();
+  for(kit = points.begin(); kit != points.end(); ++kit, ++dit)
+  {
+    dit->resize(m_bit_length);
+    dit->reset();
+
+    for(unsigned int i = 0; i < m_x1.size(); ++i)
+    {
+      x1 = (int)(kit->pt.x + m_x1[i]);
+      y1 = (int)(kit->pt.y + m_y1[i]);
+      x2 = (int)(kit->pt.x + m_x2[i]);
+      y2 = (int)(kit->pt.y + m_y2[i]);
+      
+      if(x1 >= 0 && x1 < W && y1 >= 0 && y1 < H 
+        && x2 >= 0 && x2 < W && y2 >= 0 && y2 < H)
+      {
+        if( im.ptr<unsigned char>(y1)[x1] < im.ptr<unsigned char>(y2)[x2] )
+        {
+          dit->set(i);
+        }        
+      } // if (x,y)_1 and (x,y)_2 are in the image
+            
+    } // for each (x,y)
+  } // for each keypoint
+}
+
+// ---------------------------------------------------------------------------
+
+void BRIEF::generateTestPoints()
+{  
+  m_x1.resize(m_bit_length);
+  m_y1.resize(m_bit_length);
+  m_x2.resize(m_bit_length);
+  m_y2.resize(m_bit_length);
+
+  const float g_mean = 0.f;
+  const float g_sigma = 0.2f * (float)m_patch_size;
+  const float c_sigma = 0.08f * (float)m_patch_size;
+  
+  float sigma2;
+  if(m_type == RANDOM)
+    sigma2 = g_sigma;
+  else
+    sigma2 = c_sigma;
+  
+  const int max_v = m_patch_size / 2;
+  
+  DUtils::Random::SeedRandOnce();
+  
+  for(int i = 0; i < m_bit_length; ++i)
+  {
+    int x1, y1, x2, y2;
+    
+    do
+    {
+      x1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma);
+    } while( x1 > max_v || x1 < -max_v);
+    
+    do
+    {
+      y1 = DUtils::Random::RandomGaussianValue(g_mean, g_sigma);
+    } while( y1 > max_v || y1 < -max_v);
+    
+    float meanx, meany;
+    if(m_type == RANDOM)
+      meanx = meany = g_mean;
+    else
+    {
+      meanx = x1;
+      meany = y1;
+    }
+    
+    do
+    {
+      x2 = DUtils::Random::RandomGaussianValue(meanx, sigma2);
+    } while( x2 > max_v || x2 < -max_v);
+    
+    do
+    {
+      y2 = DUtils::Random::RandomGaussianValue(meany, sigma2);
+    } while( y2 > max_v || y2 < -max_v);
+    
+    m_x1[i] = x1;
+    m_y1[i] = y1;
+    m_x2[i] = x2;
+    m_y2[i] = y2;
+  }
+
+}
+
+// ----------------------------------------------------------------------------
+
+

loop_fusion/src/ThirdParty/DVision/BRIEF.h

@@ -0,0 +1,196 @@
+/**
+ * File: BRIEF.h
+ * Author: Dorian Galvez-Lopez
+ * Date: March 2011
+ * Description: implementation of BRIEF (Binary Robust Independent 
+ *   Elementary Features) descriptor by 
+ *   Michael Calonder, Vincent Lepetit and Pascal Fua
+ *   + close binary tests (by Dorian Galvez-Lopez)
+ *
+ * If you use this code with the RANDOM_CLOSE descriptor version, please cite:
+  @INPROCEEDINGS{GalvezIROS11,
+    author={Galvez-Lopez, Dorian and Tardos, Juan D.},
+    booktitle={Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on},
+    title={Real-time loop detection with bags of binary words},
+    year={2011},
+    month={sept.},
+    volume={},
+    number={},
+    pages={51 -58},
+    keywords={},
+    doi={10.1109/IROS.2011.6094885},
+    ISSN={2153-0858}
+  }
+ *
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_BRIEF__
+#define __D_BRIEF__
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <boost/dynamic_bitset.hpp>
+
+namespace DVision {
+
+/// BRIEF descriptor
+class BRIEF
+{
+public:
+
+  /// Bitset type
+  typedef boost::dynamic_bitset<> bitset;
+
+  /// Type of pairs
+  enum Type
+  {
+    RANDOM, // random pairs (Calonder's original version)
+    RANDOM_CLOSE, // random but close pairs (used in GalvezIROS11)
+  };
+  
+public:
+
+  /**
+   * Creates the BRIEF a priori data for descriptors of nbits length
+   * @param nbits descriptor length in bits
+   * @param patch_size 
+   * @param type type of pairs to generate
+   */
+  BRIEF(int nbits = 256, int patch_size = 48, Type type = RANDOM_CLOSE);
+  virtual ~BRIEF();
+  
+  /**
+   * Returns the descriptor length in bits
+   * @return descriptor length in bits
+   */
+  inline int getDescriptorLengthInBits() const
+  {
+    return m_bit_length;
+  }
+  
+  /**
+   * Returns the type of classifier
+   */
+  inline Type getType() const
+  {
+    return m_type;
+  }
+  
+  /**
+   * Returns the size of the patch
+   */
+  inline int getPatchSize() const
+  {
+    return m_patch_size;
+  }
+  
+  /**
+   * Returns the BRIEF descriptors of the given keypoints in the given image
+   * @param image
+   * @param points
+   * @param descriptors 
+   * @param treat_image (default: true) if true, the image is converted to 
+   *   grayscale if needed and smoothed. If not, it is assumed the image has
+   *   been treated by the user
+   * @note this function is similar to BRIEF::compute
+   */
+  inline void operator() (const cv::Mat &image, 
+    const std::vector<cv::KeyPoint> &points,
+    std::vector<bitset> &descriptors,
+    bool treat_image = true) const
+  {
+    compute(image, points, descriptors, treat_image);
+  }
+  
+  /**
+   * Returns the BRIEF descriptors of the given keypoints in the given image
+   * @param image
+   * @param points
+   * @param descriptors 
+   * @param treat_image (default: true) if true, the image is converted to 
+   *   grayscale if needed and smoothed. If not, it is assumed the image has
+   *   been treated by the user
+   * @note this function is similar to BRIEF::operator()
+   */ 
+  void compute(const cv::Mat &image,
+    const std::vector<cv::KeyPoint> &points,
+    std::vector<bitset> &descriptors,
+    bool treat_image = true) const;
+  
+  /**
+   * Exports the test pattern
+   * @param x1 x1 coordinates of pairs
+   * @param y1 y1 coordinates of pairs
+   * @param x2 x2 coordinates of pairs
+   * @param y2 y2 coordinates of pairs
+   */
+  inline void exportPairs(std::vector<int> &x1, std::vector<int> &y1,
+    std::vector<int> &x2, std::vector<int> &y2) const
+  {
+    x1 = m_x1;
+    y1 = m_y1;
+    x2 = m_x2;
+    y2 = m_y2;
+  }
+  
+  /**
+   * Sets the test pattern
+   * @param x1 x1 coordinates of pairs
+   * @param y1 y1 coordinates of pairs
+   * @param x2 x2 coordinates of pairs
+   * @param y2 y2 coordinates of pairs
+   */
+  inline void importPairs(const std::vector<int> &x1, 
+    const std::vector<int> &y1, const std::vector<int> &x2, 
+    const std::vector<int> &y2)
+  {
+    m_x1 = x1;
+    m_y1 = y1;
+    m_x2 = x2;
+    m_y2 = y2;
+    m_bit_length = x1.size();
+  }
+  
+  /**
+   * Returns the Hamming distance between two descriptors
+   * @param a first descriptor vector
+   * @param b second descriptor vector
+   * @return hamming distance
+   */
+  inline static int distance(const bitset &a, const bitset &b)
+  {
+    return (a^b).count();
+  }
+
+protected:
+
+  /**
+   * Generates random points in the patch coordinates, according to 
+   * m_patch_size and m_bit_length
+   */
+  void generateTestPoints();
+  
+protected:
+
+  /// Descriptor length in bits
+  int m_bit_length;
+
+  /// Patch size
+  int m_patch_size;
+  
+  /// Type of pairs
+  Type m_type;
+
+  /// Coordinates of test points relative to the center of the patch
+  std::vector<int> m_x1, m_x2;
+  std::vector<int> m_y1, m_y2;
+
+};
+
+} // namespace DVision
+
+#endif
+
+

loop_fusion/src/ThirdParty/DVision/DVision.h

@@ -0,0 +1,43 @@
+/*
+ * File: DVision.h
+ * Project: DVision library
+ * Author: Dorian Galvez-Lopez
+ * Date: October 4, 2010
+ * Description: several functions for computer vision
+ * License: see the LICENSE.txt file
+ *
+ */
+
+/*! \mainpage DVision Library
+ *
+ * DVision library for C++ and OpenCV:
+ * Collection of classes with computer vision functionality
+ *
+ * Written by Dorian Galvez-Lopez,
+ * University of Zaragoza
+ * 
+ * Check my website to obtain updates: http://webdiis.unizar.es/~dorian
+ *
+ * \section requirements Requirements
+ * This library requires the DUtils and DUtilsCV libraries and the OpenCV library.
+ *
+ * \section license License
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License (LGPL) as 
+ * published by the Free Software Foundation, either version 3 of the License, 
+ * or any later version.
+ *
+ */
+
+#ifndef __D_VISION__
+#define __D_VISION__
+
+
+/// Computer vision functions
+namespace DVision
+{
+}
+
+#include "BRIEF.h"
+
+#endif

loop_fusion/src/ThirdParty/VocabularyBinary.cpp

@@ -0,0 +1,35 @@
+#include "VocabularyBinary.hpp"
+#include <opencv2/core/core.hpp>
+using namespace std;
+
+VINSLoop::Vocabulary::Vocabulary()
+: nNodes(0), nodes(nullptr), nWords(0), words(nullptr) {
+}
+
+VINSLoop::Vocabulary::~Vocabulary() {
+    if (nodes != nullptr) {
+        delete [] nodes;
+        nodes = nullptr;
+    }
+    
+    if (words != nullptr) {
+        delete [] words;
+        words = nullptr;
+    }
+}
+    
+void VINSLoop::Vocabulary::serialize(ofstream& stream) {
+    stream.write((const char *)this, staticDataSize());
+    stream.write((const char *)nodes, sizeof(Node) * nNodes);
+    stream.write((const char *)words, sizeof(Word) * nWords);
+}
+    
+void VINSLoop::Vocabulary::deserialize(ifstream& stream) {
+    stream.read((char *)this, staticDataSize());
+    
+    nodes = new Node[nNodes];
+    stream.read((char *)nodes, sizeof(Node) * nNodes);
+    
+    words = new Word[nWords];
+    stream.read((char *)words, sizeof(Word) * nWords);
+}

loop_fusion/src/ThirdParty/VocabularyBinary.hpp

@@ -0,0 +1,47 @@
+#ifndef VocabularyBinary_hpp
+#define VocabularyBinary_hpp
+
+#include <cstdint>
+#include <fstream>
+#include <string>
+
+namespace VINSLoop {
+    
+struct Node {
+    int32_t nodeId;
+    int32_t parentId;
+    double weight;
+    uint64_t descriptor[4];
+};
+
+struct Word {
+    int32_t nodeId;
+    int32_t wordId;
+};
+
+struct Vocabulary {
+    int32_t k;
+    int32_t L;
+    int32_t scoringType;
+    int32_t weightingType;
+    
+    int32_t nNodes;
+    int32_t nWords;
+    
+    Node* nodes;
+    Word* words;
+    
+    Vocabulary();
+    ~Vocabulary();
+    
+    void serialize(std::ofstream& stream);
+    void deserialize(std::ifstream& stream);
+    
+    inline static size_t staticDataSize() {
+        return sizeof(Vocabulary) - sizeof(Node*) - sizeof(Word*);
+    }
+};
+
+}
+
+#endif /* VocabularyBinary_hpp */

loop_fusion/src/keyframe.cpp

@@ -0,0 +1,623 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *
+ * Author: Qin Tong (qintonguav@gmail.com)
+ *******************************************************/
+
+#include "keyframe.h"
+
+template <typename Derived>
+static void reduceVector(vector<Derived> &v, vector<uchar> status)
+{
+    // changed by PI
+    int j = 0;
+    int range = 0;
+    if ( int(status.size()) <= int(v.size()) ){
+        range = (int)status.size();
+    }
+    else{
+        range = (int)v.size();
+    }
+    for (int i = 0; i < range; i++)
+        if (status[i])
+            v[j++] = v[i];
+    v.resize(j);
+}
+
+// create keyframe online
+KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image,
+		           vector<cv::Point3f> &_point_3d, vector<cv::Point2f> &_point_2d_uv, vector<cv::Point2f> &_point_2d_norm,
+		           vector<double> &_point_id, int _sequence)
+{
+	time_stamp = _time_stamp;
+	index = _index;
+	vio_T_w_i = _vio_T_w_i;
+	vio_R_w_i = _vio_R_w_i;
+	T_w_i = vio_T_w_i;
+	R_w_i = vio_R_w_i;
+	origin_vio_T = vio_T_w_i;		
+	origin_vio_R = vio_R_w_i;
+	image = _image.clone();
+	cv::resize(image, thumbnail, cv::Size(80, 60));
+	point_3d = _point_3d;
+	point_2d_uv = _point_2d_uv;
+	point_2d_norm = _point_2d_norm;
+	point_id = _point_id;
+	has_loop = false;
+	loop_index = -1;
+	has_fast_point = false;
+	loop_info << 0, 0, 0, 0, 0, 0, 0, 0;
+	sequence = _sequence;
+	computeWindowBRIEFPoint();
+	computeBRIEFPoint();
+	if(!DEBUG_IMAGE)
+		image.release();
+}
+
+// load previous keyframe
+KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i,
+					cv::Mat &_image, int _loop_index, Eigen::Matrix<double, 8, 1 > &_loop_info,
+					vector<cv::KeyPoint> &_keypoints, vector<cv::KeyPoint> &_keypoints_norm, vector<BRIEF::bitset> &_brief_descriptors)
+{
+	time_stamp = _time_stamp;
+	index = _index;
+	//vio_T_w_i = _vio_T_w_i;
+	//vio_R_w_i = _vio_R_w_i;
+	vio_T_w_i = _T_w_i;
+	vio_R_w_i = _R_w_i;
+	T_w_i = _T_w_i;
+	R_w_i = _R_w_i;
+	if (DEBUG_IMAGE)
+	{
+		image = _image.clone();
+		cv::resize(image, thumbnail, cv::Size(80, 60));
+	}
+	if (_loop_index != -1)
+		has_loop = true;
+	else
+		has_loop = false;
+	loop_index = _loop_index;
+	loop_info = _loop_info;
+	has_fast_point = false;
+	sequence = 0;
+	keypoints = _keypoints;
+	keypoints_norm = _keypoints_norm;
+	brief_descriptors = _brief_descriptors;
+}
+
+
+void KeyFrame::computeWindowBRIEFPoint()
+{
+	BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
+	for(int i = 0; i < (int)point_2d_uv.size(); i++)
+	{
+	    cv::KeyPoint key;
+	    key.pt = point_2d_uv[i];
+	    window_keypoints.push_back(key);
+	}
+	extractor(image, window_keypoints, window_brief_descriptors);
+}
+
+void KeyFrame::computeBRIEFPoint()
+{
+	BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
+	const int fast_th = 10; // corner detector response threshold
+	if(1)
+	{
+        //cv::FAST(image, keypoints, fast_th, true);
+        Grider_FAST::perform_griding(image, keypoints, 200, 1, 1, fast_th, true);
+	}
+	else
+    {
+		vector<cv::Point2f> tmp_pts;
+		cv::goodFeaturesToTrack(image, tmp_pts, 500, 0.01, 10);
+		for(int i = 0; i < (int)tmp_pts.size(); i++)
+		{
+		    cv::KeyPoint key;
+		    key.pt = tmp_pts[i];
+		    keypoints.push_back(key);
+		}
+	}
+
+	// push back the uvs used in vio
+    for(int i = 0; i < (int)point_2d_uv.size(); i++)
+    {
+        cv::KeyPoint key;
+        key.pt = point_2d_uv[i];
+        keypoints.push_back(key);
+    }
+
+    // extract and save
+	extractor(image, keypoints, brief_descriptors);
+	for (int i = 0; i < (int)keypoints.size(); i++)
+	{
+        Eigen::Vector3d tmp_p;
+        m_camera->liftProjective(Eigen::Vector2d(keypoints[i].pt.x, keypoints[i].pt.y), tmp_p);
+        cv::KeyPoint tmp_norm;
+        tmp_norm.pt = cv::Point2f(tmp_p.x()/tmp_p.z(), tmp_p.y()/tmp_p.z());
+		keypoints_norm.push_back(tmp_norm);
+	}
+}
+
+void BriefExtractor::operator() (const cv::Mat &im, vector<cv::KeyPoint> &keys, vector<BRIEF::bitset> &descriptors) const
+{
+  m_brief.compute(im, keys, descriptors);
+}
+
+
+bool KeyFrame::searchInAera(const BRIEF::bitset window_descriptor,
+                            const std::vector<BRIEF::bitset> &descriptors_old,
+                            const std::vector<cv::KeyPoint> &keypoints_old,
+                            const std::vector<cv::KeyPoint> &keypoints_old_norm,
+                            cv::Point2f &best_match,
+                            cv::Point2f &best_match_norm)
+{
+    cv::Point2f best_pt;
+    int bestDist = 128;
+    int bestIndex = -1;
+    for(int i = 0; i < (int)descriptors_old.size(); i++)
+    {
+
+        int dis = HammingDis(window_descriptor, descriptors_old[i]);
+        if(dis < bestDist)
+        {
+            bestDist = dis;
+            bestIndex = i;
+        }
+    }
+    //printf("[POSEGRAPH]: best dist %d", bestDist);
+    if (bestIndex != -1 && bestDist < 80)
+    {
+      best_match = keypoints_old[bestIndex].pt;
+      best_match_norm = keypoints_old_norm[bestIndex].pt;
+      return true;
+    }
+    else
+      return false;
+}
+
+void KeyFrame::searchByBRIEFDes(std::vector<cv::Point2f> &matched_2d_old,
+								std::vector<cv::Point2f> &matched_2d_old_norm,
+                                std::vector<uchar> &status,
+                                const std::vector<BRIEF::bitset> &descriptors_old,
+                                const std::vector<cv::KeyPoint> &keypoints_old,
+                                const std::vector<cv::KeyPoint> &keypoints_old_norm)
+{
+    for(int i = 0; i < (int)window_brief_descriptors.size(); i++)
+    {
+        cv::Point2f pt(0.f, 0.f);
+        cv::Point2f pt_norm(0.f, 0.f);
+        if (searchInAera(window_brief_descriptors[i], descriptors_old, keypoints_old, keypoints_old_norm, pt, pt_norm))
+          status.push_back(1);
+        else
+          status.push_back(0);
+        matched_2d_old.push_back(pt);
+        matched_2d_old_norm.push_back(pt_norm);
+    }
+
+}
+
+
+void KeyFrame::FundmantalMatrixRANSAC(const std::vector<cv::Point2f> &matched_2d_cur_norm,
+                                      const std::vector<cv::Point2f> &matched_2d_old_norm,
+                                      vector<uchar> &status)
+{
+	int n = (int)matched_2d_cur_norm.size();
+	for (int i = 0; i < n; i++)
+		status.push_back(0);
+    if (n >= 8)
+    {
+        vector<cv::Point2f> tmp_cur(n), tmp_old(n);
+        for (int i = 0; i < (int)matched_2d_cur_norm.size(); i++)
+        {
+            double FOCAL_LENGTH = 460.0;
+            double tmp_x, tmp_y;
+            tmp_x = FOCAL_LENGTH * matched_2d_cur_norm[i].x + COL / 2.0;
+            tmp_y = FOCAL_LENGTH * matched_2d_cur_norm[i].y + ROW / 2.0;
+            tmp_cur[i] = cv::Point2f(tmp_x, tmp_y);
+
+            tmp_x = FOCAL_LENGTH * matched_2d_old_norm[i].x + COL / 2.0;
+            tmp_y = FOCAL_LENGTH * matched_2d_old_norm[i].y + ROW / 2.0;
+            tmp_old[i] = cv::Point2f(tmp_x, tmp_y);
+        }
+        cv::findFundamentalMat(tmp_cur, tmp_old, cv::FM_RANSAC, 3.0, 0.9, status);
+    }
+}
+
+void KeyFrame::PnPRANSAC(const vector<cv::Point2f> &matched_2d_old_norm,
+                         const std::vector<cv::Point3f> &matched_3d,
+                         std::vector<uchar> &status,
+                         Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old)
+{
+	//for (int i = 0; i < matched_3d.size(); i++)
+	//	printf("[POSEGRAPH]: 3d x: %f, y: %f, z: %f\n",matched_3d[i].x, matched_3d[i].y, matched_3d[i].z );
+	//printf("[POSEGRAPH]: match size %d \n", matched_3d.size());
+    cv::Mat r, rvec, t, D, tmp_r;
+    cv::Mat K = (cv::Mat_<double>(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0);
+    Matrix3d R_inital;
+    Vector3d P_inital;
+    Matrix3d R_w_c = origin_vio_R * qic;
+    Vector3d T_w_c = origin_vio_T + origin_vio_R * tic;
+
+    R_inital = R_w_c.inverse();
+    P_inital = -(R_inital * T_w_c);
+
+    cv::eigen2cv(R_inital, tmp_r);
+    cv::Rodrigues(tmp_r, rvec);
+    cv::eigen2cv(P_inital, t);
+
+    cv::Mat inliers;
+    TicToc t_pnp_ransac;
+
+    int flags = cv::SOLVEPNP_EPNP; // SOLVEPNP_EPNP, SOLVEPNP_ITERATIVE
+    if (CV_MAJOR_VERSION < 3)
+        solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 200, PNP_INFLATION / max_focallength, 100, inliers, flags);
+    else
+    {
+        if (CV_MINOR_VERSION < 2)
+            solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 200, sqrt(PNP_INFLATION / max_focallength), 0.99, inliers, flags);
+        else
+            solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 200, PNP_INFLATION / max_focallength, 0.99, inliers, flags);
+
+    }
+
+    for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)
+        status.push_back(0);
+
+    for( int i = 0; i < inliers.rows; i++)
+    {
+        int n = inliers.at<int>(i);
+        status[n] = 1;
+    }
+
+    cv::Rodrigues(rvec, r);
+    Matrix3d R_pnp, R_w_c_old;
+    cv::cv2eigen(r, R_pnp);
+    R_w_c_old = R_pnp.transpose();
+    Vector3d T_pnp, T_w_c_old;
+    cv::cv2eigen(t, T_pnp);
+    T_w_c_old = R_w_c_old * (-T_pnp);
+
+    PnP_R_old = R_w_c_old * qic.transpose();
+    PnP_T_old = T_w_c_old - PnP_R_old * tic;
+
+}
+
+
+bool KeyFrame::findConnection(KeyFrame* old_kf)
+{
+	TicToc tmp_t;
+	//printf("[POSEGRAPH]: find Connection\n");
+	vector<cv::Point2f> matched_2d_cur, matched_2d_old;
+	vector<cv::Point2f> matched_2d_cur_norm, matched_2d_old_norm;
+	vector<cv::Point3f> matched_3d;
+	vector<double> matched_id;
+	vector<uchar> status;
+
+    // re-undistort with the latest intrinsic values
+    for (int i = 0; i < (int)point_2d_uv.size(); i++) {
+        Eigen::Vector3d tmp_p;
+        m_camera->liftProjective(Eigen::Vector2d(point_2d_uv[i].x, point_2d_uv[i].y), tmp_p);
+        point_2d_norm.push_back(cv::Point2f(tmp_p.x()/tmp_p.z(), tmp_p.y()/tmp_p.z()));
+    }
+    old_kf->keypoints_norm.clear();
+    for (int i = 0; i < (int)old_kf->keypoints.size(); i++) {
+        Eigen::Vector3d tmp_p;
+        m_camera->liftProjective(Eigen::Vector2d(old_kf->keypoints[i].pt.x, old_kf->keypoints[i].pt.y), tmp_p);
+        cv::KeyPoint tmp_norm;
+        tmp_norm.pt = cv::Point2f(tmp_p.x()/tmp_p.z(), tmp_p.y()/tmp_p.z());
+        old_kf->keypoints_norm.push_back(tmp_norm);
+    }
+
+    matched_3d = point_3d;
+    matched_2d_cur = point_2d_uv;
+    matched_id = point_id;
+    matched_2d_cur_norm = point_2d_norm;
+
+	TicToc t_match;
+	#if 0
+		if (DEBUG_IMAGE)    
+	    {
+	        cv::Mat gray_img, loop_match_img;
+	        cv::Mat old_img = old_kf->image;
+	        cv::hconcat(image, old_img, gray_img);
+	        cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);
+	        for(int i = 0; i< (int)point_2d_uv.size(); i++)
+	        {
+	            cv::Point2f cur_pt = point_2d_uv[i];
+	            cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));
+	        }
+	        for(int i = 0; i< (int)old_kf->keypoints.size(); i++)
+	        {
+	            cv::Point2f old_pt = old_kf->keypoints[i].pt;
+	            old_pt.x += COL;
+	            cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));
+	        }
+	        ostringstream path;
+	        path << "/home/tony-ws1/raw_data/loop_image/"
+	                << index << "-"
+	                << old_kf->index << "-" << "0raw_point.jpg";
+	        cv::imwrite( path.str().c_str(), loop_match_img);
+	    }
+	#endif
+	//printf("[POSEGRAPH]: search by des\n");
+	searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm);
+	reduceVector(matched_2d_cur, status);
+	reduceVector(matched_2d_old, status);
+	reduceVector(matched_2d_cur_norm, status);
+	reduceVector(matched_2d_old_norm, status);
+	reduceVector(matched_3d, status);
+	reduceVector(matched_id, status);
+	//printf("[POSEGRAPH]: search by des finish\n");
+
+	#if 0 
+		if (DEBUG_IMAGE)
+	    {
+			int gap = 10;
+        	cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255));
+            cv::Mat gray_img, loop_match_img;
+            cv::Mat old_img = old_kf->image;
+            cv::hconcat(image, gap_image, gap_image);
+            cv::hconcat(gap_image, old_img, gray_img);
+            cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);
+	        for(int i = 0; i< (int)matched_2d_cur.size(); i++)
+	        {
+	            cv::Point2f cur_pt = matched_2d_cur[i];
+	            cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));
+	        }
+	        for(int i = 0; i< (int)matched_2d_old.size(); i++)
+	        {
+	            cv::Point2f old_pt = matched_2d_old[i];
+	            old_pt.x += (COL + gap);
+	            cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));
+	        }
+	        for (int i = 0; i< (int)matched_2d_cur.size(); i++)
+	        {
+	            cv::Point2f old_pt = matched_2d_old[i];
+	            old_pt.x +=  (COL + gap);
+	            cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 1, 8, 0);
+	        }
+
+	        ostringstream path, path1, path2;
+	        path <<  "/home/tony-ws1/raw_data/loop_image/"
+	                << index << "-"
+	                << old_kf->index << "-" << "1descriptor_match.jpg";
+	        cv::imwrite( path.str().c_str(), loop_match_img);
+	        /*
+	        path1 <<  "/home/tony-ws1/raw_data/loop_image/"
+	                << index << "-"
+	                << old_kf->index << "-" << "1descriptor_match_1.jpg";
+	        cv::imwrite( path1.str().c_str(), image);
+	        path2 <<  "/home/tony-ws1/raw_data/loop_image/"
+	                << index << "-"
+	                << old_kf->index << "-" << "1descriptor_match_2.jpg";
+	        cv::imwrite( path2.str().c_str(), old_img);	        
+	        */
+	        
+	    }
+	#endif
+	status.clear();
+	/*
+	FundmantalMatrixRANSAC(matched_2d_cur_norm, matched_2d_old_norm, status);
+	reduceVector(matched_2d_cur, status);
+	reduceVector(matched_2d_old, status);
+	reduceVector(matched_2d_cur_norm, status);
+	reduceVector(matched_2d_old_norm, status);
+	reduceVector(matched_3d, status);
+	reduceVector(matched_id, status);
+	*/
+	#if 0
+		if (DEBUG_IMAGE)
+	    {
+			int gap = 10;
+        	cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255));
+            cv::Mat gray_img, loop_match_img;
+            cv::Mat old_img = old_kf->image;
+            cv::hconcat(image, gap_image, gap_image);
+            cv::hconcat(gap_image, old_img, gray_img);
+            cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);
+	        for(int i = 0; i< (int)matched_2d_cur.size(); i++)
+	        {
+	            cv::Point2f cur_pt = matched_2d_cur[i];
+	            cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));
+	        }
+	        for(int i = 0; i< (int)matched_2d_old.size(); i++)
+	        {
+	            cv::Point2f old_pt = matched_2d_old[i];
+	            old_pt.x += (COL + gap);
+	            cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));
+	        }
+	        for (int i = 0; i< (int)matched_2d_cur.size(); i++)
+	        {
+	            cv::Point2f old_pt = matched_2d_old[i];
+	            old_pt.x +=  (COL + gap) ;
+	            cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 1, 8, 0);
+	        }
+
+	        ostringstream path;
+	        path <<  "/home/tony-ws1/raw_data/loop_image/"
+	                << index << "-"
+	                << old_kf->index << "-" << "2fundamental_match.jpg";
+	        cv::imwrite( path.str().c_str(), loop_match_img);
+	    }
+	#endif
+	Eigen::Vector3d PnP_T_old;
+	Eigen::Matrix3d PnP_R_old;
+	Eigen::Vector3d relative_t;
+	Quaterniond relative_q;
+	double relative_yaw;
+	if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
+	{
+		status.clear();
+	    PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);
+	    reduceVector(matched_2d_cur, status);
+	    reduceVector(matched_2d_old, status);
+	    reduceVector(matched_2d_cur_norm, status);
+	    reduceVector(matched_2d_old_norm, status);
+	    reduceVector(matched_3d, status);
+	    reduceVector(matched_id, status);
+	    #if 1
+	    	if (DEBUG_IMAGE)
+	        {
+	        	int gap = 10;
+	        	cv::Mat gap_image(old_kf->image.rows, gap, CV_8UC1, cv::Scalar(255, 255, 255));
+	            cv::Mat gray_img, loop_match_img;
+	            cv::Mat old_img = old_kf->image;
+	            cv::hconcat(image, gap_image, gap_image);
+	            cv::hconcat(gap_image, old_img, gray_img);
+	            cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);
+	            for(int i = 0; i< (int)matched_2d_cur.size(); i++)
+	            {
+	                cv::Point2f cur_pt = matched_2d_cur[i];
+	                cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));
+	            }
+	            for(int i = 0; i< (int)matched_2d_old.size(); i++)
+	            {
+	                cv::Point2f old_pt = matched_2d_old[i];
+	                old_pt.x += (old_kf->image.cols + gap);
+	                cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));
+	            }
+	            for (int i = 0; i< (int)matched_2d_cur.size(); i++)
+	            {
+	                cv::Point2f old_pt = matched_2d_old[i];
+	                old_pt.x += (old_kf->image.cols + gap) ;
+	                cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 2, 8, 0);
+	            }
+	            cv::Mat notation(50, old_kf->image.cols + gap + old_kf->image.cols, CV_8UC3, cv::Scalar(255, 255, 255));
+	            putText(notation, "current frame: " + to_string(index) + "  sequence: " + to_string(sequence), cv::Point2f(20, 30), CV_FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3);
+
+	            putText(notation, "previous frame: " + to_string(old_kf->index) + "  sequence: " + to_string(old_kf->sequence), cv::Point2f(20 + old_kf->image.cols + gap, 30), CV_FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3);
+	            cv::vconcat(notation, loop_match_img, loop_match_img);
+
+	            /*
+	            ostringstream path;
+	            path <<  "/home/tony-ws1/raw_data/loop_image/"
+	                    << index << "-"
+	                    << old_kf->index << "-" << "3pnp_match.jpg";
+	            cv::imwrite( path.str().c_str(), loop_match_img);
+	            */
+	            if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
+	            {
+	            	/*
+	            	cv::imshow("loop connection",loop_match_img);  
+	            	cv::waitKey(10);  
+	            	*/
+	            	cv::Mat thumbimage;
+	            	cv::resize(loop_match_img, thumbimage, cv::Size(loop_match_img.cols / 2, loop_match_img.rows / 2));
+	    	    	sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumbimage).toImageMsg();
+	                msg->header.stamp = ros::Time(time_stamp);
+	    	    	pub_match_img.publish(msg);
+	            }
+	        }
+	    #endif
+	}
+
+	if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
+	{
+	    relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);
+	    relative_q = PnP_R_old.transpose() * origin_vio_R;
+	    relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());
+	    //printf("[POSEGRAPH]: PNP relative\n");
+	    //cout << "pnp relative_t " << relative_t.transpose() << endl;
+	    //cout << "pnp relative_yaw " << relative_yaw << endl;
+	    if (abs(relative_yaw) < MAX_THETA_DIFF && relative_t.norm() < MAX_POS_DIFF)
+	    {
+
+	    	has_loop = true;
+	    	loop_index = old_kf->index;
+	    	loop_info << relative_t.x(), relative_t.y(), relative_t.z(),
+	    	             relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
+	    	             relative_yaw;
+	    	//cout << "pnp relative_t " << relative_t.transpose() << endl;
+	    	//cout << "pnp relative_q " << relative_q.w() << " " << relative_q.vec().transpose() << endl;
+	        return true;
+	    }
+	}
+	//printf("[POSEGRAPH]: loop final use num %d %lf--------------- \n", (int)matched_2d_cur.size(), t_match.toc());
+	return false;
+}
+
+
+int KeyFrame::HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b)
+{
+    BRIEF::bitset xor_of_bitset = a ^ b;
+    int dis = xor_of_bitset.count();
+    return dis;
+}
+
+void KeyFrame::getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i)
+{
+    _T_w_i = vio_T_w_i;
+    _R_w_i = vio_R_w_i;
+}
+
+void KeyFrame::getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i)
+{
+    _T_w_i = T_w_i;
+    _R_w_i = R_w_i;
+}
+
+void KeyFrame::updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i)
+{
+    T_w_i = _T_w_i;
+    R_w_i = _R_w_i;
+}
+
+void KeyFrame::updateVioPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i)
+{
+	vio_T_w_i = _T_w_i;
+	vio_R_w_i = _R_w_i;
+	T_w_i = vio_T_w_i;
+	R_w_i = vio_R_w_i;
+}
+
+Eigen::Vector3d KeyFrame::getLoopRelativeT()
+{
+    return Eigen::Vector3d(loop_info(0), loop_info(1), loop_info(2));
+}
+
+Eigen::Quaterniond KeyFrame::getLoopRelativeQ()
+{
+    return Eigen::Quaterniond(loop_info(3), loop_info(4), loop_info(5), loop_info(6));
+}
+
+double KeyFrame::getLoopRelativeYaw()
+{
+    return loop_info(7);
+}
+
+void KeyFrame::updateLoop(Eigen::Matrix<double, 8, 1 > &_loop_info)
+{
+	if (abs(_loop_info(7)) < 30.0 && Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0)
+	{
+		//printf("[POSEGRAPH]: update loop info\n");
+		loop_info = _loop_info;
+	}
+}
+
+BriefExtractor::BriefExtractor(const std::string &pattern_file)
+{
+  // The DVision::BRIEF extractor computes a random pattern by default when
+  // the object is created.
+  // We load the pattern that we used to build the vocabulary, to make
+  // the descriptors compatible with the predefined vocabulary
+
+  // loads the pattern
+  cv::FileStorage fs(pattern_file.c_str(), cv::FileStorage::READ);
+  if(!fs.isOpened()) throw string("Could not open file ") + pattern_file;
+
+  vector<int> x1, y1, x2, y2;
+  fs["x1"] >> x1;
+  fs["x2"] >> x2;
+  fs["y1"] >> y1;
+  fs["y2"] >> y2;
+
+  m_brief.importPairs(x1, y1, x2, y2);
+}
+
+

loop_fusion/src/keyframe.h

@@ -0,0 +1,114 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *
+ * Author: Qin Tong (qintonguav@gmail.com)
+ *******************************************************/
+
+#pragma once
+
+#include <vector>
+#include <eigen3/Eigen/Dense>
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/eigen.hpp>
+#include "camodocal/camera_models/CameraFactory.h"
+#include "camodocal/camera_models/CataCamera.h"
+#include "camodocal/camera_models/PinholeCamera.h"
+#include "utility/tic_toc.h"
+#include "utility/utility.h"
+#include "utility/Grider_FAST.h"
+#include "parameters.h"
+#include "ThirdParty/DBoW/DBoW2.h"
+#include "ThirdParty/DVision/DVision.h"
+
+using namespace Eigen;
+using namespace std;
+using namespace DVision;
+
+
+class BriefExtractor
+{
+public:
+  virtual void operator()(const cv::Mat &im, vector<cv::KeyPoint> &keys, vector<BRIEF::bitset> &descriptors) const;
+  BriefExtractor(const std::string &pattern_file);
+
+  DVision::BRIEF m_brief;
+};
+
+class KeyFrame
+{
+public:
+	KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image,
+			 vector<cv::Point3f> &_point_3d, vector<cv::Point2f> &_point_2d_uv, vector<cv::Point2f> &_point_2d_normal, 
+			 vector<double> &_point_id, int _sequence);
+	KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, Vector3d &_T_w_i, Matrix3d &_R_w_i,
+			 cv::Mat &_image, int _loop_index, Eigen::Matrix<double, 8, 1 > &_loop_info,
+			 vector<cv::KeyPoint> &_keypoints, vector<cv::KeyPoint> &_keypoints_norm, vector<BRIEF::bitset> &_brief_descriptors);
+	bool findConnection(KeyFrame* old_kf);
+	void computeWindowBRIEFPoint();
+	void computeBRIEFPoint();
+	//void extractBrief();
+	int HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b);
+	bool searchInAera(const BRIEF::bitset window_descriptor,
+	                  const std::vector<BRIEF::bitset> &descriptors_old,
+	                  const std::vector<cv::KeyPoint> &keypoints_old,
+	                  const std::vector<cv::KeyPoint> &keypoints_old_norm,
+	                  cv::Point2f &best_match,
+	                  cv::Point2f &best_match_norm);
+	void searchByBRIEFDes(std::vector<cv::Point2f> &matched_2d_old,
+						  std::vector<cv::Point2f> &matched_2d_old_norm,
+                          std::vector<uchar> &status,
+                          const std::vector<BRIEF::bitset> &descriptors_old,
+                          const std::vector<cv::KeyPoint> &keypoints_old,
+                          const std::vector<cv::KeyPoint> &keypoints_old_norm);
+	void FundmantalMatrixRANSAC(const std::vector<cv::Point2f> &matched_2d_cur_norm,
+                                const std::vector<cv::Point2f> &matched_2d_old_norm,
+                                vector<uchar> &status);
+	void PnPRANSAC(const vector<cv::Point2f> &matched_2d_old_norm,
+	               const std::vector<cv::Point3f> &matched_3d,
+	               std::vector<uchar> &status,
+	               Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old);
+	void getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i);
+	void getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i);
+	void updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i);
+	void updateVioPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i);
+	void updateLoop(Eigen::Matrix<double, 8, 1 > &_loop_info);
+
+	Eigen::Vector3d getLoopRelativeT();
+	double getLoopRelativeYaw();
+	Eigen::Quaterniond getLoopRelativeQ();
+
+
+
+	double time_stamp; 
+	int index;
+	int local_index;
+	Eigen::Vector3d vio_T_w_i; 
+	Eigen::Matrix3d vio_R_w_i; 
+	Eigen::Vector3d T_w_i;
+	Eigen::Matrix3d R_w_i;
+	Eigen::Vector3d origin_vio_T;		
+	Eigen::Matrix3d origin_vio_R;
+	cv::Mat image;
+	cv::Mat thumbnail;
+	vector<cv::Point3f> point_3d; 
+	vector<cv::Point2f> point_2d_uv;
+	vector<cv::Point2f> point_2d_norm;
+	vector<double> point_id;
+	vector<cv::KeyPoint> keypoints;
+	vector<cv::KeyPoint> keypoints_norm;
+	vector<cv::KeyPoint> window_keypoints;
+	vector<BRIEF::bitset> brief_descriptors;
+	vector<BRIEF::bitset> window_brief_descriptors;
+	bool has_fast_point;
+	int sequence;
+
+	bool has_loop;
+	int loop_index;
+	Eigen::Matrix<double, 8, 1 > loop_info;
+};
+

loop_fusion/src/parameters.h

@@ -0,0 +1,44 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *
+ * Author: Qin Tong (qintonguav@gmail.com)
+ *******************************************************/
+
+#pragma once
+
+#include "camodocal/camera_models/CameraFactory.h"
+#include "camodocal/camera_models/CataCamera.h"
+#include "camodocal/camera_models/PinholeCamera.h"
+#include <eigen3/Eigen/Dense>
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+
+extern camodocal::CameraPtr m_camera;
+extern double max_focallength;
+extern double MIN_SCORE;
+extern double PNP_INFLATION;
+extern int RECALL_IGNORE_RECENT_COUNT;
+extern double MAX_THETA_DIFF;
+extern double MAX_POS_DIFF;
+extern int MIN_LOOP_NUM;
+extern Eigen::Vector3d tic;
+extern Eigen::Matrix3d qic;
+extern ros::Publisher pub_match_img;
+extern int VISUALIZATION_SHIFT_X;
+extern int VISUALIZATION_SHIFT_Y;
+extern std::string BRIEF_PATTERN_FILE;
+extern std::string POSE_GRAPH_SAVE_PATH;
+extern int ROW;
+extern int COL;
+extern std::string VINS_RESULT_PATH;
+extern int DEBUG_IMAGE;
+
+

loop_fusion/src/pose_graph.h

@@ -0,0 +1,334 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *
+ * Author: Qin Tong (qintonguav@gmail.com)
+ *******************************************************/
+
+#pragma once
+
+#include <thread>
+#include <mutex>
+#include <opencv2/opencv.hpp>
+#include <eigen3/Eigen/Dense>
+#include <string>
+#include <ceres/ceres.h>
+#include <ceres/rotation.h>
+#include <queue>
+#include <assert.h>
+#include <nav_msgs/Path.h>
+#include <geometry_msgs/PointStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <stdio.h>
+#include <ros/ros.h>
+#include "keyframe.h"
+#include "utility/tic_toc.h"
+#include "utility/utility.h"
+#include "utility/CameraPoseVisualization.h"
+#include "utility/tic_toc.h"
+#include "ThirdParty/DBoW/DBoW2.h"
+#include "ThirdParty/DVision/DVision.h"
+#include "ThirdParty/DBoW/TemplatedDatabase.h"
+#include "ThirdParty/DBoW/TemplatedVocabulary.h"
+
+
+#define SHOW_S_EDGE false
+#define SHOW_L_EDGE true
+#define SAVE_LOOP_PATH true
+
+using namespace DVision;
+using namespace DBoW2;
+
+class PoseGraph
+{
+public:
+	PoseGraph();
+	~PoseGraph();
+	void registerPub(ros::NodeHandle &n);
+	void addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop);
+	void loadKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop);
+	void loadVocabulary(std::string voc_path);
+	void setIMUFlag(bool _use_imu);
+	KeyFrame* getKeyFrame(int index);
+	nav_msgs::Path path[10];
+	nav_msgs::Path base_path;
+	CameraPoseVisualization* posegraph_visualization;
+	void savePoseGraph();
+	void loadPoseGraph();
+	void publish();
+	Vector3d t_drift;
+	double yaw_drift;
+	Matrix3d r_drift;
+	// world frame( base sequence or first sequence)<----> cur sequence frame  
+	Vector3d w_t_vio;
+	Matrix3d w_r_vio;
+
+
+private:
+	int detectLoop(KeyFrame* keyframe, int frame_index);
+	void addKeyFrameIntoVoc(KeyFrame* keyframe);
+	void optimize4DoF();
+	void optimize6DoF();
+	void updatePath();
+	list<KeyFrame*> keyframelist;
+	std::mutex m_keyframelist;
+	std::mutex m_optimize_buf;
+	std::mutex m_path;
+	std::mutex m_drift;
+	std::thread t_optimization;
+	std::queue<int> optimize_buf;
+
+	int global_index;
+	int sequence_cnt;
+	vector<bool> sequence_loop;
+	map<int, cv::Mat> image_pool;
+	int earliest_loop_index;
+	int base_sequence;
+	bool use_imu;
+
+	BriefDatabase db;
+	BriefVocabulary* voc;
+
+	ros::Publisher pub_pg_path;
+	ros::Publisher pub_base_path;
+	ros::Publisher pub_pose_graph;
+	ros::Publisher pub_path[10];
+};
+
+template <typename T> inline
+void QuaternionInverse(const T q[4], T q_inverse[4])
+{
+	q_inverse[0] = q[0];
+	q_inverse[1] = -q[1];
+	q_inverse[2] = -q[2];
+	q_inverse[3] = -q[3];
+};
+
+template <typename T>
+T NormalizeAngle(const T& angle_degrees) {
+  if (angle_degrees > T(180.0))
+  	return angle_degrees - T(360.0);
+  else if (angle_degrees < T(-180.0))
+  	return angle_degrees + T(360.0);
+  else
+  	return angle_degrees;
+};
+
+class AngleLocalParameterization {
+ public:
+
+  template <typename T>
+  bool operator()(const T* theta_radians, const T* delta_theta_radians,
+                  T* theta_radians_plus_delta) const {
+    *theta_radians_plus_delta =
+        NormalizeAngle(*theta_radians + *delta_theta_radians);
+
+    return true;
+  }
+
+  static ceres::LocalParameterization* Create() {
+    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
+                                                     1, 1>);
+  }
+};
+
+template <typename T> 
+void YawPitchRollToRotationMatrix(const T yaw, const T pitch, const T roll, T R[9])
+{
+
+	T y = yaw / T(180.0) * T(M_PI);
+	T p = pitch / T(180.0) * T(M_PI);
+	T r = roll / T(180.0) * T(M_PI);
+
+
+	R[0] = cos(y) * cos(p);
+	R[1] = -sin(y) * cos(r) + cos(y) * sin(p) * sin(r);
+	R[2] = sin(y) * sin(r) + cos(y) * sin(p) * cos(r);
+	R[3] = sin(y) * cos(p);
+	R[4] = cos(y) * cos(r) + sin(y) * sin(p) * sin(r);
+	R[5] = -cos(y) * sin(r) + sin(y) * sin(p) * cos(r);
+	R[6] = -sin(p);
+	R[7] = cos(p) * sin(r);
+	R[8] = cos(p) * cos(r);
+};
+
+template <typename T> 
+void RotationMatrixTranspose(const T R[9], T inv_R[9])
+{
+	inv_R[0] = R[0];
+	inv_R[1] = R[3];
+	inv_R[2] = R[6];
+	inv_R[3] = R[1];
+	inv_R[4] = R[4];
+	inv_R[5] = R[7];
+	inv_R[6] = R[2];
+	inv_R[7] = R[5];
+	inv_R[8] = R[8];
+};
+
+template <typename T> 
+void RotationMatrixRotatePoint(const T R[9], const T t[3], T r_t[3])
+{
+	r_t[0] = R[0] * t[0] + R[1] * t[1] + R[2] * t[2];
+	r_t[1] = R[3] * t[0] + R[4] * t[1] + R[5] * t[2];
+	r_t[2] = R[6] * t[0] + R[7] * t[1] + R[8] * t[2];
+};
+
+struct FourDOFError
+{
+	FourDOFError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i)
+				  :t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){}
+
+	template <typename T>
+	bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const
+	{
+		T t_w_ij[3];
+		t_w_ij[0] = tj[0] - ti[0];
+		t_w_ij[1] = tj[1] - ti[1];
+		t_w_ij[2] = tj[2] - ti[2];
+
+		// euler to rotation
+		T w_R_i[9];
+		YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
+		// rotation transpose
+		T i_R_w[9];
+		RotationMatrixTranspose(w_R_i, i_R_w);
+		// rotation matrix rotate point
+		T t_i_ij[3];
+		RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);
+
+		residuals[0] = (t_i_ij[0] - T(t_x));
+		residuals[1] = (t_i_ij[1] - T(t_y));
+		residuals[2] = (t_i_ij[2] - T(t_z));
+		residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw));
+
+		return true;
+	}
+
+	static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
+									   const double relative_yaw, const double pitch_i, const double roll_i) 
+	{
+	  return (new ceres::AutoDiffCostFunction<
+	          FourDOFError, 4, 1, 3, 1, 3>(
+	          	new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));
+	}
+
+	double t_x, t_y, t_z;
+	double relative_yaw, pitch_i, roll_i;
+
+};
+
+struct FourDOFWeightError
+{
+	FourDOFWeightError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i)
+				  :t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){
+				  	weight = 1;
+				  }
+
+	template <typename T>
+	bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const
+	{
+		T t_w_ij[3];
+		t_w_ij[0] = tj[0] - ti[0];
+		t_w_ij[1] = tj[1] - ti[1];
+		t_w_ij[2] = tj[2] - ti[2];
+
+		// euler to rotation
+		T w_R_i[9];
+		YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
+		// rotation transpose
+		T i_R_w[9];
+		RotationMatrixTranspose(w_R_i, i_R_w);
+		// rotation matrix rotate point
+		T t_i_ij[3];
+		RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);
+
+		residuals[0] = (t_i_ij[0] - T(t_x)) * T(weight);
+		residuals[1] = (t_i_ij[1] - T(t_y)) * T(weight);
+		residuals[2] = (t_i_ij[2] - T(t_z)) * T(weight);
+		residuals[3] = NormalizeAngle((yaw_j[0] - yaw_i[0] - T(relative_yaw))) * T(weight) / T(10.0);
+
+		return true;
+	}
+
+	static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
+									   const double relative_yaw, const double pitch_i, const double roll_i) 
+	{
+	  return (new ceres::AutoDiffCostFunction<
+	          FourDOFWeightError, 4, 1, 3, 1, 3>(
+	          	new FourDOFWeightError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));
+	}
+
+	double t_x, t_y, t_z;
+	double relative_yaw, pitch_i, roll_i;
+	double weight;
+
+};
+
+struct RelativeRTError
+{
+	RelativeRTError(double t_x, double t_y, double t_z, 
+					double q_w, double q_x, double q_y, double q_z,
+					double t_var, double q_var)
+				  :t_x(t_x), t_y(t_y), t_z(t_z), 
+				   q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),
+				   t_var(t_var), q_var(q_var){}
+
+	template <typename T>
+	bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const
+	{
+		T t_w_ij[3];
+		t_w_ij[0] = tj[0] - ti[0];
+		t_w_ij[1] = tj[1] - ti[1];
+		t_w_ij[2] = tj[2] - ti[2];
+
+		T i_q_w[4];
+		QuaternionInverse(w_q_i, i_q_w);
+
+		T t_i_ij[3];
+		ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);
+
+		residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);
+		residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);
+		residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);
+
+		T relative_q[4];
+		relative_q[0] = T(q_w);
+		relative_q[1] = T(q_x);
+		relative_q[2] = T(q_y);
+		relative_q[3] = T(q_z);
+
+		T q_i_j[4];
+		ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);
+
+		T relative_q_inv[4];
+		QuaternionInverse(relative_q, relative_q_inv);
+
+		T error_q[4];
+		ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); 
+
+		residuals[3] = T(2) * error_q[1] / T(q_var);
+		residuals[4] = T(2) * error_q[2] / T(q_var);
+		residuals[5] = T(2) * error_q[3] / T(q_var);
+
+		return true;
+	}
+
+	static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
+									   const double q_w, const double q_x, const double q_y, const double q_z,
+									   const double t_var, const double q_var) 
+	{
+	  return (new ceres::AutoDiffCostFunction<
+	          RelativeRTError, 6, 4, 3, 4, 3>(
+	          	new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));
+	}
+
+	double t_x, t_y, t_z, t_norm;
+	double q_w, q_x, q_y, q_z;
+	double t_var, q_var;
+
+};

loop_fusion/src/pose_graph_node.cpp

@@ -0,0 +1,633 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *
+ * Author: Qin Tong (qintonguav@gmail.com)
+ *******************************************************/
+
+#include <vector>
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
+#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <std_msgs/Bool.h>
+#include <cv_bridge/cv_bridge.h>
+#include <iostream>
+#include <ros/package.h>
+#include <mutex>
+#include <queue>
+#include <thread>
+#include <eigen3/Eigen/Dense>
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/eigen.hpp>
+#include "keyframe.h"
+#include "utility/tic_toc.h"
+#include "pose_graph.h"
+#include "utility/CameraPoseVisualization.h"
+#include "parameters.h"
+#define SKIP_FIRST_CNT 2
+using namespace std;
+
+queue<sensor_msgs::ImageConstPtr> image_buf;
+queue<sensor_msgs::PointCloudConstPtr> point_buf;
+queue<nav_msgs::Odometry::ConstPtr> pose_buf;
+queue<Eigen::Vector3d> odometry_buf;
+std::mutex m_buf;
+std::mutex m_process;
+int frame_index  = 0;
+int sequence = 1;
+PoseGraph posegraph;
+int skip_first_cnt = 0;
+int SKIP_CNT;
+int skip_cnt = 0;
+bool load_flag = 0;
+bool start_flag = 0;
+double SKIP_DIS = 0;
+double MIN_SCORE = 0.015;
+double PNP_INFLATION = 1.0;
+int RECALL_IGNORE_RECENT_COUNT = 50;
+double MAX_THETA_DIFF = 30.0;
+double MAX_POS_DIFF = 20.0;
+int MIN_LOOP_NUM = 25;
+
+int VISUALIZATION_SHIFT_X;
+int VISUALIZATION_SHIFT_Y;
+int ROW;
+int COL;
+int DEBUG_IMAGE;
+
+camodocal::CameraPtr m_camera;
+double max_focallength = 460.0;
+Eigen::Vector3d tic;
+Eigen::Matrix3d qic;
+ros::Publisher pub_match_img;
+ros::Publisher pub_camera_pose_visual;
+ros::Publisher pub_odometry_rect;
+ros::Publisher pub_pose_rect;
+
+std::string BRIEF_PATTERN_FILE;
+std::string POSE_GRAPH_SAVE_PATH;
+std::string VINS_RESULT_PATH;
+CameraPoseVisualization cameraposevisual(1, 0, 0, 1);
+Eigen::Vector3d last_t(-100, -100, -100);
+double last_image_time = -1;
+
+ros::Publisher pub_point_cloud, pub_margin_cloud;
+
+void new_sequence()
+{
+    printf("[POSEGRAPH]: new sequence\n");
+    sequence++;
+    printf("[POSEGRAPH]: sequence cnt %d \n", sequence);
+    if (sequence > 5)
+    {
+        ROS_WARN("only support 5 sequences since it's boring to copy code for more sequences.");
+        ROS_BREAK();
+    }
+    posegraph.posegraph_visualization->reset();
+    posegraph.publish();
+    m_buf.lock();
+    while(!image_buf.empty())
+        image_buf.pop();
+    while(!point_buf.empty())
+        point_buf.pop();
+    while(!pose_buf.empty())
+        pose_buf.pop();
+    while(!odometry_buf.empty())
+        odometry_buf.pop();
+    m_buf.unlock();
+}
+
+void image_callback(const sensor_msgs::ImageConstPtr &image_msg)
+{
+//    ROS_INFO("entering image_callback");
+    m_buf.lock();
+    image_buf.push(image_msg);
+    m_buf.unlock();
+    //printf("[POSEGRAPH]:  image time %f \n", image_msg->header.stamp.toSec());
+
+    // detect unstable camera stream
+    if (last_image_time == -1)
+        last_image_time = image_msg->header.stamp.toSec();
+    else if (image_msg->header.stamp.toSec() - last_image_time > 1.0 || image_msg->header.stamp.toSec() < last_image_time)
+    {
+        ROS_WARN("image discontinue! detect a new sequence!");
+        new_sequence();
+    }
+    last_image_time = image_msg->header.stamp.toSec();
+//    ROS_INFO("exiting image_callback");
+}
+
+void point_callback(const sensor_msgs::PointCloudConstPtr &point_msg)
+{
+//    ROS_INFO("entering point_callback");
+    m_buf.lock();
+    point_buf.push(point_msg);
+    m_buf.unlock();
+    /*
+    for (unsigned int i = 0; i < point_msg->points.size(); i++)
+    {
+        printf("[POSEGRAPH]: %d, 3D point: %f, %f, %f 2D point %f, %f \n",i , point_msg->points[i].x, 
+                                                     point_msg->points[i].y,
+                                                     point_msg->points[i].z,
+                                                     point_msg->channels[i].values[0],
+                                                     point_msg->channels[i].values[1]);
+    }
+    */
+    // for visualization
+    sensor_msgs::PointCloud point_cloud;
+    point_cloud.header = point_msg->header;
+    for (unsigned int i = 0; i < point_msg->points.size(); i++)
+    {
+        cv::Point3f p_3d;
+        p_3d.x = point_msg->points[i].x;
+        p_3d.y = point_msg->points[i].y;
+        p_3d.z = point_msg->points[i].z;
+        Eigen::Vector3d tmp = posegraph.r_drift * Eigen::Vector3d(p_3d.x, p_3d.y, p_3d.z) + posegraph.t_drift;
+        geometry_msgs::Point32 p;
+        p.x = tmp(0);
+        p.y = tmp(1);
+        p.z = tmp(2);
+        point_cloud.points.push_back(p);
+    }
+    pub_point_cloud.publish(point_cloud);
+//    ROS_INFO("exiting point_callback");
+}
+
+// only for visualization
+void margin_point_callback(const sensor_msgs::PointCloudConstPtr &point_msg)
+{
+//    ROS_INFO("entering margin_point_callback");
+    sensor_msgs::PointCloud point_cloud;
+    point_cloud.header = point_msg->header;
+    for (unsigned int i = 0; i < point_msg->points.size(); i++)
+    {
+        cv::Point3f p_3d;
+        p_3d.x = point_msg->points[i].x;
+        p_3d.y = point_msg->points[i].y;
+        p_3d.z = point_msg->points[i].z;
+        Eigen::Vector3d tmp = posegraph.r_drift * Eigen::Vector3d(p_3d.x, p_3d.y, p_3d.z) + posegraph.t_drift;
+        geometry_msgs::Point32 p;
+        p.x = tmp(0);
+        p.y = tmp(1);
+        p.z = tmp(2);
+        point_cloud.points.push_back(p);
+    }
+    pub_margin_cloud.publish(point_cloud);
+//    ROS_INFO("exiting margin_point_callback");
+}
+
+void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
+{
+//    ROS_INFO("entering pose_callback");
+    m_buf.lock();
+    pose_buf.push(pose_msg);
+    m_buf.unlock();
+    /*
+    printf("[POSEGRAPH]: pose t: %f, %f, %f   q: %f, %f, %f %f \n", pose_msg->pose.pose.position.x,
+                                                       pose_msg->pose.pose.position.y,
+                                                       pose_msg->pose.pose.position.z,
+                                                       pose_msg->pose.pose.orientation.w,
+                                                       pose_msg->pose.pose.orientation.x,
+                                                       pose_msg->pose.pose.orientation.y,
+                                                       pose_msg->pose.pose.orientation.z);
+    */
+//    ROS_INFO("exiting pose_callback");
+}
+
+void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
+{
+//    ROS_INFO("entering vio_callback");
+    Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
+    Quaterniond vio_q;
+    vio_q.w() = pose_msg->pose.pose.orientation.w;
+    vio_q.x() = pose_msg->pose.pose.orientation.x;
+    vio_q.y() = pose_msg->pose.pose.orientation.y;
+    vio_q.z() = pose_msg->pose.pose.orientation.z;
+
+    vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio;
+    vio_q = posegraph.w_r_vio *  vio_q;
+
+    vio_t = posegraph.r_drift * vio_t + posegraph.t_drift;
+    vio_q = posegraph.r_drift * vio_q;
+
+    nav_msgs::Odometry odometry;
+    odometry.header = pose_msg->header;
+    odometry.header.frame_id = "global";
+    odometry.pose.pose.position.x = vio_t.x();
+    odometry.pose.pose.position.y = vio_t.y();
+    odometry.pose.pose.position.z = vio_t.z();
+    odometry.pose.pose.orientation.x = vio_q.x();
+    odometry.pose.pose.orientation.y = vio_q.y();
+    odometry.pose.pose.orientation.z = vio_q.z();
+    odometry.pose.pose.orientation.w = vio_q.w();
+    odometry.twist = pose_msg->twist;
+    odometry.pose.covariance = pose_msg->pose.covariance;
+    pub_odometry_rect.publish(odometry);
+
+    Vector3d vio_t_cam;
+    Quaterniond vio_q_cam;
+    vio_t_cam = vio_t + vio_q * tic;
+    vio_q_cam = vio_q * qic;        
+
+    cameraposevisual.reset();
+    cameraposevisual.add_pose(vio_t_cam, vio_q_cam);
+    cameraposevisual.publish_by(pub_camera_pose_visual, pose_msg->header);
+//    ROS_INFO("exiting vio_callback");
+}
+
+
+void vio_callback_pose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pose_msg)
+{
+//    ROS_INFO("entering vio_callback_pose");
+    Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
+    Quaterniond vio_q;
+    vio_q.w() = pose_msg->pose.pose.orientation.w;
+    vio_q.x() = pose_msg->pose.pose.orientation.x;
+    vio_q.y() = pose_msg->pose.pose.orientation.y;
+    vio_q.z() = pose_msg->pose.pose.orientation.z;
+
+    vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio;
+    vio_q = posegraph.w_r_vio *  vio_q;
+
+    vio_t = posegraph.r_drift * vio_t + posegraph.t_drift;
+    vio_q = posegraph.r_drift * vio_q;
+
+    geometry_msgs::PoseWithCovarianceStamped odometry;
+    odometry.header = pose_msg->header;
+    odometry.header.frame_id = "global";
+    odometry.pose.pose.position.x = vio_t.x();
+    odometry.pose.pose.position.y = vio_t.y();
+    odometry.pose.pose.position.z = vio_t.z();
+    odometry.pose.pose.orientation.x = vio_q.x();
+    odometry.pose.pose.orientation.y = vio_q.y();
+    odometry.pose.pose.orientation.z = vio_q.z();
+    odometry.pose.pose.orientation.w = vio_q.w();
+    odometry.pose.covariance = pose_msg->pose.covariance;
+    pub_pose_rect.publish(odometry);
+
+    Vector3d vio_t_cam;
+    Quaterniond vio_q_cam;
+    vio_t_cam = vio_t + vio_q * tic;
+    vio_q_cam = vio_q * qic;
+
+    cameraposevisual.reset();
+    cameraposevisual.add_pose(vio_t_cam, vio_q_cam);
+    cameraposevisual.publish_by(pub_camera_pose_visual, pose_msg->header);
+//    ROS_INFO("exiting vio_callback_pose");
+}
+
+void extrinsic_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
+{
+//    ROS_INFO("entering extrinsic_callback");
+    m_process.lock();
+    tic = Vector3d(pose_msg->pose.pose.position.x,
+                   pose_msg->pose.pose.position.y,
+                   pose_msg->pose.pose.position.z);
+    qic = Quaterniond(pose_msg->pose.pose.orientation.w,
+                      pose_msg->pose.pose.orientation.x,
+                      pose_msg->pose.pose.orientation.y,
+                      pose_msg->pose.pose.orientation.z).toRotationMatrix();
+    m_process.unlock();
+//    ROS_INFO("exiting extrinsic_callback");
+}
+
+
+void intrinsics_callback(const sensor_msgs::CameraInfo::ConstPtr &msg)
+{
+//    ROS_INFO("entering intrinsics_callback");
+    m_process.lock();
+    assert(msg->K.size()==9);
+    assert(msg->D.size()==4);
+    cv::Size imageSize(msg->width, msg->height);
+    if(msg->distortion_model == "plumb_bob") {
+        m_camera = camodocal::CameraFactory::instance()->generateCamera(camodocal::Camera::ModelType::PINHOLE, "cam0", imageSize);
+        std::vector<double> parameters;
+        parameters.push_back(msg->D.at(0));
+        parameters.push_back(msg->D.at(1));
+        parameters.push_back(msg->D.at(2));
+        parameters.push_back(msg->D.at(3));
+        parameters.push_back(msg->K.at(0));
+        parameters.push_back(msg->K.at(4));
+        parameters.push_back(msg->K.at(2));
+        parameters.push_back(msg->K.at(5));
+        m_camera.get()->readParameters(parameters);
+        max_focallength = std::max(msg->K.at(0), msg->K.at(4));
+    } else if(msg->distortion_model == "equidistant") {
+        m_camera = camodocal::CameraFactory::instance()->generateCamera(camodocal::Camera::ModelType::KANNALA_BRANDT, "cam0", imageSize);
+        std::vector<double> parameters;
+        parameters.push_back(msg->D.at(0));
+        parameters.push_back(msg->D.at(1));
+        parameters.push_back(msg->D.at(2));
+        parameters.push_back(msg->D.at(3));
+        parameters.push_back(msg->K.at(0));
+        parameters.push_back(msg->K.at(4));
+        parameters.push_back(msg->K.at(2));
+        parameters.push_back(msg->K.at(5));
+        m_camera.get()->readParameters(parameters);
+        max_focallength = std::max(msg->K.at(0), msg->K.at(4));
+    } else {
+        throw std::runtime_error("Invalid distorition model, unable to parse (plumb_bob, equidistant)");
+    }
+    m_process.unlock();
+//    ROS_INFO("exiting intrinsics_callback");
+}
+
+[[noreturn]] void process()
+{
+    while (true)
+    {
+        sensor_msgs::ImageConstPtr image_msg = NULL;
+        sensor_msgs::PointCloudConstPtr point_msg = NULL;
+        nav_msgs::Odometry::ConstPtr pose_msg = NULL;
+
+        // find out the messages with same time stamp
+        m_buf.lock();
+        if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty())
+        {
+            if (image_buf.front()->header.stamp.toSec() > pose_buf.front()->header.stamp.toSec())
+            {
+                pose_buf.pop();
+                printf("[POSEGRAPH]: throw pose at beginning\n");
+            }
+            else if (image_buf.front()->header.stamp.toSec() > point_buf.front()->header.stamp.toSec())
+            {
+                point_buf.pop();
+                printf("[POSEGRAPH]: throw point at beginning\n");
+            }
+            else if (image_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec() 
+                && point_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec())
+            {
+                pose_msg = pose_buf.front();
+                pose_buf.pop();
+                while (!pose_buf.empty())
+                    pose_buf.pop();
+                while (image_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
+                    image_buf.pop();
+                image_msg = image_buf.front();
+                image_buf.pop();
+
+                while (point_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
+                    point_buf.pop();
+                point_msg = point_buf.front();
+                point_buf.pop();
+            }
+        }
+        m_buf.unlock();
+
+        if (pose_msg != NULL)
+        {
+            //printf("[POSEGRAPH]:  pose time %f \n", pose_msg->header.stamp.toSec());
+            //printf("[POSEGRAPH]:  point time %f \n", point_msg->header.stamp.toSec());
+            //printf("[POSEGRAPH]:  image time %f \n", image_msg->header.stamp.toSec());
+            // skip fisrt few
+            if (skip_first_cnt < SKIP_FIRST_CNT)
+            {
+                skip_first_cnt++;
+                continue;
+            }
+
+            if (skip_cnt < SKIP_CNT)
+            {
+                skip_cnt++;
+                continue;
+            }
+            else
+            {
+                skip_cnt = 0;
+            }
+
+            cv_bridge::CvImageConstPtr ptr;
+            if (image_msg->encoding == "8UC1")
+            {
+                sensor_msgs::Image img;
+                img.header = image_msg->header;
+                img.height = image_msg->height;
+                img.width = image_msg->width;
+                img.is_bigendian = image_msg->is_bigendian;
+                img.step = image_msg->step;
+                img.data = image_msg->data;
+                img.encoding = "mono8";
+                ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
+            }
+            else
+                ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);
+            
+            cv::Mat image = ptr->image;
+            //cv::equalizeHist(image, image);
+
+            // build keyframe
+            Vector3d T = Vector3d(pose_msg->pose.pose.position.x,
+                                  pose_msg->pose.pose.position.y,
+                                  pose_msg->pose.pose.position.z);
+            Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w,
+                                     pose_msg->pose.pose.orientation.x,
+                                     pose_msg->pose.pose.orientation.y,
+                                     pose_msg->pose.pose.orientation.z).toRotationMatrix();
+            if((T - last_t).norm() > SKIP_DIS)
+            {
+                vector<cv::Point3f> point_3d; 
+                vector<cv::Point2f> point_2d_uv; 
+                vector<cv::Point2f> point_2d_normal;
+                vector<double> point_id;
+
+                for (unsigned int i = 0; i < point_msg->points.size(); i++)
+                {
+                    cv::Point3f p_3d;
+                    p_3d.x = point_msg->points[i].x;
+                    p_3d.y = point_msg->points[i].y;
+                    p_3d.z = point_msg->points[i].z;
+                    point_3d.push_back(p_3d);
+
+                    cv::Point2f p_2d_uv, p_2d_normal;
+                    double p_id;
+                    p_2d_normal.x = point_msg->channels[i].values[0];
+                    p_2d_normal.y = point_msg->channels[i].values[1];
+                    p_2d_uv.x = point_msg->channels[i].values[2];
+                    p_2d_uv.y = point_msg->channels[i].values[3];
+                    p_id = point_msg->channels[i].values[4];
+                    point_2d_normal.push_back(p_2d_normal);
+                    point_2d_uv.push_back(p_2d_uv);
+                    point_id.push_back(p_id);
+
+                    //printf("[POSEGRAPH]: u %f, v %f \n", p_2d_uv.x, p_2d_uv.y);
+                }
+
+                KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,
+                                   point_3d, point_2d_uv, point_2d_normal, point_id, sequence);   
+                m_process.lock();
+                start_flag = 1;
+                posegraph.addKeyFrame(keyframe, 1);
+                m_process.unlock();
+                frame_index++;
+                last_t = T;
+            }
+        }
+        std::chrono::milliseconds dura(5);
+        std::this_thread::sleep_for(dura);
+    }
+}
+
+[[noreturn]] void command()
+{
+    while(1)
+    {
+        char c = getchar();
+        if (c == 's')
+        {
+            m_process.lock();
+            posegraph.savePoseGraph();
+            m_process.unlock();
+            printf("[POSEGRAPH]: save pose graph finish\nyou can set 'load_previous_pose_graph' to 1 in the config file to reuse it next time\n");
+            printf("[POSEGRAPH]: program shutting down...\n");
+            ros::shutdown();
+        }
+        if (c == 'n')
+            new_sequence();
+
+        std::chrono::milliseconds dura(5);
+        std::this_thread::sleep_for(dura);
+    }
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "loop_fusion");
+    ros::NodeHandle n("~");
+    posegraph.registerPub(n);
+    
+    VISUALIZATION_SHIFT_X = 0;
+    VISUALIZATION_SHIFT_Y = 0;
+    SKIP_CNT = 0;
+    SKIP_DIS = 0;
+
+    if(argc != 2)
+    {
+        printf("[POSEGRAPH]: please intput: rosrun loop_fusion loop_fusion_node [config file] \n"
+               "for example: rosrun loop_fusion loop_fusion_node "
+               "/home/tony-ws1/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml \n");
+        return 0;
+    }
+    
+    string config_file = argv[1];
+    printf("[POSEGRAPH]: config_file: %s\n", argv[1]);
+
+    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
+    if(!fsSettings.isOpened())
+    {
+        std::cerr << "ERROR: Wrong path to settings" << std::endl;
+    }
+
+    cameraposevisual.setScale(0.1);
+    cameraposevisual.setLineWidth(0.01);
+
+
+    std::string pkg_path = ros::package::getPath("loop_fusion");
+    string vocabulary_file = pkg_path + "/../support_files/brief_k10L6.bin";
+    cout << "vocabulary_file" << vocabulary_file << endl;
+    posegraph.loadVocabulary(vocabulary_file);
+    BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml";
+    cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl;
+
+
+    //ROW = fsSettings["image_height"];
+    //COL = fsSettings["image_width"];
+    //int pn = config_file.find_last_of('/');
+    //std::string configPath = config_file.substr(0, pn);
+    //std::string cam0Calib;
+    //fsSettings["cam0_calib"] >> cam0Calib;
+    //std::string cam0Path = configPath + "/" + cam0Calib;
+    //printf("[POSEGRAPH]: cam calib path: %s\n", cam0Path.c_str());
+    //m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(cam0Path.c_str());
+
+
+    fsSettings["pose_graph_save_path"] >> POSE_GRAPH_SAVE_PATH;
+    fsSettings["output_path"] >> VINS_RESULT_PATH;
+    fsSettings["save_image"] >> DEBUG_IMAGE;
+    fsSettings["skip_dist"] >> SKIP_DIS;
+    fsSettings["skip_cnt"] >> SKIP_CNT;
+    fsSettings["min_score"] >> MIN_SCORE;
+    fsSettings["pnp_inflation"] >> PNP_INFLATION;
+    fsSettings["recall_ignore_recent_ct"] >> RECALL_IGNORE_RECENT_COUNT;
+    fsSettings["max_theta_diff"] >> MAX_THETA_DIFF;
+    fsSettings["max_pos_diff"] >> MAX_POS_DIFF;
+    fsSettings["min_loop_feat_num"] >> MIN_LOOP_NUM;
+
+    int LOAD_PREVIOUS_POSE_GRAPH;
+    LOAD_PREVIOUS_POSE_GRAPH = fsSettings["load_previous_pose_graph"];
+    VINS_RESULT_PATH = VINS_RESULT_PATH + "/vio_loop.csv";
+    std::ofstream fout(VINS_RESULT_PATH, std::ios::out);
+    fout.close();
+
+    int USE_IMU = fsSettings["imu"];
+    posegraph.setIMUFlag(USE_IMU);
+    fsSettings.release();
+
+    if (LOAD_PREVIOUS_POSE_GRAPH)
+    {
+        printf("[POSEGRAPH]: load pose graph\n");
+        m_process.lock();
+        posegraph.loadPoseGraph();
+        m_process.unlock();
+        printf("[POSEGRAPH]: load pose graph finish\n");
+        load_flag = 1;
+    }
+    else
+    {
+        printf("[POSEGRAPH]: no previous pose graph\n");
+        load_flag = 1;
+    }
+
+
+    // Get camera information
+    printf("[POSEGRAPH]: waiting for camera info topic...\n");
+    auto msg1 = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("/ov_msckf/camera_info", ros::Duration(ros::DURATION_MAX));
+    intrinsics_callback(msg1);
+    printf("[POSEGRAPH]: received camera info message!\n");
+    std::cout << m_camera.get()->parametersToString() << std::endl;
+
+    // Get camera to imu information
+    printf("[POSEGRAPH]: waiting for camera to imu extrinsics topic...\n");
+    auto msg2 = ros::topic::waitForMessage<nav_msgs::Odometry>("/ov_msckf/odomimu", ros::Duration(ros::DURATION_MAX));
+    extrinsic_callback(msg2);
+    printf("[POSEGRAPH]: received camera to imu extrinsics message!\n");
+    std::cout << qic.transpose() << std::endl;
+    std::cout << tic.transpose() << std::endl;
+
+    // Setup the rest of the publishers
+    ros::Subscriber sub_vio1 = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);
+    ros::Subscriber sub_vio2 = n.subscribe("/vins_estimator/pose", 2000, vio_callback_pose);
+    ros::Subscriber sub_image = n.subscribe("/cam0/image_raw", 2000, image_callback);
+    ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
+    ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);
+    ros::Subscriber sub_intrinsics = n.subscribe("/vins_estimator/intrinsics", 2000, intrinsics_callback);
+    ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);
+    // PI: it says that it's only for visualization
+    ros::Subscriber sub_margin_point = n.subscribe("/vins_estimator/margin_cloud", 2000, margin_point_callback);
+
+    pub_match_img = n.advertise<sensor_msgs::Image>("match_image", 1000);
+    pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
+    pub_point_cloud = n.advertise<sensor_msgs::PointCloud>("point_cloud_loop_rect", 1000);
+    pub_margin_cloud = n.advertise<sensor_msgs::PointCloud>("margin_cloud_loop_rect", 1000);
+    pub_odometry_rect = n.advertise<nav_msgs::Odometry>("odometry_rect", 1000);
+    pub_pose_rect = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_rect", 1000);
+
+    std::thread measurement_process;
+    std::thread keyboard_command_process;
+
+    measurement_process = std::thread(process);
+    keyboard_command_process = std::thread(command);
+    
+    ros::spin();
+
+    return 0;
+}

loop_fusion/src/utility/CameraPoseVisualization.cpp

@@ -0,0 +1,327 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *******************************************************/
+
+#include "CameraPoseVisualization.h"
+
+const Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0);
+const Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0);
+const Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0,  0.5, 1.0);
+const Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d( 1.0,  0.5, 1.0);
+const Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0);
+const Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0);
+const Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0);
+const Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0);
+
+void Eigen2Point(const Eigen::Vector3d& v, geometry_msgs::Point& p) {
+    p.x = v.x();
+    p.y = v.y();
+    p.z = v.z();
+}
+
+CameraPoseVisualization::CameraPoseVisualization(float r, float g, float b, float a)
+    : m_marker_ns("CameraPoseVisualization"), m_scale(0.2), m_line_width(0.01) {
+    m_image_boundary_color.r = r;
+    m_image_boundary_color.g = g;
+    m_image_boundary_color.b = b;
+    m_image_boundary_color.a = a;
+    m_optical_center_connector_color.r = r;
+    m_optical_center_connector_color.g = g;
+    m_optical_center_connector_color.b = b;
+    m_optical_center_connector_color.a = a;
+    LOOP_EDGE_NUM = 20;
+    tmp_loop_edge_num = 1;
+}
+
+void CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a) {
+    m_image_boundary_color.r = r;
+    m_image_boundary_color.g = g;
+    m_image_boundary_color.b = b;
+    m_image_boundary_color.a = a;
+}
+
+void CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) {
+    m_optical_center_connector_color.r = r;
+    m_optical_center_connector_color.g = g;
+    m_optical_center_connector_color.b = b;
+    m_optical_center_connector_color.a = a;
+}
+
+void CameraPoseVisualization::setScale(double s) {
+    m_scale = s;
+}
+void CameraPoseVisualization::setLineWidth(double width) {
+    m_line_width = width;
+}
+void CameraPoseVisualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){
+    visualization_msgs::Marker marker;
+
+    marker.ns = m_marker_ns;
+    marker.id = m_markers.size() + 1;
+    marker.type = visualization_msgs::Marker::LINE_LIST;
+    marker.action = visualization_msgs::Marker::ADD;
+    marker.scale.x = 0.01;
+
+    marker.color.b = 1.0f;
+    marker.color.a = 1.0;
+
+    geometry_msgs::Point point0, point1;
+
+    Eigen2Point(p0, point0);
+    Eigen2Point(p1, point1);
+
+    marker.points.push_back(point0);
+    marker.points.push_back(point1);
+
+    m_markers.push_back(marker);
+}
+
+void CameraPoseVisualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){
+    //m_markers.clear();
+    visualization_msgs::Marker marker;
+
+    marker.ns = m_marker_ns;
+    marker.id = m_markers.size() + 1;
+    //tmp_loop_edge_num++;
+    //if(tmp_loop_edge_num >= LOOP_EDGE_NUM)
+    //  tmp_loop_edge_num = 1;
+    marker.type = visualization_msgs::Marker::LINE_STRIP;
+    marker.action = visualization_msgs::Marker::ADD;
+    marker.lifetime = ros::Duration();
+    //marker.scale.x = 0.4;
+    marker.scale.x = 0.02;
+    marker.color.r = 1.0f;
+    //marker.color.g = 1.0f;
+    //marker.color.b = 1.0f;
+    marker.color.a = 1.0;
+
+    geometry_msgs::Point point0, point1;
+
+    Eigen2Point(p0, point0);
+    Eigen2Point(p1, point1);
+
+    marker.points.push_back(point0);
+    marker.points.push_back(point1);
+
+    m_markers.push_back(marker);
+}
+
+
+void CameraPoseVisualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) {
+    visualization_msgs::Marker marker;
+
+    marker.ns = m_marker_ns;
+    marker.id = 0;
+    marker.type = visualization_msgs::Marker::LINE_STRIP;
+    marker.action = visualization_msgs::Marker::ADD;
+    marker.scale.x = m_line_width;
+
+    marker.pose.position.x = 0.0;
+    marker.pose.position.y = 0.0;
+    marker.pose.position.z = 0.0;
+    marker.pose.orientation.w = 1.0;
+    marker.pose.orientation.x = 0.0;
+    marker.pose.orientation.y = 0.0;
+    marker.pose.orientation.z = 0.0;
+
+
+    geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2;
+
+    Eigen2Point(q * (m_scale *imlt) + p, pt_lt);
+    Eigen2Point(q * (m_scale *imlb) + p, pt_lb);
+    Eigen2Point(q * (m_scale *imrt) + p, pt_rt);
+    Eigen2Point(q * (m_scale *imrb) + p, pt_rb);
+    Eigen2Point(q * (m_scale *lt0 ) + p, pt_lt0);
+    Eigen2Point(q * (m_scale *lt1 ) + p, pt_lt1);
+    Eigen2Point(q * (m_scale *lt2 ) + p, pt_lt2);
+    Eigen2Point(q * (m_scale *oc  ) + p, pt_oc);
+
+    // image boundaries
+    marker.points.push_back(pt_lt);
+    marker.points.push_back(pt_lb);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    marker.points.push_back(pt_lb);
+    marker.points.push_back(pt_rb);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    marker.points.push_back(pt_rb);
+    marker.points.push_back(pt_rt);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    marker.points.push_back(pt_rt);
+    marker.points.push_back(pt_lt);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    // top-left indicator
+    marker.points.push_back(pt_lt0);
+    marker.points.push_back(pt_lt1);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    marker.points.push_back(pt_lt1);
+    marker.points.push_back(pt_lt2);
+    marker.colors.push_back(m_image_boundary_color);
+    marker.colors.push_back(m_image_boundary_color);
+
+    // optical center connector
+    marker.points.push_back(pt_lt);
+    marker.points.push_back(pt_oc);
+    marker.colors.push_back(m_optical_center_connector_color);
+    marker.colors.push_back(m_optical_center_connector_color);
+
+
+    marker.points.push_back(pt_lb);
+    marker.points.push_back(pt_oc);
+    marker.colors.push_back(m_optical_center_connector_color);
+    marker.colors.push_back(m_optical_center_connector_color);
+
+    marker.points.push_back(pt_rt);
+    marker.points.push_back(pt_oc);
+    marker.colors.push_back(m_optical_center_connector_color);
+    marker.colors.push_back(m_optical_center_connector_color);
+
+    marker.points.push_back(pt_rb);
+    marker.points.push_back(pt_oc);
+    marker.colors.push_back(m_optical_center_connector_color);
+    marker.colors.push_back(m_optical_center_connector_color);
+
+    m_markers.push_back(marker);
+}
+
+void CameraPoseVisualization::reset() {
+	m_markers.clear();
+    //image.points.clear();
+    //image.colors.clear();
+}
+
+void CameraPoseVisualization::publish_by( ros::Publisher &pub, const std_msgs::Header &header ) {
+	visualization_msgs::MarkerArray markerArray_msg;
+	//int k = (int)m_markers.size();
+  /*
+  for (int i = 0; i < 5 && k > 0; i++)
+  {
+    k--;
+    m_markers[k].header = header;
+    markerArray_msg.markers.push_back(m_markers[k]);
+  }
+  */
+
+  
+	for(auto& marker : m_markers) {
+		marker.header = header;
+		markerArray_msg.markers.push_back(marker);
+	}
+  
+	pub.publish(markerArray_msg);
+}
+
+void CameraPoseVisualization::publish_image_by( ros::Publisher &pub, const std_msgs::Header &header ) {
+    image.header = header;
+
+    pub.publish(image);
+}
+/*
+void CameraPoseVisualization::add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src)
+{
+    //image.points.clear();
+    //image.colors.clear();
+
+    image.ns = "image";
+    image.id = 0;
+    image.action = visualization_msgs::Marker::ADD;
+    image.type = visualization_msgs::Marker::TRIANGLE_LIST;
+    image.scale.x = 1;
+    image.scale.y = 1;
+    image.scale.z = 1;
+
+    geometry_msgs::Point p;
+    std_msgs::ColorRGBA crgb;
+
+    double center_x = src.rows / 2.0;
+    double center_y = src.cols / 2.0;
+
+    //double scale = 0.01;
+    double scale = IMAGE_VISUAL_SCALE;
+
+    for(int r = 0; r < src.cols; ++r) {
+        for(int c = 0; c < src.rows; ++c) {
+          float intensity = (float)( src.at<uchar>(c, r));
+          crgb.r = (float)intensity / 255.0;
+          crgb.g = (float)intensity / 255.0;
+          crgb.b = (float)intensity / 255.0;
+          crgb.a = 1.0;
+
+          Eigen::Vector3d p_cam, p_w;
+          p_cam.z() = 0;
+          p_cam.x() = (r - center_x) * scale;
+          p_cam.y() = (c - center_y) * scale; 
+          p_w = R * p_cam + T;
+          p.x = p_w(0);
+          p.y = p_w(1);
+          p.z = p_w(2);
+          image.points.push_back(p);
+          image.colors.push_back(crgb);
+
+          p_cam.z() = 0;
+          p_cam.x() = (r - center_x + 1) * scale;
+          p_cam.y() = (c - center_y) * scale; 
+          p_w = R * p_cam + T;
+          p.x = p_w(0);
+          p.y = p_w(1);
+          p.z = p_w(2);
+          image.points.push_back(p);
+          image.colors.push_back(crgb);
+
+          p_cam.z() = 0;
+          p_cam.x() = (r - center_x) * scale;
+          p_cam.y() = (c - center_y + 1) * scale; 
+          p_w = R * p_cam + T;
+          p.x = p_w(0);
+          p.y = p_w(1);
+          p.z = p_w(2);
+          image.points.push_back(p);
+          image.colors.push_back(crgb);
+
+          p_cam.z() = 0;
+          p_cam.x() = (r - center_x + 1) * scale;
+          p_cam.y() = (c - center_y) * scale; 
+          p_w = R * p_cam + T;
+          p.x = p_w(0);
+          p.y = p_w(1);
+          p.z = p_w(2);
+          image.points.push_back(p);
+          image.colors.push_back(crgb);
+
+          p_cam.z() = 0;
+          p_cam.x() = (r - center_x + 1) * scale;
+          p_cam.y() = (c - center_y + 1) * scale; 
+          p_w = R * p_cam + T;
+          p.x = p_w(0);
+          p.y = p_w(1);
+          p.z = p_w(2);
+          image.points.push_back(p);
+          image.colors.push_back(crgb);
+
+          p_cam.z() = 0;
+          p_cam.x() = (r - center_x) * scale;
+          p_cam.y() = (c - center_y + 1) * scale; 
+          p_w = R * p_cam + T;
+          p.x = p_w(0);
+          p.y = p_w(1);
+          p.z = p_w(2);
+          image.points.push_back(p);
+          image.colors.push_back(crgb);
+        }
+    }
+}
+*/

loop_fusion/src/utility/CameraPoseVisualization.h

@@ -0,0 +1,58 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *******************************************************/
+
+#pragma once
+
+#include <ros/ros.h>
+#include <std_msgs/ColorRGBA.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <opencv2/opencv.hpp>
+#include "../parameters.h"
+
+class CameraPoseVisualization {
+public:
+	std::string m_marker_ns;
+
+	CameraPoseVisualization(float r, float g, float b, float a);
+	
+	void setImageBoundaryColor(float r, float g, float b, float a=1.0);
+	void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0);
+	void setScale(double s);
+	void setLineWidth(double width);
+
+	void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);
+	void reset();
+
+	void publish_by(ros::Publisher& pub, const std_msgs::Header& header);
+	void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
+	void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
+	//void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src);
+	void publish_image_by( ros::Publisher &pub, const std_msgs::Header &header);
+private:
+	std::vector<visualization_msgs::Marker> m_markers;
+	std_msgs::ColorRGBA m_image_boundary_color;
+	std_msgs::ColorRGBA m_optical_center_connector_color;
+	double m_scale;
+	double m_line_width;
+	visualization_msgs::Marker image;
+	int LOOP_EDGE_NUM;
+	int tmp_loop_edge_num;
+
+	static const Eigen::Vector3d imlt;
+	static const Eigen::Vector3d imlb;
+	static const Eigen::Vector3d imrt;
+	static const Eigen::Vector3d imrb;
+	static const Eigen::Vector3d oc  ;
+	static const Eigen::Vector3d lt0 ;
+	static const Eigen::Vector3d lt1 ;
+	static const Eigen::Vector3d lt2 ;
+};

loop_fusion/src/utility/Grider_FAST.h

@@ -0,0 +1,120 @@
+/**
+ * MIT License
+ * Copyright (c) 2018 Patrick Geneva @ University of Delaware (Robot Perception & Navigation Group)
+ * Copyright (c) 2018 Kevin Eckenhoff @ University of Delaware (Robot Perception & Navigation Group)
+ * Copyright (c) 2018 Guoquan Huang @ University of Delaware (Robot Perception & Navigation Group)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+
+#ifndef MSCKF_GRIDER_FAST_H
+#define MSCKF_GRIDER_FAST_H
+
+
+#include <vector>
+#include <iostream>
+#include <Eigen/Eigen>
+
+
+#include <opencv/cv.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+
+class Grider_FAST {
+
+public:
+
+
+    /**
+     * Compare keypoints based on their response value
+     * We want to have the keypoints with the highest values!
+     * https://stackoverflow.com/a/10910921
+     */
+    static bool compare_response(cv::KeyPoint first, cv::KeyPoint second) {
+        return first.response > second.response;
+    }
+
+
+    /**
+     * This function will perform grid extraction using FAST
+     * Given a specified grid size, this will try to extract fast features from each grid
+     * It will then return the best from each grid
+     */
+    static void perform_griding(const cv::Mat &img, std::vector<cv::KeyPoint> &pts, int num_features,
+                                int grid_x, int grid_y, int threshold, bool nonmaxSuppression) {
+
+        // Calculate the size our extraction boxes should be
+        int size_x = img.cols / grid_x;
+        int size_y = img.rows / grid_y;
+
+        // Make sure our sizes are not zero
+        assert(size_x > 0);
+        assert(size_y > 0);
+
+        // We want to have equally distributed features
+        auto num_features_grid = (int) (num_features / (grid_x * grid_y)) + 1;
+
+        // Lets loop through each grid and extract features
+        for (int x = 0; x < img.cols; x += size_x) {
+            for (int y = 0; y < img.rows; y += size_y) {
+
+                // Skip if we are out of bounds
+                if (x + size_x > img.cols || y + size_y > img.rows)
+                    continue;
+
+                // Calculate where we should be extracting from
+                cv::Rect img_roi = cv::Rect(x, y, size_x, size_y);
+
+                // Extract FAST features for this part of the image
+                std::vector<cv::KeyPoint> pts_new;
+                cv::FAST(img(img_roi), pts_new, threshold, nonmaxSuppression);
+
+                // Now lets get the top number from this
+                std::sort(pts_new.begin(), pts_new.end(), Grider_FAST::compare_response);
+
+                // Debug print out the response values
+                //for (auto pt : pts_new) {
+                //    std::cout << pt.response << std::endl;
+                //}
+                //std::cout << "======================" << std::endl;
+
+                // Append the "best" ones to our vector
+                // Note that we need to "correct" the point u,v since we extracted it in a ROI
+                // So we should append the location of that ROI in the image
+                for(size_t i=0; i<(size_t)num_features_grid && i<pts_new.size(); i++) {
+                    cv::KeyPoint pt_cor = pts_new.at(i);
+                    pt_cor.pt.x += (float)x;
+                    pt_cor.pt.y += (float)y;
+                    pts.push_back(pt_cor);
+                }
+
+
+            }
+        }
+
+
+    }
+
+
+};
+
+
+#endif /* MSCKF_GRIDER_FAST_H */

loop_fusion/src/utility/tic_toc.h

@@ -0,0 +1,38 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *******************************************************/
+
+#pragma once
+
+#include <ctime>
+#include <cstdlib>
+#include <chrono>
+
+class TicToc
+{
+  public:
+    TicToc()
+    {
+        tic();
+    }
+
+    void tic()
+    {
+        start = std::chrono::system_clock::now();
+    }
+
+    double toc()
+    {
+        end = std::chrono::system_clock::now();
+        std::chrono::duration<double> elapsed_seconds = end - start;
+        return elapsed_seconds.count() * 1000;
+    }
+
+  private:
+    std::chrono::time_point<std::chrono::system_clock> start, end;
+};

loop_fusion/src/utility/utility.cpp

@@ -0,0 +1,22 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *******************************************************/
+
+#include "utility.h"
+
+Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)
+{
+    Eigen::Matrix3d R0;
+    Eigen::Vector3d ng1 = g.normalized();
+    Eigen::Vector3d ng2{0, 0, 1.0};
+    R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();
+    double yaw = Utility::R2ypr(R0).x();
+    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
+    // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0;
+    return R0;
+}

loop_fusion/src/utility/utility.h

@@ -0,0 +1,149 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *******************************************************/
+
+#pragma once
+
+#include <cmath>
+#include <cassert>
+#include <cstring>
+#include <eigen3/Eigen/Dense>
+
+class Utility
+{
+  public:
+    template <typename Derived>
+    static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
+    {
+        typedef typename Derived::Scalar Scalar_t;
+
+        Eigen::Quaternion<Scalar_t> dq;
+        Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
+        half_theta /= static_cast<Scalar_t>(2.0);
+        dq.w() = static_cast<Scalar_t>(1.0);
+        dq.x() = half_theta.x();
+        dq.y() = half_theta.y();
+        dq.z() = half_theta.z();
+        return dq;
+    }
+
+    template <typename Derived>
+    static Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Derived> &q)
+    {
+        Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;
+        ans << typename Derived::Scalar(0), -q(2), q(1),
+            q(2), typename Derived::Scalar(0), -q(0),
+            -q(1), q(0), typename Derived::Scalar(0);
+        return ans;
+    }
+
+    template <typename Derived>
+    static Eigen::Quaternion<typename Derived::Scalar> positify(const Eigen::QuaternionBase<Derived> &q)
+    {
+        //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z());
+        //Eigen::Quaternion<typename Derived::Scalar> p(-q.w(), -q.x(), -q.y(), -q.z());
+        //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z());
+        //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion<typename Derived::Scalar>(-q.w(), -q.x(), -q.y(), -q.z());
+        return q;
+    }
+
+    template <typename Derived>
+    static Eigen::Matrix<typename Derived::Scalar, 4, 4> Qleft(const Eigen::QuaternionBase<Derived> &q)
+    {
+        Eigen::Quaternion<typename Derived::Scalar> qq = positify(q);
+        Eigen::Matrix<typename Derived::Scalar, 4, 4> ans;
+        ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose();
+        ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix<typename Derived::Scalar, 3, 3>::Identity() + skewSymmetric(qq.vec());
+        return ans;
+    }
+
+    template <typename Derived>
+    static Eigen::Matrix<typename Derived::Scalar, 4, 4> Qright(const Eigen::QuaternionBase<Derived> &p)
+    {
+        Eigen::Quaternion<typename Derived::Scalar> pp = positify(p);
+        Eigen::Matrix<typename Derived::Scalar, 4, 4> ans;
+        ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose();
+        ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix<typename Derived::Scalar, 3, 3>::Identity() - skewSymmetric(pp.vec());
+        return ans;
+    }
+
+    static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R)
+    {
+        Eigen::Vector3d n = R.col(0);
+        Eigen::Vector3d o = R.col(1);
+        Eigen::Vector3d a = R.col(2);
+
+        Eigen::Vector3d ypr(3);
+        double y = atan2(n(1), n(0));
+        double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
+        double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
+        ypr(0) = y;
+        ypr(1) = p;
+        ypr(2) = r;
+
+        return ypr / M_PI * 180.0;
+    }
+
+    template <typename Derived>
+    static Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr2R(const Eigen::MatrixBase<Derived> &ypr)
+    {
+        typedef typename Derived::Scalar Scalar_t;
+
+        Scalar_t y = ypr(0) / 180.0 * M_PI;
+        Scalar_t p = ypr(1) / 180.0 * M_PI;
+        Scalar_t r = ypr(2) / 180.0 * M_PI;
+
+        Eigen::Matrix<Scalar_t, 3, 3> Rz;
+        Rz << cos(y), -sin(y), 0,
+            sin(y), cos(y), 0,
+            0, 0, 1;
+
+        Eigen::Matrix<Scalar_t, 3, 3> Ry;
+        Ry << cos(p), 0., sin(p),
+            0., 1., 0.,
+            -sin(p), 0., cos(p);
+
+        Eigen::Matrix<Scalar_t, 3, 3> Rx;
+        Rx << 1., 0., 0.,
+            0., cos(r), -sin(r),
+            0., sin(r), cos(r);
+
+        return Rz * Ry * Rx;
+    }
+
+    static Eigen::Matrix3d g2R(const Eigen::Vector3d &g);
+
+    template <size_t N>
+    struct uint_
+    {
+    };
+
+    template <size_t N, typename Lambda, typename IterT>
+    void unroller(const Lambda &f, const IterT &iter, uint_<N>)
+    {
+        unroller(f, iter, uint_<N - 1>());
+        f(iter + N);
+    }
+
+    template <typename Lambda, typename IterT>
+    void unroller(const Lambda &f, const IterT &iter, uint_<0>)
+    {
+        f(iter);
+    }
+
+    template <typename T>
+    static T normalizeAngle(const T& angle_degrees) {
+      T two_pi(2.0 * 180);
+      if (angle_degrees > 0)
+      return angle_degrees -
+          two_pi * std::floor((angle_degrees + T(180)) / two_pi);
+      else
+        return angle_degrees +
+            two_pi * std::floor((-angle_degrees + T(180)) / two_pi);
+    };
+};