Tracker.hh

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef __Tracker_hh__
#define __Tracker_hh__
#include <Base/Common/BIASpragmaStart.hh>

#include <vector>

#include <Base/Math/Vector.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Image/PyramidImageInterface.hh>
#include <Filter/GradientSobel3x3.hh>
#include <Filter/GradientGauss.hh>
#include <Filter/GradientGaussAsymmetric.hh>
#include <Filter/Gauss.hh>
#include <Filter/Mean.hh>
#include <Filter/FilterMask.hh>

#include "TrackerBaseInterface.hh"

#include <deque>

#define D_TRACKER_INIT       0x00000001
#define D_TRACKER_TRACK      0x00000002
#define D_TRACKER_PARAM      0x00000004
#define D_TRACKER_WRITE_IM   0x00000008
#define D_TRACKER_FILL_UP    0x00000010
#define D_TRACKER_IMAGES     0x00000020
#define D_TRACKER_NUMITER    0x00000040
#define D_TRACKER_ERROR      0x00000080

namespace BIAS{

  /** defines type or combination of tracker(s) used: simple/weighted estimate
      a dx,dy, Affine estimate an affine transformation */
  enum ETrackerType {TT_Invalid=-1, TT_Simple, TT_SimpleWeighted, 
                     TT_SimpleMasked, TT_Similar, TT_Affine, TT_Weighted1D,
                     TT_Affine2, TT_Homography};

  /** @class Tracker
      @ingroup g_tracker
      
      @brief High level tracking class. Provides a convenient interface for 
      tracking tasks using pyramid images.

      See ExampleTracker3 for brief exemplary usage.

      @author woelk */
  template <class StorageType, class CalculationType>
  class BIASMatcher2D_EXPORT Tracker : public Debug
  {
  public:
    /** @name Initialization functions */
    //@{

    /// Default constructor without param object
    Tracker(int tracker_type = TT_Simple);
    virtual ~Tracker();
    
    /** Clears the internal data structures, i.e. POints_, POints2_, NumIter_,
        DisplInLastIter_, ResiduiMAD_, ResiduiMSD_, res_ and Cov_.
        @author woelk 09/2004 */
    void ClearPoints();

    /** Calls ClearPoints and sets pointer to last image and gradients to zero.
        @author streckel 11/2004 */
    void Reset();

    //@}

    /** @name Point manipulation functions */
    //@{

    /** Replaces points to which no correspondence could be established in 
        the internal data structures. These "lost" points are marked in the
        internal data structure by setting the last component (w) to
        zero (0.0).
        Traverses the argument list subsequently and makes use of as many 
        points as necessary. Only points further away than *MinBorderDist_ 
        from roi border are added to internal data structures.

        If "ghost tracks" appear, try to make the corner detector aware of 
        points which are already tracked :
        \code
        CornerDetector cd;
        Tracker tr;
        vector<HomgPoint2D> pt
        tr.GetPoints(pt);
        cd.Detect(image, pt, qual);
        tr.FillUpPoints(pt, qual);
        \endcode
        See also documentation of CornerDetectorGradient::Detect().
     
        The argument "qual" refers to the quality from the corner detector.

        @returns number of used points, negative on error.

        @author woelk 09/2004 */ 
    int FillUpPoints(const std::vector<HomgPoint2D>& points,
                     const std::vector<float>& qual);


    /** Place every point from points which is not at infinity, in the internal
        data structure Points_. 
        @author woelk 09/2004 */
    int ReplacePoints(const std::vector<HomgPoint2D>& points,
                      const std::vector<float>& qual);

    /** Place every point from points even those at infinity, in the internal
        data structure Points_. 
        @author woelk 09/2004 */
    int ReplaceAllPoints(const std::vector<HomgPoint2D>& points,
                         const std::vector<float>& qual);

    //@}


    /** @name Tracking and image preparation functions
        These functions need to be called for every new image. */
    //@{

