vpKltOpencv.cpp

/****************************************************************************
 *
 * $Id: vpKltOpencv.cpp 5189 2015-01-21 16:42:18Z ayol $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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
 *
 *****************************************************************************/

#include <visp/vpConfig.h>

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

#include <string>

#include <visp/vpDisplay.h>
#include <visp/vpKltOpencv.h>
#include <visp/vpTrackingException.h>

vpKltOpencv::vpKltOpencv()
  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
    m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1), m_pyrMaxLevel(3),
    m_next_points_id(0), m_initial_guess(false)
{
  m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT|cv::TermCriteria::EPS, 20, 0.03);

}

vpKltOpencv::vpKltOpencv(const vpKltOpencv& copy)
  : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
    m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1), m_pyrMaxLevel(3),
    m_next_points_id(0), m_initial_guess(false)
{
  *this = copy;
}

vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
{
  m_gray = copy.m_gray;
  m_prevGray = copy.m_prevGray;
  m_points[0] = copy.m_points[0];
  m_points[1] = copy.m_points[1];
  m_points_id = copy.m_points_id;
  m_maxCount = copy.m_maxCount;
  m_termcrit = copy.m_termcrit;
  m_winSize = copy.m_winSize;
  m_qualityLevel = copy.m_qualityLevel;
  m_minDistance = copy.m_minDistance;
  m_minEigThreshold = copy.m_minEigThreshold;
  m_harris_k = copy.m_harris_k;
  m_blockSize = copy.m_blockSize;
  m_useHarrisDetector = copy.m_useHarrisDetector;
  m_pyrMaxLevel = copy.m_pyrMaxLevel;
  m_next_points_id = copy.m_next_points_id;
  m_initial_guess = copy.m_initial_guess;

  return *this;
}

vpKltOpencv::~vpKltOpencv()
{
}

void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask)
{
  m_next_points_id = 0;

  //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
  I.copyTo(m_gray);

  for (size_t i=0; i<2; i++) {
    m_points[i].clear();
  }

  m_points_id.clear();

  cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, 0, m_harris_k);

  if(m_points[1].size() > 0){
    cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1,-1), m_termcrit);

    for (size_t i=0; i < m_points[1].size(); i++)
     m_points_id.push_back(m_next_points_id++);
  }
}

void vpKltOpencv::track(const cv::Mat &I)
{
  if(m_points[1].size() == 0)
    throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");

  std::vector<float> err;
  int flags = 0;

  cv::swap(m_prevGray, m_gray);

  if (m_initial_guess) {
    flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
    m_initial_guess = false;
  }
  else {
    std::swap(m_points[1], m_points[0]);
  }

  //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
  I.copyTo(m_gray);

  if(m_prevGray.empty()){
    m_gray.copyTo(m_prevGray);
  }

  std::vector<uchar> status;

  cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
      m_pyrMaxLevel, m_termcrit, flags, m_minEigThreshold);

  // Remove points that are lost
  for (int i=(int)status.size()-1; i>=0; i--) {
    if (status[(size_t)i] == 0) { // point is lost
      m_points[0].erase(m_points[0].begin()+i);
      m_points[1].erase(m_points[1].begin()+i);
      m_points_id.erase(m_points_id.begin()+i);
    }
  }
}

void vpKltOpencv::getFeature(const int &index, int &id, float &x, float &y) const
{
  if ((size_t)index >= m_points[1].size()){
    throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
  }

  x = m_points[1][(size_t)index].x;
  y = m_points[1][(size_t)index].y;
  id = m_points_id[(size_t)index];
}

void vpKltOpencv::display(const vpImage<unsigned char> &I,
                          const vpColor &color, unsigned int thickness)
{
  vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness);
}

void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
                          const vpColor &color, unsigned int thickness)
{
  vpImagePoint ip;
  for (size_t i = 0 ; i < features.size() ; i++) {
    ip.set_u( vpMath::round(features[i].x ) );
    ip.set_v( vpMath::round(features[i].y ) );
    vpDisplay::displayCross(I, ip, 10+thickness, color, thickness);
  }
}

