handvu.cpp

tracciatore di mani con webcam

* ----------------------------------------------------------
*/


/* C/C++ interface for AsyncProcessor
 */
void* asyncProcessor(void* arg)
{
  if (arg==NULL) {
    fprintf(stderr, "asyncProcessor: no argument!!\n");
    return NULL;
  }
  HandVu* hv = (HandVu*) arg;
  hv->AsyncProcessor();
}

/* allocate internal ring buffer, start processing thread
 */
void HandVu::AsyncSetup(int num_buffers, DisplayCallback* pDisplayCB)
{
  // sanity checks
  if(!m_initialized) {
    throw HVException("HandVu not initialized, cannot setup async processing");
  }

  if (num_buffers<=0 || 50<num_buffers) {
    throw HVException("num_buffers must be between 1..50");
  }

  if (pDisplayCB==NULL) {
    throw HVException("DisplayCallback can not be NULL");
  }

  // allocate ring buffer
  m_ring_buffer.resize(num_buffers);
  m_ring_buffer_occupied.resize(num_buffers);
  for (int b=0; b<num_buffers; b++) {
    CvSize size = cvSize(m_img_width, m_img_height);
    m_ring_buffer[b] = cvCreateImage(size, IPL_DEPTH_8U, 3);
    m_ring_buffer_occupied[b] = false;
  }

  m_pDisplayCallback = pDisplayCB;
  
  // start processing thread, it will suspended itself if
  // m_process_queue is empty
  m_pAsyncThread = new Thread(asyncProcessor, this);
  m_pAsyncThread->Start();
}


/* insert the frame in the processing queue
 */
void HandVu::AsyncProcessFrame(int id, RefTime& t)
{
  // insert in queue
  GrabbedImage gi(m_ring_buffer[id], t, id);
  m_process_queue.push_back(gi);
  
  // wake up processing thread
  m_pAsyncThread->Resume();
}


/* give the capture application a handle to an unused image in
   the ring buffer
*/
void HandVu::AsyncGetImageBuffer(IplImage** pImg, int* pID)
{
  // this is the only place where we occupy buffers, so we
  // don't have to use a mutex (of course, we might fail
  // for no good reason)
  for (int b=0; b<(int)m_ring_buffer.size(); b++) {
    if (!m_ring_buffer_occupied[b]) {
      *pImg = m_ring_buffer[b];
      *pID  = b;
      return;
    }
  }
  throw HVException("exceeded ring buffer size");
}


void HandVu::AsyncProcessor()
{
  for (;;) {
    // suspend this thread if processing queue is empty
    m_pAsyncThread->Lock();
    if (m_process_queue.empty()) {
      m_pAsyncThread->Suspend();
    }
    m_pAsyncThread->Unlock();
    
    while (!m_quit_thread && !m_process_queue.empty()) {
      // get front of queue - don't lock
      GrabbedImage gi = m_process_queue.front();
      m_process_queue.pop_front();
      
      ASSERT(m_ring_buffer_occupied[gi.GetBufferID()]);
      HVAction action = ProcessFrame(gi);
      m_pDisplayCallback->Display(gi.GetImage(), action);
      
      // free up the buffer - we don't have to lock
      m_ring_buffer_occupied[gi.GetBufferID()] = false;
    }
    
    if (m_quit_thread) {
      m_pAsyncThread->Stop();
    }
  }
}


