opticalflowpredict.cpp

tracciatore di mani con webcam

#include "Common.h"
#include "Skincolor.h"
#include "OpticalFlow.h"
#include "Exceptions.h"


/** Initialize the Condensation data structure and state dynamics
*/
void OpticalFlow::InitCondensation(int condens_num_samples)
{
  // initialize Condensation data structure and set the
  // system dynamics
  m_sample_confidences.resize(condens_num_samples);
  if (m_pConDens) {
    cvReleaseConDensation(&m_pConDens);
  }
  m_pConDens = cvCreateConDensation(OF_CONDENS_DIMS, OF_CONDENS_DIMS, condens_num_samples);
  CvMat dyn = cvMat(OF_CONDENS_DIMS, OF_CONDENS_DIMS, CV_32FC1, m_pConDens->DynamMatr);
//  CvMat dyn = cvMat(OF_CONDENS_DIMS, OF_CONDENS_DIMS, CV_MAT3x3_32F, m_pConDens->DynamMatr);
  cvmSetIdentity(&dyn);
  cvmSet(&dyn, 0, 1, 0.0);
  cvmSet(&dyn, 2, 3, 0.0);

  // initialize bounds for state
  float lower_bound[OF_CONDENS_DIMS];
  float upper_bound[OF_CONDENS_DIMS];
  // velocity bounds highly depend on the frame rate that we will achieve,
  // increase the factor for lower frame rates;
  // it states how much the center can move in either direction in a single
  // frame, measured in terms of the width or height of the initial match size
  double velocity_factor = .25; 
  double cx = (m_condens_init_rect.left+m_condens_init_rect.right)/2.0;
  double cy = (m_condens_init_rect.top+m_condens_init_rect.bottom)/2.0;
  double width = (m_condens_init_rect.right-m_condens_init_rect.left)*velocity_factor;
  double height = (m_condens_init_rect.bottom-m_condens_init_rect.top)*velocity_factor;
  lower_bound[0] = (float) (cx-width);
  upper_bound[0] = (float) (cx+width);
  lower_bound[1] = (float) (-width);
  upper_bound[1] = (float) (+width);
  lower_bound[2] = (float) (cy-height);
  upper_bound[2] = (float) (cy+height);
  lower_bound[3] = (float) (-height);
  upper_bound[3] = (float) (+height);
  lower_bound[4] = (float) (-10.0*velocity_factor*M_PI/180.0);
  upper_bound[4] = (float) (+10.0*velocity_factor*M_PI/180.0);
  CvMat lb = cvMat(OF_CONDENS_DIMS, 1, CV_MAT3x1_32F, lower_bound);
  CvMat ub = cvMat(OF_CONDENS_DIMS, 1, CV_MAT3x1_32F, upper_bound);
  cvConDensInitSampleSet(m_pConDens, &lb, &ub);

  // set the state that will later be computed by condensation to
  // the currently observed state
  m_condens_state.x = cx;
  m_condens_state.y = cy;
  m_condens_state.vx = 0;
  m_condens_state.vy = 0;
  m_condens_state.angle = 0;

  // debugging:
//  DbgSetModuleLevel(LOG_CUSTOM1, 3);
}




