EngineTracking.hpp

/* $Id$ */

#ifndef _GFM_ENGINETRACKING_HPP_
#define _GFM_ENGINETRACKING_HPP_

#include "image/Image.hpp"
#include "fdetect/InterestFeature.hpp"
#include "fdetect/DetectionResult.hpp"
#include "fdetect/Detector.hpp"
#include "fdetect/Descriptor.hpp"
#include "gfm/Matcher.hpp"
#include "gfm/MatchingResult.hpp"


#include <boost/serialization/nvp.hpp>
#include <boost/serialization/utility.hpp>
#include <boost/serialization/version.hpp>
#include <boost/noncopyable.hpp>

namespace jafar {
  namespace gfm {
    // class MatchSourceInfo;
    // class DescriptorFactory;
    // class Detector;
    // class Matcher;
    // class MatchingResult;
    class EngineTracking: public boost::noncopyable {
      public:
      EngineTracking(jafar::fdetect::Detector* detector, 
                     jafar::gfm::Matcher* matcher, 
                     jafar::fdetect::DescriptorFactory* descriptor = 0) : 
        m_detector(detector), 
        m_descriptorFactory(descriptor), 
        m_matcher(matcher), 
        m_sourceInfoRef(0) { }
        ~EngineTracking();
      public:
        const jafar::fdetect::DetectionResult& currentPoints() const;
      void initTracking(jafar::image::Image const& image_ref, 
                        unsigned int id = geom::InterestPoint::NO_ID);
      jafar::gfm::MatchingResult nextImage(jafar::image::Image const& image_next, 
                                           unsigned int id = geom::InterestPoint::NO_ID);
      protected:
        inline MatchSourceInfo* sourceInfoRef() const { return m_sourceInfoRef; }
        inline Matcher* matcher() { return m_matcher; }
        inline fdetect::DescriptorFactory* descriptorFactory() { return m_descriptorFactory; }
        inline fdetect::Detector* detector() { return m_detector; }
      private:
        fdetect::Detector* m_detector;
        fdetect::DescriptorFactory* m_descriptorFactory;
        Matcher* m_matcher;
        MatchSourceInfo* m_sourceInfoRef;
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive &ar, const unsigned int version)
      {
        ar & BOOST_SERIALIZATION_NVP(m_detector);
        ar & BOOST_SERIALIZATION_NVP(m_descriptorFactory);
        ar & BOOST_SERIALIZATION_NVP(m_matcher);
        //ar & BOOST_SERIALIZATION_NVP(m_sourceInfoRef);
      }
    };