HandVu::HVAction HandVu::CheckLatency()
{
  ASSERT(m_pClock);

  m_t_start_processing = m_pClock->GetCurrentTimeUsec();

  RefTime incoming_latency = 
    max((RefTime)0, m_t_start_processing-m_sample_time);
  if (m_determine_normal_latency) {
    const int drop_num_frames = 25;
    const int skip_first_num_frames = 15;

    m_last_latencies.push_back(incoming_latency);
    int num_frames_dropped = (int) m_last_latencies.size();
    if (num_frames_dropped<drop_num_frames) {
      // drop frame
      VERBOSE1(3, "HandVu: dropping frame during latency computation (latency %ldus)", 
               incoming_latency);
      return HV_DROP_FRAME;
    }
    
    // calculate average and max latencies
    RefTime sum_latencies = 0;
    RefTime max_latency = LONG_MIN;
    for (int frm=skip_first_num_frames; frm<num_frames_dropped; frm++) {
      RefTime lat = m_last_latencies[frm];
      sum_latencies += lat;
      if (lat>max_latency) {
        max_latency = lat;
      }
    }
    // average of (average and max), plus 5ms
    int counted = num_frames_dropped-skip_first_num_frames;
    m_max_normal_latency = min((max_latency + sum_latencies/counted)/2l
                                + 2000, // micro-second units -> 2ms
                               max_latency + 1000); // -> 1ms

    m_max_abnormal_latency = max(m_max_normal_latency*2, 
				      (RefTime)15000);

    VERBOSE5(3, "HandVu: avg latency in %d frames: %dus, max: %dus -> norm: %dus, abnorm: %dus", 
             counted, (int)(sum_latencies/counted), (int)(max_latency), 
             (int)(m_max_normal_latency), (int)(m_max_abnormal_latency));

    // stop latency computation
    m_determine_normal_latency = false;
    m_last_latencies.resize(1);
  }

  ASSERT(m_last_latencies.size()>0);
  m_last_latencies[0] = incoming_latency;
  if (incoming_latency>m_max_normal_latency) {
    if (incoming_latency>m_max_abnormal_latency) {
      const int max_succ_dropped_frames = 75; 
      // todo: should base this on seconds, not frames dropped.
      // like no frame for 5 sec: recompute
      m_num_succ_dropped_frames++;
      VERBOSE1(3, "HandVu: dropping frame (latency %ldms)", incoming_latency/1000);
      if (m_num_succ_dropped_frames>max_succ_dropped_frames) {
        m_determine_normal_latency = true;
        VERBOSE1(2, "HandVu: warning - %d contiguous frames dropped", 
                m_num_succ_dropped_frames);
        m_num_succ_dropped_frames = 0;
      }
      return HV_DROP_FRAME;
    }
    VERBOSE1(3, "HandVu: skipping frame (latency %ldms)", incoming_latency/1000);
    return HV_SKIP_FRAME;
  }

  m_num_succ_dropped_frames = 0;

  static bool quickreturn = false;
  //  quickreturn = !quickreturn;
  if (quickreturn) return HV_SKIP_FRAME;
  
  return HV_PROCESS_FRAME;
}


void HandVu::KeepStatistics(HVAction action)
{
  // times of frames: all frames
  RefTime t_curr = m_pClock->GetCurrentTimeUsec();
  m_frame_times.push_back(t_curr);
  if (action==HV_PROCESS_FRAME) {
    // completely processed frames
    m_processed_frame_times.push_back(t_curr);
  }
  // processing time
  RefTime prcs_time = t_curr-m_t_start_processing; // micro-second units
  if (action==HV_PROCESS_FRAME || action==HV_SKIP_FRAME) {
    m_prcs_times.push_back(prcs_time);
  } else {
    m_prcs_times.push_back(-1);
  }

  // keep only times from within the last second
  // in the three _times arrays

  // all frames that we saw
  RefTime ago = t_curr-m_frame_times.front();
  while (ago>1000000) { // 1 second
    m_frame_times.erase(m_frame_times.begin());
    m_prcs_times.erase(m_prcs_times.begin());
    ago = t_curr-m_frame_times.front();
    // we're guaranteed to have one element in there with ago==0,
    // so we won't pop the list empty
  }

  // completed frames
  while (!m_processed_frame_times.empty()) {
    ago = t_curr-m_processed_frame_times.front();
    if (ago<=1000000) { // 1 second
      break;
    }
    m_processed_frame_times.erase(m_processed_frame_times.begin());
  }
}

