opticalflow.cpp

tracciatore di mani con webcam

           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);
}