void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
                          const vpColor &color, unsigned int thickness)
{
  vpImagePoint ip;
  for (size_t i = 0 ; i < features.size() ; i++) {
    ip.set_u( vpMath::round(features[i].x ) );
    ip.set_v( vpMath::round(features[i].y ) );
    vpDisplay::displayCross(I, ip, 10+thickness, color, thickness);
  }
}

void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
                          const std::vector<long> &featuresid,
                          const vpColor &color, unsigned int thickness)
{
  vpImagePoint ip;
  for (size_t i = 0; i < features.size(); i++) {
    ip.set_u( vpMath::round(features[i].x ) );
    ip.set_v( vpMath::round(features[i].y ) );
    vpDisplay::displayCross(I, ip, 10, color, thickness);

    std::ostringstream id;
    id << featuresid[i];
    ip.set_u( vpMath::round( features[i].x + 5 ) );
    vpDisplay::displayText(I, ip, id.str(), color);
  }
}

void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
                          const std::vector<long> &featuresid,
                          const vpColor &color, unsigned int thickness)
{
  vpImagePoint ip;
  for (size_t i = 0 ; i < features.size() ; i++) {
    ip.set_u( vpMath::round(features[i].x ) );
    ip.set_v( vpMath::round(features[i].y ) );
    vpDisplay::displayCross(I, ip, 10, color, thickness);

    std::ostringstream id;
    id << featuresid[i];
    ip.set_u( vpMath::round( features[i].x + 5 ) );
    vpDisplay::displayText(I, ip, id.str(), color);
  }
}

void vpKltOpencv::setMaxFeatures(const int maxCount)
{
  m_maxCount = maxCount;
}

void vpKltOpencv::setWindowSize(const int winSize)
{
  m_winSize = winSize;
}

void vpKltOpencv::setQuality(double qualityLevel)
{
  m_qualityLevel = qualityLevel;
}

void vpKltOpencv::setHarrisFreeParameter(double harris_k)
{
  m_harris_k = harris_k;
}

void vpKltOpencv::setUseHarris(const int useHarrisDetector)
{
  m_useHarrisDetector = useHarrisDetector;
}

void vpKltOpencv::setMinDistance(double minDistance)
{
  m_minDistance = minDistance;
}

void vpKltOpencv::setMinEigThreshold(double minEigThreshold)
{
  m_minEigThreshold = minEigThreshold;
}

void vpKltOpencv::setBlockSize(const int blockSize)
{
  m_blockSize = blockSize;
}

void vpKltOpencv::setPyramidLevels(const int pyrMaxLevel)
{
  m_pyrMaxLevel = pyrMaxLevel;
}

void
vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
{
  if(guess_pts.size() != m_points[1].size()){
    throw(vpException(vpException::badValue,
                      "Cannot set initial guess: size feature vector [%d] and guess vector [%d] doesn't match",
                      m_points[1].size(), guess_pts.size()));
  }

  m_points[1] = guess_pts;
  m_initial_guess = true;
}

void
vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
{
  m_initial_guess = false;
  m_points[1] = pts;
  m_next_points_id = 0;
  m_points_id.clear();
  for(size_t i=0; i < m_points[1].size(); i++) {
    m_points_id.push_back(m_next_points_id ++);
  }

  I.copyTo(m_gray);
}

void
vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
{
  m_initial_guess = false;
  m_points[1] = pts;
  m_points_id.clear();

  if(ids.size() != pts.size()){
    m_next_points_id = 0;
    for(size_t i=0; i < m_points[1].size(); i++)
      m_points_id.push_back(m_next_points_id ++);
  }
  else{
    long max = 0;
    for(size_t i=0; i < m_points[1].size(); i++){
      m_points_id.push_back(ids[i]);
      if(ids[i] > max) max = ids[i];
    }
    m_next_points_id = max + 1;
  }

  I.copyTo(m_gray);
}

void vpKltOpencv::addFeature(const float &x, const float &y)
{
  cv::Point2f f(x, y);
  m_points[1].push_back(f);
  m_points_id.push_back(m_next_points_id++);
}

void vpKltOpencv::addFeature(const long &id, const float &x, const float &y)
{
  cv::Point2f f(x, y);
  m_points[1].push_back(f);
  m_points_id.push_back(id);
  if (id >= m_next_points_id)
    m_next_points_id = id + 1;
}