    /** Prepares the pyramid images:
        Filters im to pim[0], downsamples pim[0] and
        calculates the gradients gx and gy of pim in every pyramid level. 
        @author woelk 09/2004 */
    int PreparePyramide(const Image<StorageType>& im, 
                        PyramidImageInterface<CalculationType>& pim,
                        PyramidImageInterface<CalculationType>& gx, 
                        PyramidImageInterface<CalculationType>& gy,
                        FilterNTo2N<CalculationType, CalculationType>& grad,
                        FilterNToN<StorageType, CalculationType>& lowpass);

    /** Prepare pyramid using default gradient and lowpass filter. */
    int PreparePyramide(const Image<StorageType>& im, 
                        PyramidImageInterface<CalculationType>& pim,
                        PyramidImageInterface<CalculationType>& gx, 
                        PyramidImageInterface<CalculationType>& gy);

    /** Prepares the pyramid images. Does neither lowpass filtering,
        nor gradient computation. */
    int PreparePyramide(const Image<StorageType>& im,
                        PyramidImageInterface<CalculationType>& pim);

    /** Tracks all points from the internal data structure Points_.
        @author woelk 09/2004 */
    virtual int Track(PyramidImageInterface<CalculationType>& pim, 
                      PyramidImageInterface<CalculationType>& gradx,
                      PyramidImageInterface<CalculationType>& grady);

    /** tracks all points in Points_ 
        using prediction as previous knowledge
        @author woelk 09/2004 */
    virtual int Track(PyramidImageInterface<CalculationType>& pim, 
                      PyramidImageInterface<CalculationType>& gradx,
                      PyramidImageInterface<CalculationType>& grady,
                      const std::vector<HomgPoint2D>& prediction,
                      const std::vector<Matrix2x2<KLT_TYPE> >&affineprediction=
                      std::vector<Matrix2x2<KLT_TYPE> >());

    /** Tracks all points from the internal data structure Points_.
        @author woelk 09/2004 */
    virtual int Track(PyramidImageInterface<CalculationType>& pim);

    /** tracks all points in Points_ 
        using prediction as previous knowledge
        @author woelk 09/2004 */
    virtual int Track(PyramidImageInterface<CalculationType>& pim,
                      const std::vector<HomgPoint2D>& prediction,
                      const std::vector<Matrix2x2<KLT_TYPE> >&affineprediction=
                      std::vector<Matrix2x2<KLT_TYPE> >());

    //@}

    /** This function allows the replacement of the last images. It is intended
        to be used when all points in the current image should be mapped 
        against different reference image for every point. A set of
        "last images" is constructed from patches 
        @author woelk 02/2006 */
    void ReplaceLastImages(PyramidImageInterface<CalculationType> *pim, 
                           PyramidImageInterface<CalculationType> *gradx,
                           PyramidImageInterface<CalculationType> *grady);
    

    /** @name Result access functions
        Functions for accessing results like point position, number of 
        iterations, ... */
    //@{
    
    /** returns the point positions in the actual image
        points at infinity (w=0.0) are returned, if tracking failed
        @author woelk */
    inline void GetPoints(std::vector<HomgPoint2D>& p) const 
    { p = Points_[PyIndex_]; }

    /** @brief returns the number of iterations used in the biggest 
        pyramid level
        @author woelk 09/2004 */
    inline void GetNumIter(std::vector<int>& numiter) const 
    { numiter = NumIter_[PyIndex_]; }

    /** @brief returns the displacement in the last iteration on the biggest 
        pyramid level.
        @author woelk 09/2004 */
    inline void GetError(std::vector<double>& error) const
    { error = DisplInLastIter_[PyIndex_]; }

    /** @brief returns the mean grey level difference in the tracking window
        on the biggest pyramid level.
        @author woelk 09/2004 */
    inline void GetResiduiMAD(std::vector<double>& residui) const
    { residui = ResiduiMAD_[PyIndex_]; }