void HandVu::DrawOverlay()
{
  if (m_overlay_level>=1) {
    CvFont font;
    cvInitFont( &font, CV_FONT_VECTOR0, 0.5f /* hscale */, 
                0.5f /* vscale */, 0.1f /*italic_scale */, 
                1 /* thickness */);

    // frames per second and min/max processing times
    int fps = (int) m_frame_times.size();
    int processed_fps = (int) m_processed_frame_times.size();
    RefTime min_prcs_time=-1, max_prcs_time=-1;
    for (int f=0; f<(int)m_prcs_times.size(); f++) {
      if (m_prcs_times[f]==-1) {
        continue;
      }
      if (m_prcs_times[f]<min_prcs_time || min_prcs_time==-1) {
        min_prcs_time = m_prcs_times[f];
      } 
      if (m_prcs_times[f]>max_prcs_time || max_prcs_time==-1) {
        max_prcs_time = m_prcs_times[f];
      }
    }
    if (min_prcs_time==-1) {
      ASSERT(max_prcs_time==-1);
      min_prcs_time = 0;
      max_prcs_time = 0;
    }

    char str[256];
    sprintf(str, "%d (%d) fps, %d-%dms", fps, processed_fps, 
            (int)(min_prcs_time/1000), (int)(max_prcs_time/1000)); 
    CvSize textsize;
    int underline;
    cvGetTextSize( str, &font, &textsize, &underline );
    CvPoint pos = cvPoint(m_rgbImage->width-textsize.width-5, textsize.height+10);
    cvPutText(m_rgbImage, str, pos, &font, CV_RGB(0, 255, 0));

    VERBOSE4(3, "HandVu: %d (%d) fps, %d-%dms latency", 
             fps, processed_fps, (int)(min_prcs_time/1000), (int)(max_prcs_time/1000));

    if (m_overlay_level>=2 && m_last_latencies.size()>0) {
      RefTime last = m_last_latencies[m_last_latencies.size()-1];
      char str[256];
      sprintf(str, "(in latency: %dms)", (unsigned int)(last/1000)); 
      CvSize textsize;
      int underline;
      cvGetTextSize(str, &font, &textsize, &underline );
      CvPoint pos = cvPoint(m_rgbImage->width/2-textsize.width, textsize.height+10);
      cvPutText(m_rgbImage, str, pos, &font, CV_RGB(0, 255, 0));
    }

    if (m_tracking) {
      CvPoint pos = cvPoint(cvRound(m_center_pos.x), cvRound(m_center_pos.y));
      cvCircle(m_rgbImage, pos, 13, CV_RGB(0, 0, 0), CV_FILLED);
      cvCircle(m_rgbImage, pos, 10, CV_RGB(255, 255, 255), CV_FILLED);
//      cvCircle(m_rgbImage, cvPoint(cvRound(m_center_pos.x), cvRound(m_center_pos.y)), 
  //             5, CV_RGB(255, 0, 0), CV_FILLED);
    }

    if (m_overlay_level>=3) {
      char str[256];
      char* ptr = str;
      if (!m_active) {
        sprintf(ptr, "inactive ");
        ptr += 9;
      }
      if (m_scan_area.left>=m_scan_area.right
          || m_scan_area.top>=m_scan_area.bottom) 
      {
        sprintf(ptr, "zero-scan ");
        ptr += 10;
      }
      if (m_undistort) {
        sprintf(ptr, "u ");
        ptr += 2;
      }
      if (m_adjust_exposure) {
        sprintf(ptr, "e ");
        ptr += 2;
      }
      if (ptr!=str) {
        CvSize textsize;
        int underline;
        cvGetTextSize(str, &font, &textsize, &underline);
        CvPoint pos = cvPoint(10, textsize.height+10);
        cvPutText(m_rgbImage, str, pos, &font, CV_RGB(0, 255, 0));
      }
    }
  }
}

