vpKltOpencv.h

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
 * with opencv.
 *
 * Authors:
 * Fabien Servant
 * Fabien Spindler
 *
 *****************************************************************************/

#ifndef vpKltOpencv_h
#define vpKltOpencv_h

#include <visp3/core/vpConfig.h>
#include <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>

#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408))

#include <opencv2/video/tracking.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

class VISP_EXPORT vpKltOpencv
{
public:
  vpKltOpencv();
  vpKltOpencv(const vpKltOpencv& copy);
  virtual ~vpKltOpencv();

  void addFeature(const float &x, const float &y);
  void addFeature(const long &id, const float &x, const float &y);
  void addFeature(const cv::Point2f &f);

  void display(const vpImage<unsigned char> &I,
               const vpColor &color = vpColor::red, unsigned int thickness=1);
  static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
                      const vpColor &color = vpColor::green, unsigned int thickness=1);
  static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
                      const vpColor &color = vpColor::green, unsigned int thickness=1);
  static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
                      const std::vector<long> &featuresid,
                      const vpColor &color = vpColor::green, unsigned int thickness=1);
  static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
                      const std::vector<long> &featuresid,
                      const vpColor &color = vpColor::green, unsigned int thickness=1);

  int getBlockSize() const {return m_blockSize;}
  void getFeature(const int &index, long &id, float &x, float &y) const;
  std::vector<cv::Point2f> getFeatures() const {return m_points[1];}
  //CvPoint2D32f* getFeatures() const {return features;}
  std::vector<long> getFeaturesId() const {return m_points_id;}
  //long* getFeaturesId() const {return featuresid;}
  double getHarrisFreeParameter() const {return m_harris_k;}
  //bool *getListOfLostFeature() const { return lostDuringTrack; }
  int getMaxFeatures() const {return m_maxCount;}
  double getMinDistance() const {return m_minDistance;}
  int getNbFeatures() const { return (int)m_points[1].size(); }
  int getNbPrevFeatures() const { return (int)m_points[0].size(); }
  //void getPrevFeature(int index, int &id, float &x, float &y) const;
  std::vector<cv::Point2f> getPrevFeatures() const {return m_points[0];}
  //CvPoint2D32f* getPrevFeatures() const {return prev_features;}
  //long* getPrevFeaturesId() const {return prev_featuresid;}
  int getPyramidLevels() const {return m_pyrMaxLevel;}
  double getQuality() const {return m_qualityLevel;}
  int getWindowSize() const {return m_winSize;}

  void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat());
  void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts);
  void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids);

  vpKltOpencv & operator=(const vpKltOpencv& copy);
  void track(const cv::Mat &I);
  void setBlockSize(const int blockSize);
  void setHarrisFreeParameter(double harris_k);
  void setInitialGuess(const std::vector<cv::Point2f> &guess_pts);
  void setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts, const std::vector<long> &fid);
  void setMaxFeatures(const int maxCount);
  void setMinDistance(double minDistance);
  void setMinEigThreshold(double minEigThreshold);
  void setPyramidLevels(const int pyrMaxLevel);
  void setQuality(double qualityLevel);
  void setTrackerId(int tid) {(void)tid;}
  void setUseHarris(const int useHarrisDetector);
  void setWindowSize(const int winSize);
  void suppressFeature(const int &index);

protected:
  cv::Mat m_gray, m_prevGray;
  std::vector<cv::Point2f> m_points[2]; 
  std::vector<long> m_points_id;     
  int m_maxCount;
  cv::TermCriteria m_termcrit;
  int m_winSize;
  double m_qualityLevel;
  double m_minDistance;
  double m_minEigThreshold;
  double m_harris_k;
  int m_blockSize;
  int m_useHarrisDetector;
  int m_pyrMaxLevel;
  long m_next_points_id;
  bool m_initial_guess;

};

#elif defined(VISP_HAVE_OPENCV)
#ifdef _CH_
#pragma package <opencv>
#endif

#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
#  include <opencv2/imgproc/imgproc.hpp>
#  include <opencv2/video/tracking.hpp>
#  include <opencv2/legacy/legacy.hpp>
#  include <opencv2/highgui/highgui.hpp>
#else
#  ifndef _EiC
#    include <cv.h>
#    include <highgui.h>
#    include <stdio.h>
#    include <ctype.h>
#    endif
#endif

#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpTrackingException.h>

typedef int (*funccheck)(int,double,double);
typedef void (*funcinfo)(int,int,int,double,double);
typedef void (*funcevent)(int);

class VISP_EXPORT vpKltOpencv
{
private:
  int initialized; //Is the tracker ready ?

  int maxFeatures; //Maximum number of features to track (Default 50)
  int globalcountFeatures; //Keep over time for ID

  int win_size; //Size of search window for tracker (default 10)
  double quality; //Multiplier for the maxmin eigenvalue; specifies minimal accepted quality of image corners (default 0.01)
  double min_distance; //Limit, specifying minimum possible distance between returned corners; Euclidian distance is used. (default 10)
  double harris_free_parameter; //Harris detector free parameter. (default 0.04)
  int block_size; //Size of the averaging block used by the corner detector (default 3)
  int use_harris; //0 use a simple Minimum EigenValue Detector, != 0  use Harris (default 1)
  int pyramid_level; //Number of level for the tracker's gaussian pyramid data (default 3)
  int _tid; //tracker id for multiple trackers