    /** @brief returns the squared mean grey level difference in the tracking 
        window on the biggest pyramid level.
        @author woelk 09/2004 */
    inline void GetResiduiMSD(std::vector<double>& residui) const
    { residui = ResiduiMSD_[PyIndex_]; }

    /** @brief returns the results from the base tracker class on the 
        biggest pyramid level.
        0 on success (error< maxerror) - the point was tracked
        -1 when no spatial structure is present
        -2 when point slid out of image
        -3 when maxiter is reached an error is above maxerror
        -4 when a point lies outside of valid region in images
        -5 when the mean absolute difference between the two poinst 
        -6 when input point is at infinity
        1 when error increased from last iteration
        @author woelk 09/2004 */
    inline void GetResults(std::vector<int>& results) const
    { results = res_[PyIndex_]; }

    /** @brief returns estimated affine matrices */
    inline void GetAffineEstimates(std::vector<Matrix2x2<double> >& results) {
      results = AffineResults_[PyIndex_];
    }

    /** @brief returns covariance matrices for biggest pyramid level.
        @author woelk 06/2005 */
    inline void GetCovariances(std::vector<Matrix<double> >& cov) const
    { cov = Cov_[PyIndex_]; }

    //@}

    /** @brief write a track file in Birchfeld-style.

        The file is independent of internal data structures.
        p[i][k] is point number k in image i and 
        results[i][k] is result for point number k in image i.
        
        @author woelk 09/2004 */
    int WriteTrackFile(std::deque<std::vector<HomgPoint2D> >& p, 
                       std::deque<std::vector<int> >& results, 
                       const std::string& name);

    /////////////////////////////////////////////////////////////////////////
    
    /** @name Parameter access
        Functions for accessing the parameter */
    //@{

    /** @brief return trackerbase type currently active */
    int GetTrackerType();

    void SetTrackerType(int tracker_type);

    inline enum ERejectionType GetRejectionType()
    { return (enum ERejectionType)(tb_->GetRejectionType()); }

    inline void SetRejectionType(int rejection_type)
    { tb_->SetRejectionType(rejection_type); }
    
    inline void SetAffineBrightnessInvariance(bool bi) { 
      tb_->SetAffineBrightnessInvariance(bi);
    }

    inline const Vector<int>& GetHalfWinSize()
    { return HalfWinSize_; }

    inline void SetHalfWinSize(const Vector<int>& hws)
    { HalfWinSize_ = hws; }

    inline void SetWeightMatrix(const std::vector< Matrix<KLT_TYPE> >& matrices)
    { WeightMatrices_ = matrices; }
    
    inline void SetMaxError(const Vector<double>& max_error)
    { MaxError_ = max_error; }

    /** !!!
        Also used for X84M as maximal residuum. That is features with a 
        residuum <= 'maxres' will be accept automatically!
        !!! 
    **/
    inline void SetMaxResiduumMAD(const  Vector<KLT_TYPE>& maxres)   
    { MaxResiduumMAD_=maxres; }
    
    inline void SetMaxIterations(const Vector<int>& max_iter)
    { MaxIter_ = max_iter; }

    inline void SetCoarsePointUsage(const bool &UseCoarse) {
      UseCoarserPointIfLost_ = UseCoarse;
    }
    
    inline void SetLowpassMask(const FilterMask& mask) {
      LowpassFM_ = mask;
    }
    inline void SetGradXMask(const FilterMask& mask) {
      GradXFM_ = mask;
    }
    inline void SetGradYMask(const FilterMask& mask) {
      GradYFM_ = mask;
    }

    inline void SetLowpassMask(Vector<double>& vec, bool separable) {
      FilterMask fm;
      Vector2FilterMask_(vec, fm, separable);
      SetLowpassMask(fm);
    }
    inline void SetGradXMask(Vector<double>& vec, bool separable) {
      FilterMask fm;
      Vector2FilterMask_(vec, fm, separable);
      SetGradXMask(fm);
    }
    inline void SetGradYMask(Vector<double>& vec, bool separable) {
      FilterMask fm;
      Vector2FilterMask_(vec, fm, separable);
      SetGradYMask(fm);
    }

