Merge pull request #4 from heethesh/ayush-devel

Ayush devel
heethesh · Apr 26, 2020 · 013e666 · 013e666
2 parents cda1fe2 + 400cce8
commit 013e666
Show file tree

Hide file tree

Showing 2 changed files with 122 additions and 1 deletion.
diff --git a/orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/orb_slam_2_ros/orb_slam2/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h
@@ -26,6 +26,8 @@
 #include <algorithm>
 #include <opencv2/core/core.hpp>
 #include <limits>
+#include <stdlib.h>
+#include <time.h>
 
 #include "FeatureVector.h"
 #include "BowVector.h"
@@ -35,6 +37,12 @@
 
 using namespace std;
 
+namespace {
+  constexpr float rrThresh = 0.7f;
+  constexpr float repeatThresh = 0.7f;
+  constexpr float dynamicThresh = 0.7f;
+}
+
 namespace DBoW2 {
 
 /// @param TDescriptor class of descriptor
@@ -145,6 +153,20 @@ class TemplatedVocabulary
   virtual void transform(const std::vector<TDescriptor>& features,
     BowVector &v, FeatureVector &fv, int levelsup) const;
 
+  /**
+   * SALSA AYUSH
+   * Transform a set of descriptors into a bow vector and a feature vector
+   * And using the heuristics to determine which descriptors are good for creating bow vector
+   * @param features
+   * @param v (out) bow vector
+   * @param fv (out) feature vector of nodes and feature indexes
+   * @param levelsup levels to go up the vocabulary tree to get the node index
+   */
+  virtual void transform(const std::vector<TDescriptor>& features,
+    BowVector &v, FeatureVector &fv, int levelsup,
+    std::vector<float>& mvScoreDynamic, std::vector<float>&  mvScoreRepeatable) const;
+
+
   /**
    * Transforms a single feature into a word (without weight)
    * @param feature
@@ -1215,6 +1237,99 @@ inline double TemplatedVocabulary<TDescriptor,F>::score
   return m_scoring_object->score(v1, v2);
 }
 
+// --------------------------------------------------------------------------
+//--------------------------------------------------------------------------
+//--------------------------------------------------------------------------
+
+//SALSA AYUSH
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::transform(
+  const std::vector<TDescriptor>& features,
+  BowVector &v, FeatureVector &fv, int levelsup,
+  std::vector<float>& mvScoreDynamic, std::vector<float>&  mvScoreRepeatable) const
+{
+  srand(time(NULL));
+  v.clear();
+  fv.clear();
+
+  if(empty()) // safe for subclasses
+  {
+    return;
+  }
+
+  // normalize
+  LNorm norm;
+  bool must = m_scoring_object->mustNormalize(norm);
+
+  typename vector<TDescriptor>::const_iterator fit;
+
+  if(m_weighting == TF || m_weighting == TF_IDF)
+  {
+    unsigned int i_feature = 0;
+    int i = 0;
+
+    cout << features.size() << endl;
+    cout << mvScoreRepeatable.size() << endl;
+    cout << mvScoreDynamic.size() << endl;
+    for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
+    {
+
+      WordId id;
+      NodeId nid;
+      WordValue w;
+      // w is the idf value if TF_IDF, 1 if TF
+      // cout << "mvScoreRepeatable[i_feature] " << mvScoreRepeatable[i_feature] << " mvScoreDynamic[i_feature] " << mvScoreDynamic[i_feature] << endl; 
+      if ( (mvScoreRepeatable[i_feature] > repeatThresh &&  mvScoreDynamic[i_feature] > dynamicThresh) 
+        && ((float)rand()/RAND_MAX) > rrThresh)
+      {
+        i++;
+        continue;
+      } 
+
+      transform(*fit, id, w, &nid, levelsup);
+
+      if(w > 0) // not stopped
+      {
+        v.addWeight(id, w);
+        fv.addFeature(nid, i_feature);
+      }
+    }
+    cout << "Dropped : " << i << endl;
+    if(!v.empty() && !must)
+    {
+      // unnecessary when normalizing
+      const double nd = v.size();
+      for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++)
+        vit->second /= nd;
+    }
+
+  }
+  else // IDF || BINARY
+  {
+    unsigned int i_feature = 0;
+    for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
+    {
+      WordId id;
+      NodeId nid;
+      WordValue w;
+      // w is idf if IDF, or 1 if BINARY
+
+      cout << "HERE" << endl;
+
+      transform(*fit, id, w, &nid, levelsup);
+
+      if(w > 0) // not stopped
+      {
+        v.addIfNotExist(id, w);
+        fv.addFeature(nid, i_feature);
+      }
+    }
+  } // if m_weighting == ...
+
+  if(must) v.normalize(norm);
+}
+
+//----------------------------------------------------------------------------
 // --------------------------------------------------------------------------
 
 template<class TDescriptor, class F>

diff --git a/orb_slam_2_ros/orb_slam2/src/Frame.cc b/orb_slam_2_ros/orb_slam2/src/Frame.cc
@@ -22,6 +22,7 @@
 #include "Converter.h"
 #include "ORBmatcher.h"
 #include <thread>
+#define SALSA_BOW
 #define SALSA
 namespace ORB_SLAM2 {
 
@@ -450,7 +451,12 @@ bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY) {
 void Frame::ComputeBoW() {
   if (mBowVec.empty()) {
     vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
-    mpORBvocabulary->transform(vCurrentDesc, mBowVec, mFeatVec, 4);
+
+    #ifdef SALSA_BOW
+    mpORBvocabulary->transform(vCurrentDesc, mBowVec, mFeatVec, 4, mvScoreDynamic, mvScoreRepeatable);
+    #else
+      mpORBvocabulary->transform(vCurrentDesc, mBowVec, mFeatVec, 4, mvScoreDynamic);
+    #endif
   }
 }