engine.hpp

/* $Id$ */

#ifndef HPM_ENGINE_HPP
#define HPM_ENGINE_HPP

#include <string>
#include <sstream>
#include <iomanip>

#include "boost/tuple/tuple.hpp"

#include "kernel/jafarException.hpp"
#include "kernel/timingTools.hpp"
#include "kernel/dataLog.hpp"

#include "image/Image.hpp"

#include "gfm/Engine.hpp"
#include "gfm/EngineTracking.hpp"
#include "hpm/export.hpp"

namespace jafar {
  namespace hpm {

    class Engine : public jafar::kernel::DataLoggable {

    protected:

      // preallocated reduced ref image
      jafar::image::Image imageRefRed;
      // preallocated reduced match image
      jafar::image::Image imageMatchRed;
      
      jafar::gfm::Engine* gfmEngine;

      // duration of last match process
      mutable long elapsedTime;

      mutable unsigned int nbMatchedPoints;

      virtual void writeLogHeader(jafar::kernel::DataLogger& log) const;
      virtual void writeLogData(jafar::kernel::DataLogger& log) const;

    public:

      Engine(unsigned int imageWidth_, unsigned int imageHeight_, 
       unsigned int reduction_ = 1);

      jafar::image::Image const& getImageRef() const {return imageRefRed;}
      jafar::image::Image const& getImageMatch() const {return imageMatchRed;}

      long getElapsedTime() const {return elapsedTime;}

      template<typename TypeMatches>
      void computeMatch(jafar::image::Image const& imageRef_, jafar::image::Image const& imageMatch_,
      vecHarrisPoints& pointsRef, vecHarrisPoints& pointsMatch,
      TypeMatches& matches)
      {

  // start chrono
  kernel::Chrono chrono;
  
  // FIXME optimization on reduction when reduction=1 !
  imageRef_.resize(imageRefRed);
  imageMatch_.resize(imageMatchRed);

        gfm::MatchingResult gfmResult = gfmEngine->computeMatch(imageRefRed, imageMatchRed);
        
        exportHarrisPoints( gfmResult.pointsRef() , pointsRef);
        exportHarrisPoints( gfmResult.pointsMatch() , pointsMatch);
        exportMatches(gfmResult, matches);
            
  elapsedTime = chrono.elapsed();
  nbMatchedPoints = matches.size();
      }

    }; // class Engine

    class StereoTrackingEngine;

    class TrackingEngine : public jafar::kernel::DataLoggable {

    protected:

      bool p_isInit;

      jafar::gfm::EngineTracking* gfmEngineTracking;
      // preallocated reduced ref image
      jafar::image::Image imageRefRed;
      // preallocated reduced match image
      jafar::image::Image imageMatchRed;

      jafar::image::Image* imagePrevRed;
      jafar::image::Image* imageCurRed;

      vecMatches matches;

      mutable unsigned int nbTrackedPoints;

      mutable long elapsedTime;

      void writeLogHeader(jafar::kernel::DataLogger& log) const;
      void writeLogData(jafar::kernel::DataLogger& log) const;

    public:

      TrackingEngine(unsigned int imageWidth_, unsigned int imageHeight_, 
         unsigned int reduction_ = 1);

      jafar::image::Image const& getPreviousImage() const {return *imagePrevRed;}
      jafar::image::Image const& getCurrentImage() const {return *imageCurRed;}

      long getElapsedTime() const { return elapsedTime;}

      bool isInit() const {return p_isInit;} 

      vecMatches const& getMatches() const {return matches;}
      vecMatches const& getTrackingMatches() const {return getMatches();}