     //@}

#ifdef BIAS_DEBUG
    void DumpResults(std::ostream& os=std::cout) {
      const int pysize = (int)LastImage_->size()-1;
      const int maxf = (int)Points_[PyIndex_].size();
      //const int maxf=25;
      const int pyind=PyIndex_;
      for (int i=0; i<maxf; i++){
        os << std::setw(5) << i << " : ";
        for (int p=pysize; p>=pyind; p--){
          os << "  " << Points_[p][i] << std::setw(4) << res_[p][i];
        }
        os << std::endl;
      }
    }
#endif

  protected:
    /// the base tracker
    TrackerBaseInterface<CalculationType> *tb_; 

    /// the gradient filter
    FilterNTo2N<CalculationType, CalculationType> *grad_;
    
    /// the lowpass filter
    FilterNToN<StorageType, CalculationType> *lowpass_;

    /// the parameter 
    Vector<int> MaxIter_;
    Vector<KLT_TYPE> MaxError_;
    Vector<KLT_TYPE> MaxResiduumMAD_;
    Vector<int> HalfWinSize_;
    int PyIndex_;
    /// set if you want to keep track of points regardless of trackability
    /// in highest resolution 
    bool UseCoarserPointIfLost_;

    /// temporary data
    std::vector<HomgPoint2D> p2tracked_;
    /// result code for every point 
    std::vector<std::vector<int> > res_;
    /// the prediction for every pyramid image
    std::vector<std::vector<HomgPoint2D> > PredictedPoints_;
    /// the point position in the actual image
    std::vector<std::vector<HomgPoint2D> > Points_;
    /// number of iterations used
    std::vector<std::vector<int> > NumIter_;
    /// displacement inlast iteration
    std::vector<std::vector<double> > DisplInLastIter_;
    /// mean absolute grey level difference (MAD) in tracking window 
    std::vector<std::vector<double> > ResiduiMAD_;
    /// mean squared grey level difference (MSD) in tracking window 
    std::vector<std::vector<double> > ResiduiMSD_;
    /// covariance matrices
    std::vector<std::vector<Matrix<double> > > Cov_;
    /// results of affine tracking
    std::vector<std::vector<Matrix2x2<double> > > AffineResults_;
    /// prediction for affine warping, same for all pyramid levels
    std::vector<Matrix2x2<double> > AffinePrediction_;
    // the filter masks    
    FilterMask LowpassFM_, GradXFM_, GradYFM_;
    // vector for weight matrices for different pyramid levels
    std::vector< BIAS::Matrix<KLT_TYPE> > WeightMatrices_;

    // the pyramid images from last call to Track
    PyramidImageInterface<CalculationType> *LastImage_;
    PyramidImageInterface<CalculationType> *LastGradX_, *LastGradY_; 

    // adapts the internal memory to new pyramid size if necessary
    void ResizeInternals_(const int pysize);

    /** adapts the internal memory to new pyramid size and new 
        point count if necessary.
        @author woelk 02/2006 */
    void ResizeInternals_(const int pysize, const int num_points);

    // reserves point size space in every pyramid level
    void ReserveInternals_(const int pysize, const int pointsize);

    /// sets all points with index>=start in the internal data structures
    /// to infinity
    void PadPoints_(const int start);

    /** logs reasons for lost tracks into tracker.log */
    void WriteTrackLog_(int num);

    void DetermineROI_(int &tlx, int &tly, int &brx, int &bry) const ;

    void Vector2FilterMask_(Vector<double>& input, FilterMask& mask, bool separable);
  };
}

#include <Base/Common/BIASpragmaEnd.hh>

#endif // __Tracker_hh__