AllTimeTracker.hpp

#ifndef GFM_ALL_TIME_TRACKER
#define GFM_ALL_TIME_TRACKER

#include "gfm/EngineTracking.hpp"
#include <set>

namespace jafar {
  namespace gfm_v2 {
    
    template<typename DESCRIPTOR>
    class AllTimeTracker : public gfm_v2::EngineTracking<DESCRIPTOR> {

    public:
      typedef fdetect_v2::InterestFeature<DESCRIPTOR> Feature;
      typedef fdetect_v2::DetectionResult<DESCRIPTOR> DR;
      typedef gfm_v2::MatchingResult<DESCRIPTOR> MR;

    private:
      struct features_comp {
        bool operator() (const Feature* lhf, const Feature* rhf) const 
        {
          return lhf->index() < rhf->index();
        }
      };
      std::map<unsigned int, std::set< Feature*, features_comp > > unmatched;
      typedef typename std::map<unsigned int, typename std::set< Feature*, features_comp > >::iterator unmatched_iterator;
      unsigned int global_id;
      unsigned int initial_id;
      unsigned int second_id;
      DR previous_points;
      MR current_match;
      DR current_unmatched;
      void matchWithOthers ( DR& local_unmatched )
      {
        using namespace std;
        MatchSourceInfo<DESCRIPTOR>* match_info = this->m_matcher->initMatchSourceInfo(local_unmatched);
        MatchSourceInfo<DESCRIPTOR>* source_info;

        for(unmatched_iterator uit = unmatched.begin();
            uit != unmatched.end();
            ++uit) {
          DR source;
          source.insert(uit->second.begin(), uit->second.end());
          source.id = uit->first;
          source_info = this->m_matcher->initMatchSourceInfo(source);
          MR mr = this->m_matcher->computeMatch(source_info, match_info);
          JFR_DEBUG("Found " << mr.size() << " matches");
          if (mr.size() != 0) {
            mr.propagateId(global_id);
            for (typename MR::const_iterator mit = mr.begin(); mit != mr.end(); ++mit) {
              if(mit->point_ref->id() != geom::InterestPoint::NO_ID) {
                unmatched_iterator finder = unmatched.find(mit->point_ref->parent_id);
                JFR_ASSERT(finder != unmatched.end(), 
                           "no DR with id "<<mit->point_ref->parent_id<<" found!")
                  finder->second.erase(mit->point_ref);
                current_match.push_back(mit->point_ref, mit->point_match);
              } 
            }
          }         
        }
      }

    public:

      AllTimeTracker(jafar::fdetect_v2::Detector<DESCRIPTOR>* detector, 
                     jafar::gfm_v2::Matcher<DESCRIPTOR>* matcher, 
                     unsigned int start = 0,
                     jafar::fdetect_v2::DescriptorFactory<DESCRIPTOR>* descriptor = 0) :
        gfm_v2::EngineTracking<DESCRIPTOR>(detector, matcher, descriptor), 
        global_id(start), initial_id(-1), second_id(-1) { }

      void initTracking(jafar::image::Image const& image_ref, 
                        unsigned int id)
      {
        this->initTracking(image_ref, jafar::image::Image(), id);
      }
      
      void initTracking(jafar::image::Image const& image_ref, 
                        jafar::image::Image const& mask_ref, 
                        unsigned int id)
      {
        JFR_PRECOND(id != -1,
                    "id must be different from -1")
          initial_id = id;
        EngineTracking<DESCRIPTOR>::initTracking(image_ref, mask_ref, id);
      }
      
      void initTracking(fdetect_v2::DetectionResult<DESCRIPTOR> &points_ref, 
                        unsigned int id)
      {
        JFR_PRECOND(id != -1,
                    "id must be different from -1")
          initial_id = id;
        EngineTracking<DESCRIPTOR>::initTracking(points_ref, id);
      }

      MR nextImage(jafar::image::Image const& image_next, unsigned int id)
      {
        return this->nextImage(image_next, jafar::image::Image(), id);
      }