    class EngineStereoTracking : public EngineTracking {
      public:
        EngineStereoTracking(jafar::fdetect::Detector* detector,
                             jafar::gfm::Matcher* matcher,
                             jafar::gfm::Matcher* stereomatcher,
                             jafar::fdetect::DescriptorFactory* descriptor = 0);
        ~EngineStereoTracking();
      public:
        jafar::gfm::MatchingResult matchCurrentWith(jafar::image::Image const& image_match );
      private:
        Matcher* m_stereoMatcher;
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive &ar, const unsigned int version)
      {
        ar & BOOST_SERIALIZATION_NVP(m_stereoMatcher);
      }
    };
  }

  namespace gfm_v2 {
    template<typename InterestFeatureT>
    class EngineTracking: public boost::noncopyable {
      public:
      EngineTracking(jafar::fdetect_v2::Detector<InterestFeatureT>* detector, 
                     jafar::gfm_v2::Matcher<InterestFeatureT>* matcher, 
                     jafar::fdetect_v2::DescriptorFactory<InterestFeatureT>* descriptor = 0) : 
        m_detector(detector), 
        m_descriptorFactory(descriptor), 
        m_matcher(matcher), 
        m_sourceInfoRef(0) { }
      ~EngineTracking()
      {
        delete m_detector;
        if(m_descriptorFactory)
          delete m_descriptorFactory;
        delete m_matcher;
        if(m_sourceInfoRef)
          delete m_sourceInfoRef;
      }

      const jafar::fdetect_v2::DetectionResult<InterestFeatureT>& currentPoints() const
      {
        return m_sourceInfoRef->points();
      }

      void initTracking(jafar::image::Image const& image_ref, 
                        unsigned int id = geom::InterestPoint::NO_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 = geom::InterestPoint::NO_ID)
      {
        using namespace fdetect_v2;
        DetectionResult<InterestFeatureT> points_ref = m_detector->detectIn(image_ref, mask_ref, true);
        if(id != geom::InterestPoint::NO_ID) 
        {
          points_ref.id = id;
          for (typename DetectionResult<InterestFeatureT>::iterator it = points_ref.begin();
               it != points_ref.end();
               ++it)
            (*it)->parent_id = id;
        }
        if(m_descriptorFactory)
        {
          m_descriptorFactory->createDescriptor(points_ref, image_ref);
        }
        m_sourceInfoRef = m_matcher->initMatchSourceInfo(points_ref);
      }

      void initTracking(fdetect_v2::DetectionResult<InterestFeatureT> &points_ref, 
                        unsigned int id = geom::InterestPoint::NO_ID)
      {
        using namespace fdetect_v2;
        if(id != geom::InterestPoint::NO_ID) 
        {
          points_ref.id = id;
          for (typename DetectionResult<InterestFeatureT>::iterator it = points_ref.begin();
               it != points_ref.end();
               ++it)
            (*it)->parent_id = id;
        }
        m_sourceInfoRef = m_matcher->initMatchSourceInfo(points_ref);
      }

      MatchingResult<InterestFeatureT> nextImage(jafar::image::Image const& image_next, 
                                           unsigned int id = geom::InterestPoint::NO_ID)
      {
        return this->nextImage(image_next, jafar::image::Image(), id);
      }

      MatchingResult<InterestFeatureT> nextImage(jafar::image::Image const& image_next,
                                           jafar::image::Image const& mask_next,
                                           unsigned int id = geom::InterestPoint::NO_ID)
      {
        using namespace fdetect_v2;
        DetectionResult<InterestFeatureT> points_match = m_detector->detectIn(image_next, mask_next);
        if(id != geom::InterestPoint::NO_ID) 
        {
          points_match.id = id;
          for ( typename DetectionResult<InterestFeatureT>::iterator it = points_match.begin();
                it != points_match.end();
                ++it)
            (*it)->parent_id = id;
        }
        if(m_descriptorFactory)
        {
          m_descriptorFactory->createDescriptor(points_match, image_next);
        }
        MatchSourceInfo<InterestFeatureT>* infomatch = m_matcher->initMatchSourceInfo(points_match);
        m_sourceInfoRef->reinit();
        MatchingResult<InterestFeatureT> result = m_matcher->computeMatch(m_sourceInfoRef, infomatch);
        JFR_DEBUG("Found " << result.matches().size() << " matches");
        delete m_sourceInfoRef;
        m_sourceInfoRef = infomatch;
        return result;
      }

      MatchingResult<InterestFeatureT> nextImage(fdetect_v2::DetectionResult<InterestFeatureT> &points_next, 
                                           unsigned int id = geom::InterestPoint::NO_ID)
      {
        using namespace fdetect_v2;
        if(id != geom::InterestPoint::NO_ID) 
        {
          points_next.id = id;
          for ( typename DetectionResult<InterestFeatureT>::iterator it = points_next.begin();
                it != points_next.end();
                ++it)
            (*it)->parent_id = id;
        }
        MatchSourceInfo<InterestFeatureT>* infomatch = m_matcher->initMatchSourceInfo(points_next);
        m_sourceInfoRef->reinit();
        MatchingResult<InterestFeatureT> result = m_matcher->computeMatch(m_sourceInfoRef, infomatch);
        JFR_DEBUG("Found " << result.matches().size() << " matches");
        delete m_sourceInfoRef;
        m_sourceInfoRef = infomatch;
        return result;
      }

      MatchSourceInfo<InterestFeatureT>* sourceInfoRef() const 
      { 
        return m_sourceInfoRef; 
      }

    protected:
      Matcher<InterestFeatureT>* matcher() 
      { 
        return m_matcher; 
      }
      fdetect_v2::DescriptorFactory<InterestFeatureT>* descriptorFactory() 
      { 
        return m_descriptorFactory; 
      }
      fdetect_v2::Detector<InterestFeatureT>* detector() 
      { 
        return m_detector; 
      }
    protected:
      fdetect_v2::Detector<InterestFeatureT>* m_detector;
      fdetect_v2::DescriptorFactory<InterestFeatureT>* m_descriptorFactory;
      Matcher<InterestFeatureT>* m_matcher;
      MatchSourceInfo<InterestFeatureT>* m_sourceInfoRef;
    private:
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive &ar, const unsigned int version)
      {
        ar & BOOST_SERIALIZATION_NVP(m_detector);
        ar & BOOST_SERIALIZATION_NVP(m_descriptorFactory);
        ar & BOOST_SERIALIZATION_NVP(m_matcher);
        //ar & BOOST_SERIALIZATION_NVP(m_sourceInfoRef);
      }
    };

    template<typename InterestFeatureT>
    class EngineStereoTracking : public EngineTracking<InterestFeatureT> {
    public:
      EngineStereoTracking(fdetect_v2::Detector<InterestFeatureT>* detector, 
                           Matcher<InterestFeatureT>* matcher, 
                           Matcher<InterestFeatureT>* stereomatcher, 
                           fdetect_v2::DescriptorFactory<InterestFeatureT>* descriptor = 0)
        : EngineTracking<InterestFeatureT>(detector, matcher, descriptor), 
          m_stereoMatcher(stereomatcher)
      { }
      ~EngineStereoTracking()
      {
        delete m_stereoMatcher;
      }
    public:
      MatchingResult<InterestFeatureT> matchCurrentWith(jafar::image::Image const& image_match )
      {
        using namespace fdetect_v2;
        DetectionResult<InterestFeatureT> points_match = this->detector()->detectIn(image_match);
        if(this->descriptorFactory())
        {
          this->descriptorFactory()->createDescriptor(points_match, image_match);
        }
        MatchSourceInfo<InterestFeatureT>* infomatch = this->matcher()->initMatchSourceInfo(points_match);
        this->sourceInfoRef()->reinit();
        MatchingResult<InterestFeatureT> result = this->m_stereoMatcher->computeMatch(this->sourceInfoRef(), infomatch);
        delete infomatch;
        return result;
      }

    private:
      Matcher<InterestFeatureT>* m_stereoMatcher;
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive &ar, const unsigned int version)
      {
        ar & BOOST_SERIALIZATION_NVP(m_stereoMatcher);
      }
    };
  }
}

#endif