opticalflow.cpp

tracciatore di mani con webcam

#include "Common.h"
#include "Skincolor.h"
#include "OpticalFlow.h"
#include "Exceptions.h"
#include <time.h>
#ifdef HAVE_FLOAT_H
#include <float.h>
#endif

#if defined(WIN32) && defined(DEBUG)
//#include <streams.h>
#endif

//
// Constructor
//
OpticalFlow::OpticalFlow()
  : m_num_features_tracked(0),
    m_recent_max_rdv(0),
    m_recent_max_rdv_decay(0),
    m_condens_is_tracking(false),
    m_pConDens(NULL),
    m_num_features_lost(0),
    m_winsize_width(-1),
    m_winsize_height(-1),
    m_min_distance(-1),
    m_pProbDistrProvider(NULL),
    m_num_pyramid_levels(3),
    m_target_num_features(-1),
    m_saved_prev_indx(-1),
    m_saved_curr_indx(-1),
    m_max_feature_error(-1),
    m_prev_buf_meaningful(false),
    m_prepared(false)
{
  m_pyramids[0] = NULL;
  m_pyramids[1] = NULL;
  m_tmpEVImage[0] = NULL;
  m_tmpEVImage[1] = NULL;
  m_mean_feature_pos.x = -1;
  m_mean_feature_pos.y = -1;

  srand((unsigned) time(NULL));
} // (Constructor)


OpticalFlow::~OpticalFlow()
{
  cvReleaseImage(&m_pyramids[0]);
  cvReleaseImage(&m_pyramids[1]);
  cvReleaseImage(&m_tmpEVImage[0]);
  cvReleaseImage(&m_tmpEVImage[1]);
  
  if (m_pConDens) {
    cvReleaseConDensation(&m_pConDens);
    m_pConDens = NULL;
  }
}

void OpticalFlow::Initialize(int width, int height)
{
  CvSize imgsize = cvSize(width, height);
  cvReleaseImage(&m_pyramids[0]);
  cvReleaseImage(&m_pyramids[1]);
  m_pyramids[0] = cvCreateImage(imgsize, IPL_DEPTH_8U, 1);
  m_pyramids[1] = cvCreateImage(imgsize, IPL_DEPTH_8U, 1);
  cvReleaseImage(&m_tmpEVImage[0]);
  cvReleaseImage(&m_tmpEVImage[1]);
  m_tmpEVImage[0] = cvCreateImage(imgsize, IPL_DEPTH_32F, 1);
  m_tmpEVImage[1] = cvCreateImage(imgsize, IPL_DEPTH_32F, 1);
}

#pragma warning (disable:4786)
void OpticalFlow::PrepareTracking(IplImage* rgbImage,
                                  IplImage* currGrayImage, int curr_indx,
                                  ProbDistrProvider* pProbProv,
                                  const CuScanMatch& match,
                                  ConstMaskIt mask,
                                  int target_num_features,
                                  int winsize_width,
                                  int winsize_height,
                                  double min_distance,
                                  double max_feature_error)
{
  m_pProbDistrProvider = pProbProv;
  m_target_num_features = target_num_features;
  m_num_features_tracked = 0;
  m_prev_buf_meaningful = false;
  m_winsize_width = winsize_width;
  m_winsize_height = winsize_height;
  m_min_distance = min_distance;
  m_max_feature_error = max_feature_error;
  
  // first find a big set of features that sits on corners
  int num_corners = m_target_num_features*3;
  CPointVector corners;
  corners.resize(num_corners);
  CRect bbox(match);
  FindGoodFeatures(currGrayImage, bbox, corners);

  // then play with the color probability distribution to pick
  // the ones that are on skin color, or if those aren't enough,
  // pick some additional ones on skin colored pixels
  m_features[0].resize(m_target_num_features);
  m_features[1].resize(m_target_num_features);
  m_feature_status.resize(m_target_num_features);
  m_errors.resize(m_target_num_features);
  PickSkinColoredFeatures(rgbImage, corners, m_features[curr_indx], match, mask);

  // fine-tune feature locations
  cvFindCornerSubPix(currGrayImage,
                     (CvPoint2D32f*) &m_features[curr_indx][0],
                     m_target_num_features, 
                     cvSize(5,5), cvSize(-1,-1),
                     cvTermCriteria( CV_TERMCRIT_ITER, 10, 0.1f ));

  // set status right for these features
  for (int i=0; i<m_target_num_features; i++) {
    m_feature_status[i] = 1;
  }
  GetAverage(m_features[curr_indx], m_mean_feature_pos);

  m_condens_is_tracking = false;
  m_condens_init_rect = CRect(match);

  m_prepared = true;
}
#pragma warning (default:4786)