void vpKltOpencv::addFeature(const cv::Point2f &f)
{
  m_points[1].push_back(f);
  m_points_id.push_back(m_next_points_id++);
}

void vpKltOpencv::suppressFeature(const int &index)
{
  if ((size_t)index >= m_points[1].size()){
    throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
  }

  m_points[1].erase(m_points[1].begin()+index);
  m_points_id.erase(m_points_id.begin()+index);
}


#elif defined(VISP_HAVE_OPENCV)

#include <string>

#include <visp/vpKltOpencv.h>

void vpKltOpencv::clean()
{
  if (image) cvReleaseImage(&image);
  if (prev_image) cvReleaseImage(&prev_image);
  if (pyramid) cvReleaseImage(&pyramid);
  if (prev_pyramid) cvReleaseImage(&prev_pyramid);
  
  image = NULL;
  prev_image = NULL;
  pyramid = NULL;
  prev_pyramid = NULL;
  
  swap_temp = NULL;
  countFeatures = 0;
  countPrevFeatures = 0;
  flags = 0;
  initialized = 0;
  globalcountFeatures = 0;
}

void vpKltOpencv::cleanAll()
{
  clean();
  if (features) cvFree(&features);
  if (prev_features) cvFree(&prev_features);
  if (status) cvFree(&status);
  if (lostDuringTrack) cvFree(&lostDuringTrack);
  if (featuresid) cvFree(&featuresid);
  if (prev_featuresid) cvFree(&prev_featuresid);
  features = NULL;
  prev_features = NULL;
  status = NULL;
  lostDuringTrack = 0;
  featuresid = NULL;
  prev_featuresid = NULL;
}

void vpKltOpencv::reset()
{
  clean();

}

vpKltOpencv::vpKltOpencv()
  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
    min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
    pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
    prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
    features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
    flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
    OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
{
  features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(features[0]));
  prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(prev_features[0]));
  status = (char*)cvAlloc((size_t)maxFeatures);
  lostDuringTrack = (bool*)cvAlloc((size_t)maxFeatures);
  featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
  prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
}

vpKltOpencv::vpKltOpencv(const vpKltOpencv& copy)
  : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
    min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
    pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
    prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
    features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
    flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
    OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
{
  *this = copy;
}

vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
{
  //Shallow copy of primitives
  initialized = copy.initialized;
  maxFeatures = copy.maxFeatures;
  countFeatures = copy.countFeatures;
  countPrevFeatures = copy.countPrevFeatures;
  globalcountFeatures = copy.globalcountFeatures;
  flags = copy.flags;
  win_size = copy.win_size;
  quality = copy.quality;
  min_distance = copy.min_distance;
  harris_free_parameter = copy.harris_free_parameter;
  block_size = copy.block_size;
  use_harris = copy.use_harris;
  pyramid_level = copy.pyramid_level;
  _tid = copy._tid;

  OnInitialize = copy.OnInitialize;
  OnFeatureLost = copy.OnFeatureLost;
  OnNewFeature = copy.OnNewFeature;
  OnMeasureFeature = copy.OnMeasureFeature;
  IsFeatureValid = copy.IsFeatureValid;

  initial_guess = copy.initial_guess;
  lostDuringTrack = copy.lostDuringTrack;

  if (!initialized) {
    status = 0;
    lostDuringTrack = 0;
    countFeatures = 0;
    countPrevFeatures = 0;
    flags = 0;
    initialized = 0;
    globalcountFeatures = 0;
  }

  if (copy.image)
  {
    image =  cvCreateImage(cvGetSize(copy.image), 8, 1);
    //      /*IplImage **/cvCopyImage(copy.image,image);
    cvCopy(copy.image, image, 0);
  }

  if (copy.prev_image)
  {
    prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
    //  /*IplImage **/ cvCopyImage(copy.prev_image,prev_image);
    cvCopy(copy.prev_image, prev_image, 0);
  }

  if (copy.pyramid)
  {
    pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
    // /*IplImage **/cvCopyImage(copy.pyramid,pyramid);
    cvCopy(copy.pyramid, pyramid, 0);
  }

  if (copy.prev_pyramid)
  {
    prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
    //  /*IplImage **/cvCopyImage(copy.prev_pyramid,prev_pyramid);
    cvCopy(copy.prev_pyramid, prev_pyramid, 0);
  }

  //Deep copy of arrays
  if (copy.features) {
    /*CvPoint2D32f **/features =
        (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
    for (int i = 0; i < copy.maxFeatures; i++)
      features[i] = copy.features[i];
  }

  if (copy.prev_features) {
    /*CvPoint2D32f **/prev_features =
        (CvPoint2D32f*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(CvPoint2D32f));
    for (int i = 0; i < copy.maxFeatures; i++)
      prev_features[i] = copy.prev_features[i];
  }

  if (copy.featuresid) {
    /*long **/featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
    for (int i = 0; i < copy.maxFeatures; i++)
      featuresid[i] = copy.featuresid[i];
  }

  if (copy.prev_featuresid) {
    /*long **/prev_featuresid = (long*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(long));
    for (int i = 0; i < copy.maxFeatures; i++)
      prev_featuresid[i] = copy.prev_featuresid[i];
  }

  if (copy.status) {
    /*char **/status = (char*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(char));
    for (int i = 0; i < copy.maxFeatures; i++)
      status[i] = copy.status[i];
  }

  if (copy.lostDuringTrack) {
    /*bool **/lostDuringTrack = (bool*)cvAlloc((unsigned int)copy.maxFeatures*sizeof(bool));
    for (int i = 0; i < copy.maxFeatures; i++)
      lostDuringTrack[i] = copy.lostDuringTrack[i];
  }

  return *this;
}

vpKltOpencv::~vpKltOpencv()
{
  cleanAll();
}

void vpKltOpencv::setMaxFeatures(const int input) {
  initialized = 0; maxFeatures=input;

  if (features) cvFree(&features);
  if (prev_features) cvFree(&prev_features);
  if (status) cvFree(&status);
  if (lostDuringTrack) cvFree(&lostDuringTrack);
  if (featuresid) cvFree(&featuresid);
  if (prev_featuresid) cvFree(&prev_featuresid);


  features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
  prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
  status = (char*)cvAlloc((unsigned int)maxFeatures*sizeof(char));
  lostDuringTrack = (bool*)cvAlloc((unsigned int)maxFeatures*sizeof(bool));
  featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
  prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
}

void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask)
{
  if (!I) {
    throw(vpException(vpTrackingException::initializationError,  "Image Not initialized")) ;
  }

  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1)  {
    throw(vpException(vpTrackingException::initializationError,  "Bad Image format")) ;
  }

  if (mask) {
    if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1)   {
      throw(vpException(vpTrackingException::initializationError,  "Bad Image format")) ;
    }
  }

  //Creation des buffers
  CvSize Sizeim, SizeI;
  SizeI = cvGetSize(I);
  bool b_imOK = true;
  if(image != NULL){
    Sizeim = cvGetSize(image);
    if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
  }
  if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
    reset();
    image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
    prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
    pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
    prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
  }else{
    swap_temp = 0;
    countFeatures = 0;
    countPrevFeatures = 0;
    flags = 0;
    initialized = 0;
    globalcountFeatures = 0;
  }

  initialized = 1;

  //Import
  cvCopy(I, image, 0);

  //Recherche de points d'interets
  countFeatures = maxFeatures;
  countPrevFeatures = 0;
  IplImage* eig = cvCreateImage(cvGetSize(image), 32, 1);
  IplImage* temp = cvCreateImage(cvGetSize(image), 32, 1);
  cvGoodFeaturesToTrack(image, eig, temp, features,
                        &countFeatures, quality, min_distance,
                        mask, block_size, use_harris, harris_free_parameter);
  cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size),
                     cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|
                                                  CV_TERMCRIT_EPS,20,0.03));
  cvReleaseImage(&eig);
  cvReleaseImage(&temp);

  if (OnInitialize)
    OnInitialize(_tid);

  //printf("Number of features at init: %d\n", countFeatures);
  for (int boucle=0; boucle<countFeatures;boucle++)  {
    featuresid[boucle] = globalcountFeatures;
    globalcountFeatures++;
    
    if (OnNewFeature){
      OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x,
                   features[boucle].y);
    }
  }
}