bool HandVu::DoDetection()
{
  // scan cubicles
  // todo RefTime before = m_pClock->GetCurrentTimeUsec();
  m_pCubicle->Process(m_grayImages[m_curr_buf_indx]);
  // todo RefTime after = m_pClock->GetCurrentTimeUsec();
  // todo FILE* fp = fopen("c:\\hv_tmp\\times.txt", "a+");
  // todo RefTime took = after-before;
  // todo fprintf(fp, "%u\n", took);
  // todo fclose(fp);
  if (m_pCubicle->GotMatches()) {
    m_last_match = m_pCubicle->GetBestMatch();

    // got a match, but how and where?
    // if m_dt_min_match_duration>0, we need more than a single match 
    // but instead a succession of matches within a certain radius
    // from each other
    if (m_dt_first_match_time==0) {
      m_dt_first_match = m_last_match;
      m_dt_first_match_time = m_pClock->GetCurrentTimeUsec();
    }
      
    VERBOSE4(4, "HandVu detection: area %d, %d, %d, %d", 
             m_last_match.left, m_last_match.top, 
             m_last_match.right, m_last_match.bottom);

    // was the match close enough to the first match to be considered 
    // within the same area?
    int curr_center_x = (m_last_match.left+m_last_match.right)/2;
    int curr_center_y = (m_last_match.top+m_last_match.bottom)/2;
    int first_center_x =
      (m_dt_first_match.left+m_dt_first_match.right)/2;
    int first_center_y =
      (m_dt_first_match.top+m_dt_first_match.bottom)/2;
    int dx = curr_center_x-first_center_x;
    int dy = curr_center_y-first_center_y;
    if (sqrt((double) (dx*dx+dy*dy))<m_pConductor->m_dt_radius*m_img_width) {

      // was the match duration long enough?
      RefTime curr_time = m_pClock->GetCurrentTimeUsec();
      if ((curr_time-m_dt_first_match_time)/1000
          >= m_pConductor->m_dt_min_match_duration) 
      {
        // color verification
        bool mostly_skin = VerifyColor();
        if (mostly_skin) {
          // that's it!
          // turn off detection
          for (int cc=0; cc<m_pConductor->m_dt_cascades_end; cc++) {
            cuSetScannerActive((CuCascadeID)cc, false);
          }
          m_dt_first_match_time = 0;

          // set scan area for tracking and recognition
          int halfwidth = (m_last_match.right-m_last_match.left)/2;
          int halfheight = (m_last_match.bottom-m_last_match.top)/2;
          SetScanAreaVerified(CRect(m_last_match.left-halfwidth, m_last_match.top-halfheight, 
                                    m_last_match.right+halfwidth, m_last_match.bottom+halfheight));

          return true;
        }
      }
    } else {
      m_dt_first_match_time = 0;
    }
  } else {
    m_dt_first_match_time = 0;
  }
  return false;
}

bool HandVu::DoRecognition()
{
  // set scan areas and
  // activate detection scanners
  for (int cc=m_pConductor->m_rc_cascades_start;
       cc<m_pConductor->m_rc_cascades_end; cc++) {
    cuSetScanArea((CuCascadeID)cc, 
      m_scan_area.left, m_scan_area.top, m_scan_area.right, m_scan_area.bottom);
    double sct = m_pConductor->m_rc_scale_tolerance;
    cuSetScanScales((CuCascadeID)cc, m_last_match.scale/sct,
                                 m_last_match.scale*sct);
    cuSetScannerActive((CuCascadeID)cc, true);
  }

  m_pCubicle->Process(m_grayImages[m_curr_buf_indx]);
  if (m_pCubicle->GotMatches()) {
    m_last_match = m_pCubicle->GetBestMatch();
    VERBOSE4(4, "HandVu detection: area %d, %d, %d, %d", 
             m_last_match.left, m_last_match.top, 
             m_last_match.right, m_last_match.bottom);

    // set scan area for future
    int halfwidth = (m_last_match.right-m_last_match.left)/2;
    int halfheight = (m_last_match.bottom-m_last_match.top)/2;
    SetScanAreaVerified(CRect(m_last_match.left-halfwidth, m_last_match.top-halfheight, 
                              m_last_match.right+halfwidth, m_last_match.bottom+halfheight));

    return true;
  }
  return false;