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 index 4f4ca45..bde8cd9 100755 --- 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 #include #include +#include +#include #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& 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& features, + BowVector &v, FeatureVector &fv, int levelsup, + std::vector& mvScoreDynamic, std::vector& mvScoreRepeatable) const; + + /** * Transforms a single feature into a word (without weight) * @param feature @@ -1215,6 +1237,99 @@ inline double TemplatedVocabulary::score return m_scoring_object->score(v1, v2); } +// -------------------------------------------------------------------------- +//-------------------------------------------------------------------------- +//-------------------------------------------------------------------------- + +//SALSA AYUSH +template +void TemplatedVocabulary::transform( + const std::vector& features, + BowVector &v, FeatureVector &fv, int levelsup, + std::vector& mvScoreDynamic, std::vector& 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::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 diff --git a/orb_slam_2_ros/orb_slam2/src/Frame.cc b/orb_slam_2_ros/orb_slam2/src/Frame.cc index 3e04fd5..61b29bc 100644 --- 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 +#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 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 } }