void
vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size)
{
  if (size > maxFeatures)
    throw(vpException(vpTrackingException::initializationError,
                      "Cannot initialize tracker from points"));

  //Creation des buffers
  CvSize Sizeim, SizeI;
  SizeI = cvGetSize(I);
  bool b_imOK = true;
  if(image != NULL){
    Sizeim = cvGetSize(image);
    if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
  }
  if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
    reset();
    image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
    prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
    pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
    prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
  } else {
    flags = 0;
  }
  // Save current features as previous features
  countFeatures = size;
  for (int i=0; i<countFeatures;i++)  {
    features[i] = pts[i];
    featuresid[i] = i;
  }

  globalcountFeatures = size;
  initialized = 1;

  cvCopy(I, image, 0);
}

void
vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size)
{
  if (size > maxFeatures)
    throw(vpException(vpTrackingException::initializationError,
                      "Cannot initialize tracker from points"));

  //Creation des buffers
  CvSize Sizeim, SizeI;
  SizeI = cvGetSize(I);
  bool b_imOK = true;
  if(image != NULL){
    Sizeim = cvGetSize(image);
    if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
  }
  if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
    reset();
    image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
    prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
    pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
    prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
  } else {
    flags = 0;
  }
  // Save current features as previous features
  countFeatures = size;
  long max = 0;
  for (int i=0; i<countFeatures;i++)  {
    features[i] = pts[i];
    featuresid[i] = fid[i];
    if (fid[i] > max)
      max = fid[i];
  }

  globalcountFeatures = max + 1;
  initialized = 1;

  cvCopy(I, image, 0);
}

void vpKltOpencv::track(const IplImage *I)
{
  if (!initialized) {
    vpERROR_TRACE("KLT Not initialized") ;
    throw(vpException(vpTrackingException::initializationError,
                      "KLT Not initialized")) ;
  }

  if (!I) {
    throw(vpException(vpTrackingException::initializationError,
                      "Image Not initialized")) ;
  }

  if (I->depth != IPL_DEPTH_8U || I->nChannels != 1)  {
    throw(vpException(vpTrackingException::initializationError,
                      "Bad Image format")) ;
  }

  

  CV_SWAP(prev_image, image, swap_temp);
  CV_SWAP(prev_pyramid, pyramid, swap_temp);
  
  cvCopy(I, image, 0);
  
  if(!initial_guess){
    // Save current features as previous features
    countPrevFeatures = countFeatures;
    for (int boucle=0; boucle<countFeatures;boucle++)  {
      prev_featuresid[boucle] = featuresid[boucle];
    }
    
    CvPoint2D32f *swap_features = 0;
    CV_SWAP(prev_features, features, swap_features);
  }
  
  if (countFeatures <= 0) return;

  cvCalcOpticalFlowPyrLK( prev_image, image, prev_pyramid, pyramid,
                          prev_features, features, countFeatures,
                          cvSize(win_size, win_size), pyramid_level,
                          status, 0, cvTermCriteria(CV_TERMCRIT_ITER
                                                    |CV_TERMCRIT_EPS,20,0.03),
                          flags );
  
  if(!initial_guess)
    flags |= CV_LKFLOW_PYR_A_READY;
  else{
    flags = CV_LKFLOW_PYR_A_READY;
    initial_guess = false;
  }

  int i,k;
  for (i = k = 0; i < countFeatures ; i++)  {
    if (!status[i])     {
      lostDuringTrack[i] = 1;
      if (OnFeatureLost)
        OnFeatureLost(_tid, i, featuresid[i], features[i].x,
                      features[i].y);
      continue;
    }
    
    if (IsFeatureValid) {
      if (!IsFeatureValid(_tid, features[i].x, features[i].y))   {
        lostDuringTrack[i] = 1;
        if (OnFeatureLost)
          OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
        continue;
      }
    }
    features[k] = features[i];
    featuresid[k] = featuresid[i];

    if (OnMeasureFeature) OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
    
    lostDuringTrack[i] = 0;
    k++;
  }
  countFeatures = k;
}