int OpticalFlow::Track(IplImage* rgbImage,
                       IplImage* prevImage, IplImage* currImage,
                       int prev_indx, int curr_indx, 
                       int last_width, int last_height,
                       bool flock, bool use_prob_distr)
{
  ASSERT(m_prepared);

  m_prev_buf_meaningful = true;

  LKPyramids(prevImage, currImage, prev_indx, curr_indx);

#if 0
  if (todo) {
    /* track a number of KLT features with an n-stage pyramid
    * and globally optimize their positions with Condensation
    */
    const int condens_num_samples = 128;
    if (!m_condens_is_tracking) {
      InitCondensation(condens_num_samples);
      ASSERT(m_pConDens->SamplesNum==condens_num_samples);
      m_condens_is_tracking = true;
    }

    UpdateCondensation(rgbImage, prev_indx, curr_indx);
    ASSERT(m_pConDens->SamplesNum==condens_num_samples);
  }
#endif

  if (flock) {
    ConcentrateFeatures(rgbImage, m_features[curr_indx], m_feature_status, 
                        last_width, last_height, use_prob_distr);

  } else {
    AddNewFeatures(rgbImage, m_features[curr_indx], m_feature_status, 
                   last_width, last_height, use_prob_distr);
  }

  GetAverage(m_features[curr_indx], m_mean_feature_pos);
  m_saved_prev_indx = prev_indx;
  m_saved_curr_indx = curr_indx;

  return m_num_features_tracked;
} // Track


void OpticalFlow::DrawOverlay(IplImage* iplImage, int overlay_level)
{
  DrawFeaturesStatistics(iplImage, overlay_level);
}

void OpticalFlow::GetMeanFeaturePos(CvPoint2D32f& mean)
{
  mean = m_mean_feature_pos;
}


/* track a number of KLT features with an n-stage pyramid
*/
void OpticalFlow::LKPyramids(IplImage* prevImage, IplImage* currImage,
                             int prev_indx, int curr_indx)
{
  CvTermCriteria criteria =
    cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03);
  int flags = 0;
  if (m_num_features_tracked>0) {
    flags |= CV_LKFLOW_PYR_A_READY;
  }

  /* note: m_num_pyramid_levels can only be changed before the
   * playback is started.  To release this restriction, the m_pyramids
   * must be re-initialized, that is pyr0_ready set appropriately.
  */

  // in last frame, and possibly during Get/SetFeatures, how many of
  // them were lost?
  ASSERT((int)m_feature_status.size()>=m_target_num_features);
  if (m_target_num_features>0) {
    cvCalcOpticalFlowPyrLK(prevImage, // frame A
                           currImage,   // frame B
                           m_pyramids[prev_indx], // buffer for pyramid for A
                           m_pyramids[curr_indx], // buffer for pyramid for B
                           // feature points to track in A
                           (CvPoint2D32f*) &m_features[prev_indx][0], 
                           // calculated positions in B
                           (CvPoint2D32f*) &m_features[curr_indx][0],
                           // number of feature points to track
                           m_target_num_features,
                           // search window size per pyramid level
                           cvSize(m_winsize_width, m_winsize_height),
                           // max number of pyramid levels
                           m_num_pyramid_levels,
                           // array pos will be set to 1 if corresponding
                           // feature point was found
                           &m_feature_status[0],
                           // may be NULL, diff btw old
                           // and new area around features
                           &m_errors[0],
                           criteria, // iteration termination criteria
                           flags  // todo: put estimate, see Documentation
      );

    int count = m_num_features_tracked = m_target_num_features;
    for (int cnt1=0, k=0; cnt1<count; cnt1++) {
      if (m_feature_status[cnt1] && m_errors[cnt1]<m_max_feature_error) {
        m_features[prev_indx][k] = m_features[prev_indx][cnt1];
        m_features[curr_indx][k] = m_features[curr_indx][cnt1];
        k++;
      } else {
        m_feature_status[cnt1] = 0;
        m_num_features_tracked --;
      }
    }
  }
  m_num_features_lost = m_target_num_features-m_num_features_tracked;
}







