#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;} // Trackvoid 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),