void OpticalFlow::UpdateCondensation(IplImage* /*rgbImage*/,
                                     int prev_indx, int curr_indx)
{
  //VERBOSE5(3, "m_condens_state x %f, y %f, vx %f, vy %f, a %f", 
  //  m_condens_state.x, m_condens_state.y, m_condens_state.vx, m_condens_state.vy, m_condens_state.angle);

  // for each condensation sample, predict the feature locations,
  // compare to the observed KLT tracking, and check the probmask
  // at each predicted location. The combination of these yields the
  // confidence in this sample's estimate.
  int num_ft = (int) m_features[prev_indx].size();
  CPointVector predicted;
  predicted.resize(num_ft);
  CDoubleVector probs_locations;
  CDoubleVector probs_colors;
  probs_locations.reserve(m_pConDens->SamplesNum);
  probs_colors.reserve(m_pConDens->SamplesNum);
  double sum_probs_locations = 0.0;
  double sum_probs_colors = 0.0;
  CDoubleVector old_lens;
  CDoubleVector old_d_angles;
  // prepare data structures so that prediction based on centroid
  // is fast
  PreparePredictFeatureLocations(m_condens_state, m_features[prev_indx], old_lens, old_d_angles);
  CvPoint2D32f avg_obs, avg_prev;
  GetAverage(m_features[curr_indx], avg_prev);
//  GetAverage(m_features[prev_indx], avg_prev);
  GetAverage(m_features[curr_indx]/*_observation*/, avg_obs);
  double dvx = avg_obs.x - avg_prev.x;
  double dvy = avg_obs.y - avg_prev.y;

  // for each sample
  for (int scnt=0; scnt<m_pConDens->SamplesNum; scnt++) {
    // hack - todo
    if (scnt==m_pConDens->SamplesNum-1) {
      m_pConDens->flSamples[scnt][0] = avg_obs.x;
      m_pConDens->flSamples[scnt][2] = avg_obs.y;
      m_pConDens->flSamples[scnt][1] = (float) dvx;
      m_pConDens->flSamples[scnt][3] = (float) dvy;
    }

    // the Condensation sample's guess:
    CondensState sample_state;
    sample_state.x = m_pConDens->flSamples[scnt][0];
    sample_state.y = m_pConDens->flSamples[scnt][2];
    sample_state.vx = m_pConDens->flSamples[scnt][1];
    sample_state.vy = m_pConDens->flSamples[scnt][3];
    sample_state.angle = 0;//m_pConDens->flSamples[scnt][4];
    ASSERT(!isnan(sample_state.x) && !isnan(sample_state.y) && !isnan(sample_state.angle));
    
    double fac = (m_condens_init_rect.right-m_condens_init_rect.left)/3.0;
    double dx = avg_obs.x - sample_state.x;
    double dy = avg_obs.y - sample_state.y;
    double probloc = dx*dx+dy*dy;
    probloc = fac/(fac+probloc);
    probs_locations.push_back(probloc);
    sum_probs_locations += probloc;

#if 0
    PredictFeatureLocations(old_lens, old_d_angles, sample_state, predicted);

    // probability of predicted feature locations given the KLT observation
    int discard_num_distances = (int)(0.15*(double)num_ft);
    double probloc = EstimateProbability(predicted, m_features[curr_indx]/*_observation*/, discard_num_distances);
    probs_locations.push_back(probloc);
    sum_probs_locations += probloc;

    // probability of predicted feature locations given the outside probability map (color)
    double probcol = EstimateProbability(predicted, rgbImage);
    probs_colors.push_back(probcol);
    sum_probs_colors += probcol;
#endif

  } // end for each sample

//  ASSERT(!isnan(sum_probs_locations) && sum_probs_locations>0);

  //
  // normalize the individual probabilities and set sample confidence
  //
  int best_sample_indx = -1;
  double best_confidence = 0;
  for (int scnt=0; scnt<m_pConDens->SamplesNum; scnt++) {
    double norm_prob_locations = probs_locations[scnt]/sum_probs_locations;
//    double norm_prob_colors    = probs_colors[scnt]/sum_probs_colors;
    double confidence;
    if (sum_probs_colors>0) {
 //     confidence = norm_prob_locations*norm_prob_colors;
      confidence = norm_prob_locations;
    } else {
      confidence = norm_prob_locations;
    }
    m_pConDens->flConfidence[scnt] = (float) confidence;
    m_sample_confidences[scnt] = confidence;
    if (confidence>best_confidence) {
      best_confidence = confidence;
      best_sample_indx = scnt;
    }
  }
//  for (int scnt=0; scnt<m_pConDens->SamplesNum; scnt++) {
//    VERBOSE2(3, "%d: %f ", scnt, m_sample_confidences[scnt]);
//  }
  ASSERT(best_sample_indx!=-1);

  ASSERT(best_sample_indx==m_pConDens->SamplesNum-1);
 
  CondensState best_sample_state;
  best_sample_state.x = m_pConDens->flSamples[best_sample_indx][0];
  best_sample_state.y = m_pConDens->flSamples[best_sample_indx][2];
  best_sample_state.vx = m_pConDens->flSamples[best_sample_indx][1];
  best_sample_state.vy = m_pConDens->flSamples[best_sample_indx][3];
  best_sample_state.angle = m_pConDens->flSamples[best_sample_indx][4];
  //VERBOSE3(3, "sample_state %f, %f, %f", 
  // sample_state.x, sample_state.y, sample_state.angle);
  //    VERBOSE4(3, "sample_state %f, %f, %f, %f"), 
  //      sample_state.x, sample_state.y, sample_state.vx, sample_state.vy);
  ASSERT(!isnan(best_sample_state.x) && !isnan(best_sample_state.y) && !isnan(best_sample_state.angle));

  // probability of predicted feature locations given the KLT observation
  m_tmp_predicted.resize(m_features[0].size());
  PredictFeatureLocations(old_lens, old_d_angles, best_sample_state, m_tmp_predicted);

  //
  // do one condensation step, then get the state prediction from Condensation;
  //
  cvConDensUpdateByTime(m_pConDens);

#if 0
  if (false) { // todo
    m_condens_state.x = max(0, min(rgbImage->width-1, m_pConDens->State[0]));
    m_condens_state.y = max(0, min(rgbImage->height-1, m_pConDens->State[2]));
    m_condens_state.vx = m_pConDens->State[1];
    m_condens_state.vy = m_pConDens->State[3];
    m_condens_state.angle = m_pConDens->State[4];
  } else 
#endif