CV_RGB(255,255,0), 2);    cvCircle( pImage, cvPoint(rose_x, rose_y), 30, CV_RGB(255,0,0), 1 );        // bars for direction stddev    /*      double rd = dir_stddev/2*M_PI;      cvRectangle (pImage, cvPoint(xpos, pImage->height),      cvPoint(xpos+width, pImage->height-(int)height), CV_RGB(0,255,255), 1);      cvRectangle (pImage, cvPoint(xpos, pImage->height),      cvPoint(xpos+width, pImage->height-(int)(height*rd)),      CV_RGB(0,255,255), CV_FILLED);      xpos += width+space;    */    // bars for direction stddev multiplied with velocity    if (m_recent_max_rdv_decay>0) {      // draw peak bar      m_recent_max_rdv_decay--;      if (m_recent_max_rdv_decay==0) {        m_recent_max_rdv = 0;      } else {        cvRectangle (pImage, cvPoint(xpos, pImage->height),                      cvPoint(xpos+width,                             pImage->height-(int)(height*m_recent_max_rdv)),                     CV_RGB(0,0,255), CV_FILLED);      }    }    double rdv = vel_mean*dir_stddev/(2*M_PI*5);    if (rdv>m_recent_max_rdv) {      m_recent_max_rdv = rdv;      m_recent_max_rdv_decay = 10;    }    cvRectangle (pImage, cvPoint(xpos, pImage->height),                 cvPoint(xpos+width, pImage->height-(int)height),                 CV_RGB(0,255,255), 1);    cvRectangle (pImage, cvPoint(xpos, pImage->height),                 cvPoint(xpos+width, pImage->height-(int)(height*rdv)),                 CV_RGB(0,255,255), CV_FILLED);    xpos += width+space;  }#endif}/** find good features to track */void OpticalFlow::FindGoodFeatures(IplImage* grayImage, const CRect& bbox,                                   CPointVector& corners) {  double quality = 0.1;   //only those corners are selected, which minimal eigen value is  //non-less than maximum of minimal eigen values on the image,  //multiplied by quality_level. For example, quality_level = 0.1  //means that selected corners must be at least 1/10 as good as  //the best corner.  int count = (int) corners.size();  int width = min(grayImage->width, bbox.right-bbox.left);  int height = min(grayImage->height, bbox.bottom-bbox.top);  CvRect cvBox = cvRect(bbox.left, bbox.top, width, height);  cvSetImageROI(grayImage, cvBox);  cvSetImageROI(m_tmpEVImage[0], cvBox);  cvSetImageROI(m_tmpEVImage[1], cvBox);  cvGoodFeaturesToTrack(grayImage, m_tmpEVImage[0], m_tmpEVImage[1],    (CvPoint2D32f*) &corners[0], &count, quality, m_min_distance);  ASSERT(count); // we want at least SOME features!  cvResetImageROI(grayImage);  cvResetImageROI(m_tmpEVImage[0]);  cvResetImageROI(m_tmpEVImage[1]);  corners.resize(count);  // their locations are relative to the bbox origin - translate them  for (int cnt=0; cnt<count; cnt++) {    corners[cnt].x += bbox.left;    corners[cnt].y += bbox.top;  }}#define MIN_PROB_FOREGROUND 0.5/* pick features on foreground color */#pragma warning (disable:4786)void OpticalFlow::PickSkinColoredFeatures(IplImage* rgbImage,                                          const CPointVector& corners,                                          CPointVector& features,                                          const CuScanMatch& match,                                          ConstMaskIt mask){  ASSERT(m_pProbDistrProvider);    int num_features = (int) features.size();  int poolsize = (int) corners.size();  CDoubleVector corner_probs(poolsize);  int width = match.right-match.left;  int height = match.bottom-match.top;  double scale_x = (double)(width-1)/((*mask).second.GetWidth()-1);  double scale_y = (double)(height-1)/((*mask).second.GetHeight()-1);  // create a vector with probabilities, combined of observed color and mask  for (int pp=0; pp<poolsize; pp++) {    ColorBGR* color;    GetPixel(rgbImage, (int) corners[pp].x, (int) corners[pp].y, &color);    double color_prob = m_pProbDistrProvider->LookupProb(*color);    int m_x = cvRound((corners[pp].x-match.left)/scale_x);    int m_y = cvRound((corners[pp].y-match.top)/scale_y);    double loc_prob = (*mask).second.GetProb(m_x, m_y);    corner_probs[pp] = color_prob*loc_prob;  }  // for each feature, find the most skin-colored corner and pick it  for (int fcnt=0; fcnt<num_features; fcnt++) {    double max_prob = -DBL_MAX;    int max_corner_indx = -1;    for (int pp=0; pp<poolsize; pp++) {      if (corner_probs[pp]>max_prob) {        max_prob = corner_probs[pp];        max_corner_indx = pp;      }    }    ASSERT(max_prob>=.0);    if (max_prob>=MIN_PROB_FOREGROUND) {      // found a (the best!) corner with skin color      features[fcnt].x = (float)corners[max_corner_indx].x;      features[fcnt].y = (float)corners[max_corner_indx].y;      corner_probs[max_corner_indx] = .0;    } else {      // pick our own random location, somewhere with skin color      // and far away enough from the other features if possible      int x, y;      double prob;      int cnt = 0;      bool too_close;      // try to find a location that was segmented as foreground color      do {        x = match.left+(rand()%width);        y = match.top+(rand()%height);        x = min(x, rgbImage->width-1);        y = min(y, rgbImage->height-1);        x = max(x, 0);        y = max(y, 0);        ColorBGR* color;        GetPixel(rgbImage, x, y, &color);        double color_prob = m_pProbDistrProvider->LookupProb(*color);        int m_x = cvRound((double)(x-match.left)/scale_x);        int m_y = cvRound((double)(y-match.top)/scale_y);        double loc_prob = (*mask).second.GetProb(m_x, m_y);        prob = color_prob*loc_prob;        too_close = TooCloseToOthers(features, fcnt, fcnt);        cnt++;      } while ((prob<MIN_PROB_FOREGROUND || too_close) && cnt<20);      features[fcnt].x = (float)x;      features[fcnt].y = (float)y;        }  }}#pragma warning (default:4786)/** find feature that has the minimum* cumulative distance from all other features;* will discard the discard_num_furthest distances in the computation*/int OpticalFlow::FindCentroid(const CDoubleMatrix& distances,                              int discard_num_furthest){  double min_cum_dist = DBL_MAX;  int min_cum_dist_indx = -1;  int num_elems = (int)distances.size();  ASSERT(num_elems>discard_num_furthest);  vector<double> furthest; // will be sorted highest to smallest  furthest.resize(discard_num_furthest);  for (int icnt1=0; icnt1<num_elems; icnt1++) {    double cum_dist=0;    for (int f=0; f<discard_num_furthest; f++) furthest[f]=0;    for (int icnt2=0; icnt2<num_elems; icnt2++) {      double dist;      if (icnt1<icnt2) {        dist = distances[icnt1][icnt2];      } else if (icnt2<icnt1) {        dist = distances[icnt2][icnt1];      } else {        ASSERT(icnt1==icnt2);        continue;      }      // check if it's a far-away feature, update "furthest" vector if so.      // only add the distance if it's not too far away      for (int f=0; f<discard_num_furthest; f++) {        if (dist>furthest[f]) {          // add smallest "furthest" dist to the sum before we kick it out          // of the vector          cum_dist += furthest[discard_num_furthest-1];          for (int s=f; s<discard_num_furthest-1; s++) {            furthest[s+1] = furthest[s];          }          furthest[f] = dist;          break;        }      }      cum_dist += dist;    }    if (cum_dist<min_cum_dist) {      min_cum_dist = cum_dist;      min_cum_dist_indx = icnt1;    }  }  return min_cum_dist_indx;}/** find average of feature locations*/void OpticalFlow::GetAverage(const CPointVector& features, CvPoint2D32f& avg){  double sum_x = 0;  double sum_y = 0;  int num_elems = (int) features.size();  for (int icnt=0; icnt<num_elems; icnt++) {    sum_x += features[icnt].x;    sum_y += features[icnt].y;  }  avg.x = (float)(sum_x/(double)num_elems);  avg.y = (float)(sum_y/(double)num_elems);}/* fill in half of the matrix with the distance measures;* only the upper half is filled in, i.e. if row<col there's a value*/void OpticalFlow::DistanceMatrix(const CPointVector& features,                                 CDoubleMatrix& distances){  int num_features = (int)features.size();  distances.resize(num_features);  for (int row=0; row<num_features; row++) {    distances[row].resize(num_features);    for (int col=row+1; col<num_features; col++) {      double dx = features[col].x-features[row].x;      double dy = features[col].y-features[row].y;      distances[row][col] = sqrt(dx*dx+dy*dy);    }  }}bool OpticalFlow::TooCloseToOthers(const CPointVector& features, int indx, int len){  for (int fcnt=0; fcnt<len; fcnt++) {    if (indx==fcnt) continue;    double dx = features[fcnt].x-features[indx].x;    double dy = features[fcnt].y-features[indx].y;    double dist = sqrt(dx*dx+dy*dy);    if (dist<m_min_distance) {      return true;    }  }  return false;}/* randomly re-place one of two points that are too close to each other,* move others that are too far from the centroid towards the centroid*/void OpticalFlow::ConcentrateFeatures(IplImage* rgbImage,                                      CPointVector& features,                                      CStatusVector& status,                                      int last_width, int last_height,                                      bool use_prob_distr){  ASSERT(m_pProbDistrProvider);  int round_or_square = 2; // the area in which we will concentrate the features, 1 is round, 2 is rectangular  CDoubleMatrix distances;  DistanceMatrix(features, distances);  // discard the highest 15percent of distances when computing centroid  int discard_num_distances = (int)(0.15*(double)distances.size());  int centroid_index = FindCentroid(distances, discard_num_distances);  CvPoint2D32f centroid = features[centroid_index];  double halfwidth = last_width/2.0;  double halfheight = last_height/2.0;  double left = centroid.x-halfwidth;  double right = centroid.x+halfwidth;  double top = centroid.y-halfheight;  double bottom = centroid.y+halfheight;  int num_features = (int)features.size();  for (int fcnt=0; fcnt<num_features; fcnt++) {    m_feature_status[fcnt] = 1;    for (int scnd=fcnt+1; scnd<num_features; scnd++) {      double dist;      if (fcnt<scnd) {        dist = distances[fcnt][scnd];      } else { // scnd<fcnt        dist = distances[scnd][fcnt];      }      int cnt=0;      bool too_close = dist<m_min_distance;      while (too_close && cnt<20) {        // try to find a location that was segmented as foreground color        // and one that's far away enough from all other features        int x, y;        double prob = 1.0;        int prob_cnt = 0;        do {          x = (int) (centroid.x-last_width/2.0+(rand()%last_width));          y = (int) (centroid.y-last_height/2.0+(rand()%last_height));          x = min(x, rgbImage->width-1);          y = min(y, rgbImage->height-1);          x = max(x, 0);          y = max(y, 0);          ColorBGR* color;          GetPixel(rgbImage, x, y, &color);          if (use_prob_distr) {            prob = m_pProbDistrProvider->LookupProb(*color);          }          prob_cnt++;        } while (prob<MIN_PROB_FOREGROUND && prob_cnt<20);        features[fcnt].x = (float)x;        features[fcnt].y = (float)y;        status[fcnt] = 0;        too_close = TooCloseToOthers(features, fcnt, (int)features.size());        cnt++;      } // end while    } // end for scnd    if (centroid_index!=fcnt) {      if (round_or_square==1) {        // round        double dist = -1;        if (fcnt<centroid_index) {          dist = distances[fcnt][centroid_index];        } else if (centroid_index<fcnt) {          dist = distances[centroid_index][fcnt];        } else {          // will not enter the following loop        }        while (dist>last_width/2.0) {          features[fcnt].x = (float)((features[fcnt].x+centroid.x)/2.0);          features[fcnt].y = (float)((features[fcnt].y+centroid.y)/2.0);          m_feature_status[fcnt] = 0;          double dx = features[fcnt].x-centroid.x;          double dy = features[fcnt].y-centroid.y;          dist = sqrt(dx*dx+dy*dy);;        }      } else {        // rectangular area        // move towards horizontal center        while (features[fcnt].x<left || features[fcnt].x>right) {          features[fcnt].x = (float)((features[fcnt].x+centroid.x)/2.0);          status[fcnt] = 0;        }        // move towards vertical center        while (features[fcnt].y<top || features[fcnt].y>bottom) {          features[fcnt].y = (float)((features[fcnt].y+centroid.y)/2.0);          status[fcnt] = 0;        }      }    } // end centroid_index!=fcnt    features[fcnt].x = min(max(features[fcnt].x, .0f), rgbImage->width-1.0f);    features[fcnt].y = min(max(features[fcnt].y, .0f), rgbImage->height-1.0f);  }}/* randomly re-place one of two points that are too close to each other,* move others that are too far from the centroid towards the centroid*/void OpticalFlow::AddNewFeatures(IplImage* rgbImage,                                 CPointVector& features,                                 CStatusVector& status,                                 int last_width, int last_height,                                 bool use_prob_distr){  ASSERT(m_pProbDistrProvider);  CDoubleMatrix distances;  DistanceMatrix(features, distances);  // discard the highest 15percent of distances when computing centroid  int discard_num_distances = (int)(0.15*(double)distances.size());  int centroid_index = FindCentroid(distances, discard_num_distances);  CvPoint2D32f centroid = features[centroid_index];//  double halfwidth = last_width/2.0;//  double halfheight = last_height/2.0;//  double left = centroid.x-halfwidth;//  double right = centroid.x+halfwidth;//  double top = centroid.y-halfheight;//  double bottom = centroid.y+halfheight;  for (int fcnt=m_num_features_tracked; fcnt<m_target_num_features; fcnt++) {    // try to find a location that was segmented as foreground color    int x, y;    double prob = 1.0;    int prob_cnt = 0;    do {      x = (int) (centroid.x-last_width/2.0+(rand()%last_width));      y = (int) (centroid.y-last_height/2.0+(rand()%last_height));      x = min(x, rgbImage->width-1);      y = min(y, rgbImage->height-1);      x = max(x, 0);      y = max(y, 0);      ColorBGR* color;      GetPixel(rgbImage, x, y, &color);      if (use_prob_distr) {        prob = m_pProbDistrProvider->LookupProb(*color);      }      prob_cnt++;    } while (prob<MIN_PROB_FOREGROUND && prob_cnt<20);    features[fcnt].x = (float)x;    features[fcnt].y = (float)y;    status[fcnt] = 0;  } // end for fcnt}void OpticalFlow::GetPixel(IplImage* rgbImage, int x, int y, ColorBGR** color){  ASSERT(rgbImage->nChannels==3);  *color = (ColorBGR*) cvPtr2D(rgbImage, y, x);}