hough_tracking.hh

#ifndef MMVL_HOUGH_TRACKING_HH_INCLUDED
#define MMVL_HOUGH_TRACKING_HH_INCLUDED

#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>
#include <set>
#include <algorithm>
#include <boost/multi_array.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <mimas/mm_linalg.h>

#include <mimas/mm_image_fileinput.h>
#include <mimas/mm_image_fileoutput.h>
#include <mimas/mm_image_qtoutput.h>
#include "tools.hh"
#include <mimas/mm_image.h>

#include <mimas/mm_xml_node_reference_list.h>
#include <mimas/mm_xml_document.h>
//why these 2 include are not documented in mimas
#include <util/PlatformUtils.hpp>
#include <XPath/XPathInit.hpp>
#ifndef NDEBUG
#include <mimas/mm_image_qtoutput.h>
#include <qdatetime.h> 
#endif

#include <qthread.h> //for parallelization of the tracking
#include <qthreadstorage.h> 
#include <qmutex.h> 


//interval_type can be continuous or discrete

class Interval{
public:
  enum interval_type {Continuous = 0, Discrete};
  enum interval_type type;

  //for Continous type
  float lower_bound;
  float upper_bound;

  //for Discrete type
  int value;
  
  Interval(float lowerBound, float uperBound)
    :type(Continuous), lower_bound(lowerBound), upper_bound(uperBound) 
  {};

  Interval(int value)
    :type(Discrete),value(value)
  {};
};


class Transformation{
public:
  std::vector<Interval> intervals;
  int votes;

  Transformation():
    votes(0){}

void debug(void)
  {
    std::cerr<<"transformation is:"<<std::endl
  <<"x: "<< (intervals[0].lower_bound + intervals[0].upper_bound)/2 <<std::endl
  <<"y: "<< (intervals[1].lower_bound + intervals[1].upper_bound)/2 <<std::endl
  <<"theta: "<< (intervals[2].lower_bound + intervals[2].upper_bound)/2 <<std::endl
  <<"z: "<< intervals[3].value <<std::endl
  <<"number of hit: "<< votes<<std::endl;
  }
};

struct lttransf : public std::binary_function<Transformation, Transformation, bool> {
    bool operator()(Transformation x, Transformation y) { return  x.votes < y.votes;}
  };
  
typedef boost::numeric::ublas::vector<int> Feature;

struct ltfeature
{
  bool operator()(const Feature &f1, const Feature &f2) const
  {
    if (f1(0) == f2(0))
      {
  return f1(1) < f2(1);
      }
    return f1(0) < f2(0);
  }
};

//one way to generalize might be to pass the number of dimension of the transform space as a template argument and
//use boost::shape to pass as argument of the constructor to specify the discretization of each
//dimension.

class hough_tracking {
public:
 
  //put the frame of the model in the middle of the image.
  //K needs to be calculated to take into account the added translation of point due to roation
  //Features are given in a model centered coordinate system.
  // We assume that all model images are of
  //the same size and the object oriented the same way and centered in the middle of the image. 


  hough_tracking( const std::vector< std::vector<Feature> > &feature_images,
      int model_width, int model_height, int K ,
      int x_min, int x_max, int x_sub,
      int y_min, int y_max, int y_sub,
      float theta_min, float theta_max, int theta_sub,
      int nb_thread);

  //For each images we store the number of feature it contains.
  //This is used as an indication (when compared with the number of
  //hit find) of the success of the tracking
  std::vector<int> number_of_features;
 
/*
  Pseudo-code 

  fill_image_space()
  {
  for each transformation
  for each feature of the image corresponding to the transformation(think of the stack)
    {
      calculate locus of point according to the sub-transformed space;      
      add transformation reference to this locus to the image_space array;
    }
  }
*/  

//this is separated from the constructor as the data
//structure it fills might be latter loaded
  void fill_image_space(void);
  
  //The feature should be expressed in a object centered coordinate system of
  //the previous tracked location.
  Transformation track(std::vector<Feature> &features);

  int get_model_width(void) const { return model_width; }
  int get_model_height(void) const { return model_height; }

  ~hough_tracking()
  {
    /*    mutex_1.unlock();
    mutex_2.unlock();
    mutex_1_b.unlock();    
    mutex_2_b.unlock();
    thread1.terminate();
    thread2.terminate();
    thread1.wait();
    thread2.wait();*/
  }

private:
  int model_width;
  int model_height;
  int x_sub;
  int y_sub;
  int theta_sub;
  int z_max;

  class track_t:public QThread
  {
  public:
    boost::multi_array<std::vector<Transformation *>,2>& image_space;
    std::vector<Feature> &features;
    std::vector<Transformation *>::iterator transf_it;
    QMutex &mutex;
    QMutex &mutex_b;

    track_t(boost::multi_array<std::vector<Transformation *>,2>& image_space, /*std::vector<Transformation *>::iterator transf_it,*/ QMutex &mutex, QMutex &mutex_b)
      :image_space(image_space),features(*(new std::vector<Feature>) ),/*transf_it(transf_it),*/ mutex(mutex), mutex_b(mutex_b)
    {
      this->mutex.lock();
      this->mutex_b.lock();
    }//with memory leak

    
    void set_features(std::vector<Feature> &features_l)
    {
      features = features_l;
    }
    
  
    void run(void)
    {
      while(true)
  {
    mutex_b.lock();
    for( std::vector<Feature>::const_iterator feature_it = features.begin(); feature_it != features.end(); feature_it++)
      {
        Feature temp(2);
        temp(0) = (*feature_it)(0) + image_space.shape()[0]/2;
        temp(1) = image_space.shape()[1]/2 - (*feature_it)(1);
        
        for(transf_it = image_space[temp(0)][temp(1)].begin();
      transf_it != image_space[temp(0)][temp(1)].end();
      transf_it++)
    (*transf_it)->votes++;  
      }
    mutex.unlock();
  }
    }

  };
  
  //each image has its vector of Feature.
  std::vector< std::vector<Feature> > feature_images;
  
  boost::multi_array<Transformation,4> transformation_space;
  typedef boost::multi_array<Transformation,4>::index idx_ts;//for index transform space
  
  //for parallelisation 

  int nb_thread;
  std::vector<boost::multi_array<std::vector<Transformation *>,2> *> image_space_vec; //replace with  smart pointers ?
  std::vector<QMutex *> mutexes;    
  std::vector<QMutex *> mutexes_b;
  std::vector<track_t *> threads;
  
  //  std::vector<Transformation *>::iterator transf_it;
  typedef boost::multi_array<std::vector<Transformation *>,2>::index idx_is;//for index image space

#ifndef NDEBUG

  mutable mimas::mm_image_qtoutput< mimas::mm_rgba< unsigned char > > display;
#endif
  
};

typedef boost::shared_ptr< hough_tracking > hough_tracking_ptr;

#endif

---