      MR nextImage(jafar::image::Image const& image_next, 
                   jafar::image::Image const& mask_next, 
                   unsigned int id)
      {
        using namespace std;
        JFR_PRECOND(initial_id != -1,
                    "call initTracking() first!")
          JFR_PRECOND(id != -1,
                      "id must be different from -1!")
          using namespace fdetect_v2;
        //we need more than 2 sources to do the magic
        previous_points = this->currentPoints();
        DR points_match = this->m_detector->detectIn(image_next);
        points_match.id = id;
        for ( typename DR::iterator it = points_match.begin();
              it != points_match.end();
              ++it)
          (*it)->parent_id = id;
        if(this->m_descriptorFactory)
        {
          this->m_descriptorFactory->createDescriptor(points_match, image_next);
        }
        MatchSourceInfo<DESCRIPTOR>* infomatch = this->m_matcher->initMatchSourceInfo(points_match);
        this->m_sourceInfoRef->reinit();
        current_match = this->m_matcher->computeMatch(this->m_sourceInfoRef, infomatch);
        JFR_DEBUG("Found " << current_match.size() << " matches");
        delete this->m_sourceInfoRef;
        current_match.propagateId(global_id);
        //this is the first call to nextImage()
        if(second_id == -1) {
          //populate unmatched with all the not matched features from init
          for(typename DR::iterator fit = previous_points.begin(); 
              fit != previous_points.end(); 
              ++fit) {
            if((*fit)->id() == geom::InterestPoint::NO_ID)
              unmatched[initial_id].insert(*fit);
          }
          second_id = id;
        } else {
          current_unmatched.clear();
          //populate the local unmatched with all the non matched features
          for(typename DR::const_iterator fit = points_match.begin(); 
              fit != points_match.end(); 
              ++fit) {
            if((*fit)->id() == geom::InterestPoint::NO_ID) {
              current_unmatched.push_back(*fit);
            }
          }
          matchWithOthers(current_unmatched);
          JFR_DEBUG("After others matching, found " << current_match.size() << " matches")
            //populate the global unmatched with all the previous non matched features 
            typename DR::iterator fit = previous_points.begin(); 
          unsigned int prev_id = (*fit)->parent_id;
          for(; fit != previous_points.end(); ++fit) {
            if((*fit)->id() == geom::InterestPoint::NO_ID)
              unmatched[prev_id].insert(*fit);
          }
        }
        //delete this->m_sourceInfoRef;
        this->m_sourceInfoRef = this->m_matcher->initMatchSourceInfo(points_match);
        return current_match;
      }
        
      MR nextImage(DR &points_next, 
                   unsigned int id = geom::InterestPoint::NO_ID)
      {
        using namespace std;
        JFR_PRECOND(initial_id != -1,
                    "call initTracking() first!")
          JFR_PRECOND(id != -1,
                      "id must be different from -1!")
          using namespace fdetect_v2;
        //we need more than 2 sources to do the magic
        previous_points = this->currentPoints();
        points_next.id = id;
        for(typename DR::iterator it = points_next.begin();
            it != points_next.end();
            ++it)
          (*it)->parent_id = id;

        MatchSourceInfo<DESCRIPTOR>* infomatch = this->m_matcher->initMatchSourceInfo(points_next);
        this->m_sourceInfoRef->reinit();
        current_match = this->m_matcher->computeMatch(this->m_sourceInfoRef, infomatch);
        JFR_DEBUG("Found " << current_match.size() << " matches");
        delete this->m_sourceInfoRef;
        current_match.propagateId(global_id);
        //this is the first call to nextImage()
        if(second_id == -1) {
          //populate unmatched with all the not matched features from init
          for(typename DR::iterator fit = previous_points.begin(); 
              fit != previous_points.end(); 
              ++fit) {
            if((*fit)->id() == geom::InterestPoint::NO_ID)
              unmatched[initial_id].insert(*fit);
          }
          second_id = id;
        } else {
          current_unmatched.clear();
          //populate the local unmatched with all the non matched features
          for(typename DR::const_iterator fit = points_next.begin(); 
              fit != points_next.end(); 
              ++fit) {
            if((*fit)->id() == geom::InterestPoint::NO_ID) {
              current_unmatched.push_back(*fit);
            }
          }
          matchWithOthers(current_unmatched);
          JFR_DEBUG("After others matching, found " << current_match.size() << " matches")
            //populate the global unmatched with all the previous non matched features 
            typename DR::iterator fit = previous_points.begin(); 
          unsigned int prev_id = (*fit)->parent_id;
          for(; fit != previous_points.end(); ++fit) {
            if((*fit)->id() == geom::InterestPoint::NO_ID)
              unmatched[prev_id].insert(*fit);
          }
        }
        //        delete this->m_sourceInfoRef;
        this->m_sourceInfoRef = this->m_matcher->initMatchSourceInfo(points_next);
        return current_match;
      }

    };//class AllTimeTracker

  }//namespace gfm_v2

}//namespace jafar

#endif