  IplImage *image; //Image buffer
  IplImage *prev_image; //Image buffer for the previous iteration
  IplImage *pyramid; //Gaussian pyramid data
  IplImage *prev_pyramid; //Gaussian pyramid data for the previous iteration
  IplImage *swap_temp; //Internal

  int countFeatures; //Currently tracked features
  int countPrevFeatures; //Previously tracked features
  
  CvPoint2D32f *features; //List of features
  CvPoint2D32f *prev_features; //List of features for previous iteration
  
  long *featuresid; //Array of ids for features
  long *prev_featuresid; //Array of ids for previous features
  
  int flags; //Flags for tracking (internal)
  
  bool initial_guess; //Bool to precise if the next call to track() uses an initial guess
  
  bool *lostDuringTrack; // Result of the tracker for every feature : 1 = lost, 0 = still present
  char *status; //Result of the tracker for every features : 0 = lost, 1 = found



  //EVENT FUNCTION POINTERS
  funcevent OnInitialize;
  funcinfo OnFeatureLost;
  funcinfo OnNewFeature;
  funcinfo OnMeasureFeature;
  funccheck IsFeatureValid;

private:

  //Internal
  void clean();
  void cleanAll();
  void reset();

public:
  vpKltOpencv();
  vpKltOpencv(const vpKltOpencv& copy);
  virtual ~vpKltOpencv();

  void addFeature(const int &id, const float &x, const float &y);

  //Draw the tracked features on the given image
  void display(const vpImage<unsigned char> &I,
               vpColor color = vpColor::red, unsigned int thickness=1);

  int getBlockSize() const {return block_size;}
  void getFeature(int index, long &id, float &x, float &y) const;
  CvPoint2D32f* getFeatures() const {return features;}
  long* getFeaturesId() const {return featuresid;}
  double getHarrisFreeParameter() const {return harris_free_parameter;}
  bool *getListOfLostFeature() const { return lostDuringTrack; }
  int getMaxFeatures() const {return maxFeatures;}
  double getMinDistance() const {return min_distance;}
  int getNbFeatures() const { return countFeatures; }
  int getNbPrevFeatures() const { return countPrevFeatures; }
  void getPrevFeature(int index, int &id, float &x, float &y) const;
  CvPoint2D32f* getPrevFeatures() const {return prev_features;}
  long* getPrevFeaturesId() const {return prev_featuresid;}
  int getPyramidLevels() const {return pyramid_level;}
  double getQuality() const {return quality;}
  int getWindowSize() const {return win_size;}

  //Detect corners in the image. Initialize the tracker
  void initTracking(const IplImage *I, const IplImage *mask = NULL);
  void initTracking(const IplImage *I, CvPoint2D32f *pts, int size);
  void initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size);
  vpKltOpencv & operator=(const vpKltOpencv& copy);
  //Track !
  void track(const IplImage *I);


  //Seters
  void setBlockSize(const int input) {initialized = 0; block_size=input;}
  void setHarrisFreeParameter(double input) {initialized = 0; harris_free_parameter=input;}
  void setInitialGuess(CvPoint2D32f **guess_pts);
  void setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size);
  void setIsFeatureValid(funccheck input) {IsFeatureValid = input;}

  /* Should be used only before initTracking */
  void setMaxFeatures(const int input);
  void setMinDistance(double input) {initialized = 0; min_distance=input;}

  //Functors

  //Event when tracker is initialized -> event(id_tracker)
  void setOnInitialize(funcevent input) {OnInitialize = input;}
  //Event when a feature is lost -> event(id_tracker, index, uid, x, y)
  void setOnFeatureLost(funcinfo input) {OnFeatureLost = input;}
  //Event when a new feature is found -> event(id_tracker, index, uid, x, y)
  void setOnNewFeature(funcinfo input) {OnNewFeature = input;}
  //Event when a feature is found while tracking -> event(id_tracker, index, uid, x, y)
  void setOnMeasureFeature(funcinfo input) {OnMeasureFeature = input;}
  void setPyramidLevels(const int input) {initialized = 0; pyramid_level=input;}
  void setQuality(double input) {initialized = 0; quality=input;}
  void setTrackerId(int tid) {_tid = tid;}
  void setUseHarris(const int input) {initialized = 0; use_harris=input;}
  void setWindowSize(const int input) {initialized = 0; win_size=input;}

  void suppressFeature(int index);
  
  //Static Functions
public: 
  static void display(const vpImage<unsigned char>& I, const CvPoint2D32f* features_list,
                      const int &nbFeatures, vpColor color = vpColor::green,
                      unsigned int thickness=1);
  static void display(const vpImage<vpRGBa>& I, const CvPoint2D32f* features_list,
                      const int &nbFeatures, vpColor color = vpColor::green,
                      unsigned int thickness=1);

  static void display(const vpImage<unsigned char>& I, const CvPoint2D32f* features_list,
                      const long *featuresid_list, const int &nbFeatures,
                      vpColor color = vpColor::green, unsigned int thickness=1);
  static void display(const vpImage<vpRGBa>& I, const CvPoint2D32f* features_list,
                      const long *featuresid_list, const int &nbFeatures,
                      vpColor color = vpColor::green, unsigned int thickness=1);

};

#endif
#endif