void OpticalFlow::DrawFeaturesStatistics(IplImage* pImage, int overlay_level)
{
#if 0
  // for drawing statistics bars
  double height = (double)pImage->height/2.0;
  int xpos = 0;
  const int width = 20;
  const int space = 5;

  if (overlay_level>=3) {
    // num_features
    double goal = m_target_num_features;
    double rf = (goal-(double)m_num_features_lost)/goal;
    cvRectangle (pImage, cvPoint(xpos, pImage->height),
      cvPoint(xpos+width, pImage->height-(int)height),
      CV_RGB(255,0,0), 1);
    cvRectangle (pImage, cvPoint(xpos, pImage->height),
      cvPoint(xpos+width, pImage->height-(int)(height*rf)),
      CV_RGB(255,0,0), CV_FILLED);
    xpos += width+space;
  }
#endif

  const CvScalar black = CV_RGB(0, 0, 0);
  const CvScalar blue  = CV_RGB(255, 0, 0);
//  const CvScalar green = CV_RGB(0, 255, 0);				
  const CvScalar red   = CV_RGB(0, 0, 255);
  const CvScalar yellow= CV_RGB(0, 255, 255);
  const CvScalar white = CV_RGB(255, 255, 255);

  if (m_condens_is_tracking && overlay_level>=3) {
    // a circle at each sample location, its size indicating its confidence
    int num_samples = m_pConDens->SamplesNum;
    double min_conf = DBL_MAX;
    double max_conf = DBL_MIN;
    for (int scnt=0; scnt<num_samples; scnt++) {
      min_conf = min(min_conf, m_sample_confidences[scnt]);
      max_conf = max(max_conf, m_sample_confidences[scnt]);
    }
    double size_scale = 10.0/(max_conf-min_conf);
    for (int scnt=0; scnt<num_samples; scnt++) {
      int x = cvRound(m_pConDens->flSamples[scnt][0]);
      int y = cvRound(m_pConDens->flSamples[scnt][2]);
      int size = cvRound(1.0+(m_sample_confidences[scnt]-min_conf)*size_scale);
      cvCircle(pImage, cvPoint(x, y), size, yellow, 1); 
//      VERBOSE3(3, "%d: %f -> %d", scnt, m_sample_confidences[scnt], size);
    }
  }

  if (overlay_level>=3) {
    for (int cnt2 = 0; cnt2 < m_num_features_tracked; cnt2 ++) {
      // predicted location - blue
      if ((int)m_tmp_predicted.size()>cnt2) {
        int xp = cvRound(m_tmp_predicted[cnt2].x);
        int yp = cvRound(m_tmp_predicted[cnt2].y);
        cvCircle(pImage, cvPoint(xp, yp), 3, blue, CV_FILLED); 
      }
#if 0
      // old location - white
      if (false && m_prev_buf_meaningful) {
        int xo = cvRound(m_features[m_saved_prev_indx][cnt2].x);
        int yo = cvRound(m_features[m_saved_prev_indx][cnt2].y);
        cvCircle(pImage, cvPoint(xo, yo), 2, white, CV_FILLED); 
      }
#endif
      // observation - red
      if ((int)m_features_observation.size()>cnt2) {
        int xp = cvRound(m_features_observation[cnt2].x);
        int yp = cvRound(m_features_observation[cnt2].y);
        cvCircle(pImage, cvPoint(xp, yp), 2, red, 1); 
      }
    }
  }

  if (overlay_level>=2) {
    for (int cnt2 = 0; cnt2 < m_num_features_tracked; cnt2 ++) {
      // new location - green
      int xn = cvRound(m_features[m_saved_curr_indx][cnt2].x);
      int yn = cvRound(m_features[m_saved_curr_indx][cnt2].y);
      cvCircle(pImage, cvPoint(xn, yn), 5, black, CV_FILLED);
      cvCircle(pImage, cvPoint(xn, yn), 3, white, CV_FILLED);
    }
  }

#if 0
  if (m_prev_buf_meaningful && overlay_level>=3) {
    // also draw rose during following loop
    int rose_x = 100;
    int rose_y = pImage->height-100;
    int len_factor = 2;
    double dx_sum = 0;
    double dy_sum = 0;
    double vel_sqr_sum = 0;
    for (int ft=0; ft<m_num_features_tracked; ft++) {
      // current
      double x1 = m_features[m_saved_curr_indx][ft].x;
      double y1 = m_features[m_saved_curr_indx][ft].y;
      // previous frame
      double x2 = m_features[m_saved_prev_indx][ft].x;
      double y2 = m_features[m_saved_prev_indx][ft].y;
      // velocity
      double dx = x1-x2;
      double dy = y1-y2;
      dx_sum += dx;
      dy_sum += dy;
      double vel = sqrt(dx*dx+dy*dy);
      vel_sqr_sum += vel*vel;
      cvLine(pImage, 
             cvPoint(rose_x, rose_y),
             cvPoint(rose_x+(int)dx*len_factor, rose_y+(int)dy*len_factor),
             CV_RGB(255,255,255), 2);
    }

    double dx_mean = dx_sum/(double)m_num_features_tracked;
    double dy_mean = dy_sum/(double)m_num_features_tracked;
    double vel_mean = sqrt(dx_mean*dx_mean+dy_mean*dy_mean);
    double dir_mean;
    if (vel_mean>0) {
      dir_mean = atan(dy_mean/dx_mean);
      if (dx_mean<0) dir_mean += M_PI;
      if (dir_mean<0) dir_mean += 2.0*M_PI;
      if (dir_mean>=2.0*M_PI) dir_mean -= 2.0*M_PI;
    } else {
      dir_mean = 0;
    }
    ASSERT(0<=dir_mean && dir_mean<2.0*M_PI);
    
    double vel_stddev = 0;
    double dir_stddev = 0;
    for (int ft=0; ft<m_num_features_tracked; ft++) {
      // current
      double x1 = m_features[m_saved_curr_indx][ft].x;
      double y1 = m_features[m_saved_curr_indx][ft].y;
      // previous frame
      double x2 = m_features[m_saved_prev_indx][ft].x;
      double y2 = m_features[m_saved_prev_indx][ft].y;
      // velocity
      double dx = x1-x2;
      double dy = y1-y2;
      dx_sum += dx;
      dy_sum += dy;
      double vel = sqrt(dx*dx+dy*dy);
      vel_stddev += (vel-vel_mean)*(vel-vel_mean);
      double dir = atan(dy/dx);
      if (dx<0) dir += M_PI;
      if (dir<0) dir += 2.0*M_PI;
      if (dir>=2.0*M_PI) dir -= 2.0*M_PI;
      double dirdiff = min(fabs(dir-dir_mean), fabs(dir-2.0*M_PI-dir_mean));
      dirdiff = min(dirdiff, fabs(dir+2.0*M_PI-dir_mean));
      dir_stddev += dirdiff*dirdiff;
    }
    vel_stddev /= (double)m_num_features_tracked;
    vel_stddev = sqrt(vel_stddev);
    dir_stddev /= (double)m_num_features_tracked;
    dir_stddev = sqrt(dir_stddev);
    
    // mean velocity and direction
    cvLine(pImage, 
           cvPoint(rose_x, rose_y), 
           cvPoint(rose_x+(int)dx_mean*len_factor,
                   rose_y+(int)dy_mean*len_factor),
           CV_RGB(255,0,0), 8);
    // +/- stddev velocity
    double vel_stddev_x = cos(dir_mean)*vel_stddev;
    double vel_stddev_y = sin(dir_mean)*vel_stddev;
    cvLine(pImage, 
           cvPoint(rose_x+(int)(dx_mean-vel_stddev_x)*len_factor,
                   rose_y+(int)(dy_mean-vel_stddev_y)*len_factor),
           cvPoint(rose_x+(int)(dx_mean+vel_stddev_x)*len_factor,
                   rose_y+(int)(dy_mean+vel_stddev_y)*len_factor),