void vpKltOpencv::display(const vpImage<unsigned char> &I,
                          vpColor color, unsigned int thickness)
{
  if ((features == 0) || (I.bitmap==0) || (!initialized))
  {
    vpERROR_TRACE(" Memory problem ");
    throw(vpException(vpException::memoryAllocationError," Memory problem"));
  }

  vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);
}

void vpKltOpencv::getFeature(int index, int &id, float &x, float &y) const
{
  if (index >= countFeatures)
  {
    vpERROR_TRACE(" Memory problem ");
    throw(vpException(vpException::memoryAllocationError," Memory problem"));
  }

  x = features[index].x;
  y = features[index].y;
  id = featuresid[index];
}


void 
vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
{
  // Save current features as previous features
  countPrevFeatures = countFeatures;
  for (int boucle=0; boucle<countFeatures;boucle++)  {
    prev_featuresid[boucle] = featuresid[boucle];
  }
  
  CvPoint2D32f *swap_features = NULL;
  CV_SWAP(prev_features, *guess_pts, swap_features);
  
  CV_SWAP(features, prev_features, swap_features);
  
  flags |= CV_LKFLOW_INITIAL_GUESSES;
  
  initial_guess = true;
}

void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const
{
  if (index >= countPrevFeatures)
  {
    vpERROR_TRACE(" Memory problem ");
    throw(vpException(vpException::memoryAllocationError," Memory problem"));
  }

  x = prev_features[index].x;
  y = prev_features[index].y;
  id = prev_featuresid[index];
}

void vpKltOpencv::addFeature(const int &id,
                             const float &x, const float &y)
{
  if (maxFeatures == countFeatures)
  {
    vpERROR_TRACE(" Cannot add the feature ");
    return;
  }

  CvPoint2D32f f;
  f.x = x;
  f.y = y;
  features[countFeatures] = f;
  featuresid[countFeatures] = id;
  countFeatures ++;
}

void vpKltOpencv::suppressFeature(int index)
{
  if (index >= countFeatures)
  {
    vpERROR_TRACE(" Memory problem ");
    throw(vpException(vpException::memoryAllocationError," Memory problem"));
  }

  countFeatures --;

  for (int i=index ; i < countFeatures; i ++) {
    features[i] = features[i+1];
    featuresid[i] = featuresid[i+1];
  }
}

void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
                          const int &nbFeatures, vpColor color, unsigned int thickness)
{
  vpImagePoint ip;
  for (int i = 0 ; i < nbFeatures ; i++)
  {
    ip.set_u( vpMath::round(features_list[i].x ) );
    ip.set_v( vpMath::round(features_list[i].y ) );
    vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
  }
}
void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
                          const int &nbFeatures, vpColor color, unsigned int thickness)
{
  vpImagePoint ip;
  for (int i = 0 ; i < nbFeatures ; i++)
  {
    ip.set_u( vpMath::round(features_list[i].x ) );
    ip.set_v( vpMath::round(features_list[i].y ) );
    vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
  }
}

void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
                          const long *featuresid_list, const int &nbFeatures,
                          vpColor color, unsigned int thickness)
{
  vpImagePoint ip;
  for (int i = 0 ; i < nbFeatures ; i++)
  {
    ip.set_u( vpMath::round(features_list[i].x ) );
    ip.set_v( vpMath::round(features_list[i].y ) );
    vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;

    char id[10];
    sprintf(id, "%ld", featuresid_list[i]);
    ip.set_u( vpMath::round( features_list[i].x + 5 ) );
    vpDisplay::displayText(I, ip, id, color);
  }
}

void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
                          const long *featuresid_list, const int &nbFeatures,
                          vpColor color, unsigned int thickness)
{
  vpImagePoint ip;
  for (int i = 0 ; i < nbFeatures ; i++)
  {
    ip.set_u( vpMath::round(features_list[i].x ) );
    ip.set_v( vpMath::round(features_list[i].y ) );
    vpDisplay::displayCross(I, ip, 10, color, thickness) ;

    char id[10];
    sprintf(id, "%ld", featuresid_list[i]);
    ip.set_u( vpMath::round( features_list[i].x + 5 ) );
    vpDisplay::displayText(I, ip, id, color);
  }
}

#endif