      template<class VecPoints>
      void initTracking(jafar::image::Image const& imageInit_, VecPoints& pointsInit)
      {
        JFR_PRECOND(imageInit_.colorSpace() == JfrImage_CS_GRAY,
                    "Engine::initTracking: invalid colorspace: " << imageInit_.colorSpace());
        // start chrono
        kernel::Chrono chrono;
        imagePrevRed = &imageRefRed;
        imageCurRed = &imageMatchRed;
        
        matches.clear();
        
        imageInit_.resize(*imageCurRed);
        gfmEngineTracking->initTracking(imageInit_);
        exportHarrisPoints( gfmEngineTracking->currentPoints(), pointsInit);
        p_isInit = true;
      }

      template<class VecPoints>
      void track(jafar::image::Image const& imageCur_,
                 VecPoints const& pointsPrev, VecPoints& pointsCur,
                 bool doPropagateId = false)
      {
        JFR_PRECOND(isInit(), 
                    "TrackingEngine::track: tracking process is not initialized, call initTracking() method first");
        JFR_PRECOND(imageCur_.colorSpace() == JfrImage_CS_GRAY,
                  "Engine::initTracking: invalid colorspace: " << imageCur_.colorSpace());

        // start chrono
        kernel::Chrono chrono;

        // swap image pointers
        jafar::image::Image* imageSwap = imagePrevRed;
        imagePrevRed = imageCurRed;
        imageCurRed = imageSwap;
        // clean up hs results

        imageCur_.resize(*imageCurRed);

        gfm::MatchingResult gfmResult = gfmEngineTracking->nextImage( *imageCurRed);
        
        exportHarrisPoints( gfmResult.pointsMatch() , pointsCur);
        exportMatches(gfmResult, matches);

        elapsedTime = chrono.elapsed();
  
        if (doPropagateId)
          nbTrackedPoints = propagateId(matches, pointsPrev, pointsCur);

      }

    }; // class TrackingEngine

    class StereoTrackingEngine : public jafar::kernel::DataLoggable {

    protected:

      // preallocated reduced ref image
      jafar::image::Image imageRefRed;
      // preallocated reduced match image
      jafar::image::Image imageMatchRed;

      jafar::image::Image* imagePrevRed;
      jafar::image::Image* imageCurRed;

      // preallocated reduced stereo image
      jafar::image::Image imageStereoRed;

      jafar::gfm::EngineStereoTracking* gfmEngineStereoTracking;

      vecMatches trackingMatches;
      vecMatches stereoMatches;
      
      bool p_isInit;

      mutable unsigned int nbTrackedPoints;

      mutable unsigned int elapsedTime;

//       void stereoMatch(jafar::image::Image const& imageLeft_, jafar::image::Image const& imageRight_,
//           vecStereoHarrisPoints& pointsLeft, vecStereoHarrisPoints& pointsRight);

      void writeLogHeader(jafar::kernel::DataLogger& log) const;
      void writeLogData(jafar::kernel::DataLogger& log) const;
      
    public:

      StereoTrackingEngine(unsigned int imageWidth_, unsigned int imageHeight_, 
         unsigned int reduction_ = 1);

      jafar::image::Image const& getPreviousImage() const {return *imagePrevRed;}
      jafar::image::Image const& getCurrentImage() const {return *imageCurRed;}
      jafar::image::Image const& getStereoImage() const {return imageStereoRed;}

      unsigned int getElapsedTime() const { return elapsedTime;}

      bool isInit() const {return p_isInit;} 

      vecMatches const& getTrackingMatches() const {return trackingMatches;}
      vecMatches const& getStereoMatches() const {return stereoMatches;}

      void initTracking(jafar::image::Image const& imageLeft_, jafar::image::Image const& imageRight_,
      vecStereoHarrisPoints& pointsLeft, vecStereoHarrisPoints& pointsRight);

      void track(jafar::image::Image const& imageLeft_, jafar::image::Image const& imageRight_,
     vecStereoHarrisPoints const& pointsPrev, vecStereoHarrisPoints& pointsCur, 
     vecStereoHarrisPoints& pointsStereo,
     bool doPropagateId = false);

    }; // class StereoTrackingEngine
    

  } // namespace hpm
} // namespace jafar

#endif // HPM_ENGINE_HPP