开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 多媒体 > 视频捕捉/采集 > 查看源码
OpticalFlow.cpp
资源名称：handvu-beta3.tar.gz　[点击查看]
上传用户：lijia5631
上传日期：2008-11-10
资源大小：1214k
文件大小：30k
源码类别：

视频捕捉/采集

开发平台：

MultiPlatform

OpticalFlow.cpp：源码内容
  1. /**
  2.   * HandVu - a library for computer vision-based hand gesture
  3.   * recognition.
  4.   * Copyright (C) 2004 Mathias Kolsch, matz@cs.ucsb.edu
  5.   *
  6.   * This program is free software; you can redistribute it and/or
  7.   * modify it under the terms of the GNU General Public License
  8.   * as published by the Free Software Foundation; either version 2
  9.   * of the License, or (at your option) any later version.
  10.   *
  11.   * This program is distributed in the hope that it will be useful,
  12.   * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13.   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14.   * GNU General Public License for more details.
  15.   *
  16.   * You should have received a copy of the GNU General Public License
  17.   * along with this program; if not, write to the Free Software
  18.   * Foundation, Inc., 59 Temple Place - Suite 330, 
  19.   * Boston, MA  02111-1307, USA.
  20.   *
  21.   * $Id: OpticalFlow.cpp,v 1.20 2005/08/16 00:09:55 matz Exp $
  22. **/
  24. #include "Common.h"
  25. #include "Skincolor.h"
  26. #include "OpticalFlow.h"
  27. #include "Exceptions.h"
  28. #include <time.h>
  29. #ifdef HAVE_FLOAT_H
  30. #include <float.h>
  31. #endif
  33. #if defined(WIN32) && defined(DEBUG)
  34. //#include <streams.h>
  35. #endif
  37. //
  38. // Constructor
  39. //
  40. OpticalFlow::OpticalFlow()
  41.   : m_num_features_tracked(0),
  42.     m_recent_max_rdv(0),
  43.     m_recent_max_rdv_decay(0),
  44.     m_condens_is_tracking(false),
  45.     m_pConDens(NULL),
  46.     m_num_features_lost(0),
  47.     m_winsize_width(-1),
  48.     m_winsize_height(-1),
  49.     m_min_distance(-1),
  50.     m_pProbDistrProvider(NULL),
  51.     m_num_pyramid_levels(3),
  52.     m_target_num_features(-1),
  53.     m_saved_prev_indx(-1),
  54.     m_saved_curr_indx(-1),
  55.     m_max_feature_error(-1),
  56.     m_prev_buf_meaningful(false),
  57.     m_prepared(false)
  58. {
  59.   m_pyramids[0] = NULL;
  60.   m_pyramids[1] = NULL;
  61.   m_tmpEVImage[0] = NULL;
  62.   m_tmpEVImage[1] = NULL;
  63.   m_mean_feature_pos.x = -1;
  64.   m_mean_feature_pos.y = -1;
  66.   srand((unsigned) time(NULL));
  67. } // (Constructor)
  70. OpticalFlow::~OpticalFlow()
  71. {
  72.   cvReleaseImage(&m_pyramids[0]);
  73.   cvReleaseImage(&m_pyramids[1]);
  74.   cvReleaseImage(&m_tmpEVImage[0]);
  75.   cvReleaseImage(&m_tmpEVImage[1]);
  76.   
  77.   if (m_pConDens) {
  78.     cvReleaseConDensation(&m_pConDens);
  79.     m_pConDens = NULL;
  80.   }
  81. }
  83. void OpticalFlow::Initialize(int width, int height)
  84. {
  85.   CvSize imgsize = cvSize(width, height);
  86.   cvReleaseImage(&m_pyramids[0]);
  87.   cvReleaseImage(&m_pyramids[1]);
  88.   m_pyramids[0] = cvCreateImage(imgsize, IPL_DEPTH_8U, 1);
  89.   m_pyramids[1] = cvCreateImage(imgsize, IPL_DEPTH_8U, 1);
  90.   cvReleaseImage(&m_tmpEVImage[0]);
  91.   cvReleaseImage(&m_tmpEVImage[1]);
  92.   m_tmpEVImage[0] = cvCreateImage(imgsize, IPL_DEPTH_32F, 1);
  93.   m_tmpEVImage[1] = cvCreateImage(imgsize, IPL_DEPTH_32F, 1);
  94. }
  96. #pragma warning (disable:4786)
  97. void OpticalFlow::PrepareTracking(IplImage* rgbImage,
  98.                                   IplImage* currGrayImage, int curr_indx,
  99.                                   ProbDistrProvider* pProbProv,
  100.                                   const CuScanMatch& match,
  101.                                   ConstMaskIt mask,
  102.                                   int target_num_features,
  103.                                   int winsize_width,
  104.                                   int winsize_height,
  105.                                   double min_distance,
  106.                                   double max_feature_error)
  107. {
  108.   m_pProbDistrProvider = pProbProv;
  109.   m_target_num_features = target_num_features;
  110.   m_num_features_tracked = 0;
  111.   m_prev_buf_meaningful = false;
  112.   m_winsize_width = winsize_width;
  113.   m_winsize_height = winsize_height;
  114.   m_min_distance = min_distance;
  115.   m_max_feature_error = max_feature_error;
  116.   
  117.   // first find a big set of features that sits on corners
  118.   int num_corners = m_target_num_features*3;
  119.   CPointVector corners;
  120.   corners.resize(num_corners);
  121.   CRect bbox(match);
  122.   FindGoodFeatures(currGrayImage, bbox, corners);
  124.   // then play with the color probability distribution to pick
  125.   // the ones that are on skin color, or if those aren't enough,
  126.   // pick some additional ones on skin colored pixels
  127.   m_features[0].resize(m_target_num_features);
  128.   m_features[1].resize(m_target_num_features);
  129.   m_feature_status.resize(m_target_num_features);
  130.   m_errors.resize(m_target_num_features);
  131.   PickSkinColoredFeatures(rgbImage, corners, m_features[curr_indx], match, mask);
  133.   // fine-tune feature locations
  134.   cvFindCornerSubPix(currGrayImage,
  135.                      (CvPoint2D32f*) &m_features[curr_indx][0],
  136.                      m_target_num_features, 
  137.                      cvSize(5,5), cvSize(-1,-1),
  138.                      cvTermCriteria( CV_TERMCRIT_ITER, 10, 0.1f ));
  140.   // set status right for these features
  141.   for (int i=0; i<m_target_num_features; i++) {
  142.     m_feature_status[i] = 1;
  143.   }
  144.   GetAverage(m_features[curr_indx], m_mean_feature_pos);
  146.   m_condens_is_tracking = false;
  147.   m_condens_init_rect = CRect(match);
  149.   m_prepared = true;
  150. }
  151. #pragma warning (default:4786)
  154. int OpticalFlow::Track(IplImage* rgbImage,
  155.                        IplImage* prevImage, IplImage* currImage,
  156.                        int prev_indx, int curr_indx, 
  157.                        int last_width, int last_height,
  158.                        bool flock, bool use_prob_distr)
  159. {
  160.   ASSERT(m_prepared);
  162.   m_prev_buf_meaningful = true;
  164.   LKPyramids(prevImage, currImage, prev_indx, curr_indx);
  166. #if 0
  167.   if (todo) {
  168.     /* track a number of KLT features with an n-stage pyramid
  169.     * and globally optimize their positions with Condensation
  170.     */
  171.     const int condens_num_samples = 128;
  172.     if (!m_condens_is_tracking) {
  173.       InitCondensation(condens_num_samples);
  174.       ASSERT(m_pConDens->SamplesNum==condens_num_samples);
  175.       m_condens_is_tracking = true;
  176.     }
  178.     UpdateCondensation(rgbImage, prev_indx, curr_indx);
  179.     ASSERT(m_pConDens->SamplesNum==condens_num_samples);
  180.   }
  181. #endif
  183.   if (flock) {
  184.     ConcentrateFeatures(rgbImage, m_features[curr_indx], m_feature_status, 
  185.                         last_width, last_height, use_prob_distr);
  187.   } else {
  188.     AddNewFeatures(rgbImage, m_features[curr_indx], m_feature_status, 
  189.                    last_width, last_height, use_prob_distr);
  190.   }
  192.   GetAverage(m_features[curr_indx], m_mean_feature_pos);
  193.   m_saved_prev_indx = prev_indx;
  194.   m_saved_curr_indx = curr_indx;
  196.   return m_num_features_tracked;
  197. } // Track
  200. void OpticalFlow::DrawOverlay(IplImage* iplImage, int overlay_level)
  201. {
  202.   DrawFeaturesStatistics(iplImage, overlay_level);
  203. }
  205. void OpticalFlow::GetMeanFeaturePos(CvPoint2D32f& mean)
  206. {
  207.   mean = m_mean_feature_pos;
  208. }
  211. /* track a number of KLT features with an n-stage pyramid
  212. */
  213. void OpticalFlow::LKPyramids(IplImage* prevImage, IplImage* currImage,
  214.                              int prev_indx, int curr_indx)
  215. {
  216.   CvTermCriteria criteria =
  217.     cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03);
  218.   int flags = 0;
  219.   if (m_num_features_tracked>0) {
  220.     flags |= CV_LKFLOW_PYR_A_READY;
  221.   }
  223.   /* note: m_num_pyramid_levels can only be changed before the
  224.    * playback is started.  To release this restriction, the m_pyramids
  225.    * must be re-initialized, that is pyr0_ready set appropriately.
  226.   */
  228.   // in last frame, and possibly during Get/SetFeatures, how many of
  229.   // them were lost?
  230.   ASSERT((int)m_feature_status.size()>=m_target_num_features);
  231.   if (m_target_num_features>0) {
  232.     cvCalcOpticalFlowPyrLK(prevImage, // frame A
  233.                            currImage,   // frame B
  234.                            m_pyramids[prev_indx], // buffer for pyramid for A
  235.                            m_pyramids[curr_indx], // buffer for pyramid for B
  236.                            // feature points to track in A
  237.                            (CvPoint2D32f*) &m_features[prev_indx][0], 
  238.                            // calculated positions in B
  239.                            (CvPoint2D32f*) &m_features[curr_indx][0],
  240.                            // number of feature points to track
  241.                            m_target_num_features,
  242.                            // search window size per pyramid level
  243.                            cvSize(m_winsize_width, m_winsize_height),
  244.                            // max number of pyramid levels
  245.                            m_num_pyramid_levels,
  246.                            // array pos will be set to 1 if corresponding
  247.                            // feature point was found
  248.                            &m_feature_status[0],
  249.                            // may be NULL, diff btw old
  250.                            // and new area around features
  251.                            &m_errors[0],
  252.                            criteria, // iteration termination criteria
  253.                            flags  // todo: put estimate, see Documentation
  254.       );
  256.     int count = m_num_features_tracked = m_target_num_features;
  257.     for (int cnt1=0, k=0; cnt1<count; cnt1++) {
  258.       if (m_feature_status[cnt1] && m_errors[cnt1]<m_max_feature_error) {
  259.         m_features[prev_indx][k] = m_features[prev_indx][cnt1];
  260.         m_features[curr_indx][k] = m_features[curr_indx][cnt1];
  261.         k++;
  262.       } else {
  263.         m_feature_status[cnt1] = 0;
  264.         m_num_features_tracked --;
  265.       }
  266.     }
  267.   }
  268.   m_num_features_lost = m_target_num_features-m_num_features_tracked;
  269. }
  277. void OpticalFlow::DrawFeaturesStatistics(IplImage* pImage, int overlay_level)
  278. {
  279. #if 0
  280.   // for drawing statistics bars
  281.   double height = (double)pImage->height/2.0;
  282.   int xpos = 0;
  283.   const int width = 20;
  284.   const int space = 5;
  286.   if (overlay_level>=3) {
  287.     // num_features
  288.     double goal = m_target_num_features;
  289.     double rf = (goal-(double)m_num_features_lost)/goal;
  290.     cvRectangle (pImage, cvPoint(xpos, pImage->height),
  291.       cvPoint(xpos+width, pImage->height-(int)height),
  292.       CV_RGB(255,0,0), 1);
  293.     cvRectangle (pImage, cvPoint(xpos, pImage->height),
  294.       cvPoint(xpos+width, pImage->height-(int)(height*rf)),
  295.       CV_RGB(255,0,0), CV_FILLED);
  296.     xpos += width+space;
  297.   }
  298. #endif
  300.   const CvScalar black = CV_RGB(0, 0, 0);
  301.   const CvScalar blue  = CV_RGB(255, 0, 0);
  302. //  const CvScalar green = CV_RGB(0, 255, 0);
  303.   const CvScalar red   = CV_RGB(0, 0, 255);
  304.   const CvScalar yellow= CV_RGB(0, 255, 255);
  305.   const CvScalar white = CV_RGB(255, 255, 255);
  307.   if (m_condens_is_tracking && overlay_level>=3) {
  308.     // a circle at each sample location, its size indicating its confidence
  309.     int num_samples = m_pConDens->SamplesNum;
  310.     double min_conf = DBL_MAX;
  311.     double max_conf = DBL_MIN;
  312.     for (int scnt=0; scnt<num_samples; scnt++) {
  313.       min_conf = min(min_conf, m_sample_confidences[scnt]);
  314.       max_conf = max(max_conf, m_sample_confidences[scnt]);
  315.     }
  316.     double size_scale = 10.0/(max_conf-min_conf);
  317.     for (int scnt=0; scnt<num_samples; scnt++) {
  318.       int x = cvRound(m_pConDens->flSamples[scnt][0]);
  319.       int y = cvRound(m_pConDens->flSamples[scnt][2]);
  320.       int size = cvRound(1.0+(m_sample_confidences[scnt]-min_conf)*size_scale);
  321.       cvCircle(pImage, cvPoint(x, y), size, yellow, 1); 
  322. //      VERBOSE3(3, "%d: %f -> %d", scnt, m_sample_confidences[scnt], size);
  323.     }
  324.   }
  326.   if (overlay_level>=3) {
  327.     for (int cnt2 = 0; cnt2 < m_num_features_tracked; cnt2 ++) {
  328.       // predicted location - blue
  329.       if ((int)m_tmp_predicted.size()>cnt2) {
  330.         int xp = cvRound(m_tmp_predicted[cnt2].x);
  331.         int yp = cvRound(m_tmp_predicted[cnt2].y);
  332.         cvCircle(pImage, cvPoint(xp, yp), 3, blue, CV_FILLED); 
  333.       }
  334. #if 0
  335.       // old location - white
  336.       if (false && m_prev_buf_meaningful) {
  337.         int xo = cvRound(m_features[m_saved_prev_indx][cnt2].x);
  338.         int yo = cvRound(m_features[m_saved_prev_indx][cnt2].y);
  339.         cvCircle(pImage, cvPoint(xo, yo), 2, white, CV_FILLED); 
  340.       }
  341. #endif
  342.       // observation - red
  343.       if ((int)m_features_observation.size()>cnt2) {
  344.         int xp = cvRound(m_features_observation[cnt2].x);
  345.         int yp = cvRound(m_features_observation[cnt2].y);
  346.         cvCircle(pImage, cvPoint(xp, yp), 2, red, 1); 
  347.       }
  348.     }
  349.   }
  351.   if (overlay_level>=2) {
  352.     for (int cnt2 = 0; cnt2 < m_num_features_tracked; cnt2 ++) {
  353.       // new location - green
  354.       int xn = cvRound(m_features[m_saved_curr_indx][cnt2].x);
  355.       int yn = cvRound(m_features[m_saved_curr_indx][cnt2].y);
  356.       cvCircle(pImage, cvPoint(xn, yn), 5, black, CV_FILLED);
  357.       cvCircle(pImage, cvPoint(xn, yn), 3, white, CV_FILLED);
  358.     }
  359.   }
  361. #if 0
  362.   if (m_prev_buf_meaningful && overlay_level>=3) {
  363.     // also draw rose during following loop
  364.     int rose_x = 100;
  365.     int rose_y = pImage->height-100;
  366.     int len_factor = 2;
  367.     double dx_sum = 0;
  368.     double dy_sum = 0;
  369.     double vel_sqr_sum = 0;
  370.     for (int ft=0; ft<m_num_features_tracked; ft++) {
  371.       // current
  372.       double x1 = m_features[m_saved_curr_indx][ft].x;
  373.       double y1 = m_features[m_saved_curr_indx][ft].y;
  374.       // previous frame
  375.       double x2 = m_features[m_saved_prev_indx][ft].x;
  376.       double y2 = m_features[m_saved_prev_indx][ft].y;
  377.       // velocity
  378.       double dx = x1-x2;
  379.       double dy = y1-y2;
  380.       dx_sum += dx;
  381.       dy_sum += dy;
  382.       double vel = sqrt(dx*dx+dy*dy);
  383.       vel_sqr_sum += vel*vel;
  384.       cvLine(pImage, 
  385.              cvPoint(rose_x, rose_y),
  386.              cvPoint(rose_x+(int)dx*len_factor, rose_y+(int)dy*len_factor),
  387.              CV_RGB(255,255,255), 2);
  388.     }
  390.     double dx_mean = dx_sum/(double)m_num_features_tracked;
  391.     double dy_mean = dy_sum/(double)m_num_features_tracked;
  392.     double vel_mean = sqrt(dx_mean*dx_mean+dy_mean*dy_mean);
  393.     double dir_mean;
  394.     if (vel_mean>0) {
  395.       dir_mean = atan(dy_mean/dx_mean);
  396.       if (dx_mean<0) dir_mean += M_PI;
  397.       if (dir_mean<0) dir_mean += 2.0*M_PI;
  398.       if (dir_mean>=2.0*M_PI) dir_mean -= 2.0*M_PI;
  399.     } else {
  400.       dir_mean = 0;
  401.     }
  402.     ASSERT(0<=dir_mean && dir_mean<2.0*M_PI);
  403.     
  404.     double vel_stddev = 0;
  405.     double dir_stddev = 0;
  406.     for (int ft=0; ft<m_num_features_tracked; ft++) {
  407.       // current
  408.       double x1 = m_features[m_saved_curr_indx][ft].x;
  409.       double y1 = m_features[m_saved_curr_indx][ft].y;
  410.       // previous frame
  411.       double x2 = m_features[m_saved_prev_indx][ft].x;
  412.       double y2 = m_features[m_saved_prev_indx][ft].y;
  413.       // velocity
  414.       double dx = x1-x2;
  415.       double dy = y1-y2;
  416.       dx_sum += dx;
  417.       dy_sum += dy;
  418.       double vel = sqrt(dx*dx+dy*dy);
  419.       vel_stddev += (vel-vel_mean)*(vel-vel_mean);
  420.       double dir = atan(dy/dx);
  421.       if (dx<0) dir += M_PI;
  422.       if (dir<0) dir += 2.0*M_PI;
  423.       if (dir>=2.0*M_PI) dir -= 2.0*M_PI;
  424.       double dirdiff = min(fabs(dir-dir_mean), fabs(dir-2.0*M_PI-dir_mean));
  425.       dirdiff = min(dirdiff, fabs(dir+2.0*M_PI-dir_mean));
  426.       dir_stddev += dirdiff*dirdiff;
  427.     }
  428.     vel_stddev /= (double)m_num_features_tracked;
  429.     vel_stddev = sqrt(vel_stddev);
  430.     dir_stddev /= (double)m_num_features_tracked;
  431.     dir_stddev = sqrt(dir_stddev);
  432.     
  433.     // mean velocity and direction
  434.     cvLine(pImage, 
  435.            cvPoint(rose_x, rose_y), 
  436.            cvPoint(rose_x+(int)dx_mean*len_factor,
  437.                    rose_y+(int)dy_mean*len_factor),
  438.            CV_RGB(255,0,0), 8);
  439.     // +/- stddev velocity
  440.     double vel_stddev_x = cos(dir_mean)*vel_stddev;
  441.     double vel_stddev_y = sin(dir_mean)*vel_stddev;
  442.     cvLine(pImage, 
  443.            cvPoint(rose_x+(int)(dx_mean-vel_stddev_x)*len_factor,
  444.                    rose_y+(int)(dy_mean-vel_stddev_y)*len_factor),
  445.            cvPoint(rose_x+(int)(dx_mean+vel_stddev_x)*len_factor,
  446.                    rose_y+(int)(dy_mean+vel_stddev_y)*len_factor),
  447.            CV_RGB(255,255,0), 2);
  448.     cvCircle( pImage, cvPoint(rose_x, rose_y), 30, CV_RGB(255,0,0), 1 );
  449.     
  450.     // bars for direction stddev
  451.     /*
  452.       double rd = dir_stddev/2*M_PI;
  453.       cvRectangle (pImage, cvPoint(xpos, pImage->height),
  454.       cvPoint(xpos+width, pImage->height-(int)height), CV_RGB(0,255,255), 1);
  455.       cvRectangle (pImage, cvPoint(xpos, pImage->height),
  456.       cvPoint(xpos+width, pImage->height-(int)(height*rd)),
  457.       CV_RGB(0,255,255), CV_FILLED);
  458.       xpos += width+space;
  459.     */
  460.     // bars for direction stddev multiplied with velocity
  461.     if (m_recent_max_rdv_decay>0) {
  462.       // draw peak bar
  463.       m_recent_max_rdv_decay--;
  464.       if (m_recent_max_rdv_decay==0) {
  465.         m_recent_max_rdv = 0;
  466.       } else {
  467.         cvRectangle (pImage, cvPoint(xpos, pImage->height), 
  468.                      cvPoint(xpos+width,
  469.                              pImage->height-(int)(height*m_recent_max_rdv)),
  470.                      CV_RGB(0,0,255), CV_FILLED);
  471.       }
  472.     }
  473.     double rdv = vel_mean*dir_stddev/(2*M_PI*5);
  474.     if (rdv>m_recent_max_rdv) {
  475.       m_recent_max_rdv = rdv;
  476.       m_recent_max_rdv_decay = 10;
  477.     }
  478.     cvRectangle (pImage, cvPoint(xpos, pImage->height),
  479.                  cvPoint(xpos+width, pImage->height-(int)height),
  480.                  CV_RGB(0,255,255), 1);
  481.     cvRectangle (pImage, cvPoint(xpos, pImage->height),
  482.                  cvPoint(xpos+width, pImage->height-(int)(height*rdv)),
  483.                  CV_RGB(0,255,255), CV_FILLED);
  484.     xpos += width+space;
  485.   }
  486. #endif
  487. }
  490. /** find good features to track
  491.  */
  492. void OpticalFlow::FindGoodFeatures(IplImage* grayImage, const CRect& bbox,
  493.                                    CPointVector& corners) {
  494.   double quality = 0.1; 
  495.   //only those corners are selected, which minimal eigen value is
  496.   //non-less than maximum of minimal eigen values on the image,
  497.   //multiplied by quality_level. For example, quality_level = 0.1
  498.   //means that selected corners must be at least 1/10 as good as
  499.   //the best corner.
  501.   int count = (int) corners.size();
  502.   int width = min(grayImage->width, bbox.right-bbox.left);
  503.   int height = min(grayImage->height, bbox.bottom-bbox.top);
  504.   CvRect cvBox = cvRect(bbox.left, bbox.top, width, height);
  505.   cvSetImageROI(grayImage, cvBox);
  506.   cvSetImageROI(m_tmpEVImage[0], cvBox);
  507.   cvSetImageROI(m_tmpEVImage[1], cvBox);
  508.   cvGoodFeaturesToTrack(grayImage, m_tmpEVImage[0], m_tmpEVImage[1],
  509.     (CvPoint2D32f*) &corners[0], &count, quality, m_min_distance);
  510.   ASSERT(count); // we want at least SOME features!
  511.   cvResetImageROI(grayImage);
  512.   cvResetImageROI(m_tmpEVImage[0]);
  513.   cvResetImageROI(m_tmpEVImage[1]);
  515.   corners.resize(count);
  517.   // their locations are relative to the bbox origin - translate them
  518.   for (int cnt=0; cnt<count; cnt++) {
  519.     corners[cnt].x += bbox.left;
  520.     corners[cnt].y += bbox.top;
  521.   }
  522. }
  524. #define MIN_PROB_FOREGROUND 0.5
  526. /* pick features on foreground color
  527.  */
  528. #pragma warning (disable:4786)
  529. void OpticalFlow::PickSkinColoredFeatures(IplImage* rgbImage,
  530.                                           const CPointVector& corners,
  531.                                           CPointVector& features,
  532.                                           const CuScanMatch& match,
  533.                                           ConstMaskIt mask)
  534. {
  535.   ASSERT(m_pProbDistrProvider);
  536.   
  537.   int num_features = (int) features.size();
  538.   int poolsize = (int) corners.size();
  539.   CDoubleVector corner_probs(poolsize);
  540.   int width = match.right-match.left;
  541.   int height = match.bottom-match.top;
  542.   double scale_x = (double)(width-1)/((*mask).second.GetWidth()-1);
  543.   double scale_y = (double)(height-1)/((*mask).second.GetHeight()-1);
  545.   // create a vector with probabilities, combined of observed color and mask
  546.   for (int pp=0; pp<poolsize; pp++) {
  547.     ColorBGR* color;
  548.     GetPixel(rgbImage, (int) corners[pp].x, (int) corners[pp].y, &color);
  549.     double color_prob = m_pProbDistrProvider->LookupProb(*color);
  550.     int m_x = cvRound((corners[pp].x-match.left)/scale_x);
  551.     int m_y = cvRound((corners[pp].y-match.top)/scale_y);
  552.     double loc_prob = (*mask).second.GetProb(m_x, m_y);
  553.     corner_probs[pp] = color_prob*loc_prob;
  554.   }
  556.   // for each feature, find the most skin-colored corner and pick it
  557.   for (int fcnt=0; fcnt<num_features; fcnt++) {
  558.     double max_prob = -DBL_MAX;
  559.     int max_corner_indx = -1;
  560.     for (int pp=0; pp<poolsize; pp++) {
  561.       if (corner_probs[pp]>max_prob) {
  562.         max_prob = corner_probs[pp];
  563.         max_corner_indx = pp;
  564.       }
  565.     }
  566.     ASSERT(max_prob>=.0);
  567.     if (max_prob>=MIN_PROB_FOREGROUND) {
  568.       // found a (the best!) corner with skin color
  569.       features[fcnt].x = (float)corners[max_corner_indx].x;
  570.       features[fcnt].y = (float)corners[max_corner_indx].y;
  571.       corner_probs[max_corner_indx] = .0;
  573.     } else {
  574.       // pick our own random location, somewhere with skin color
  575.       // and far away enough from the other features if possible
  576.       int x, y;
  577.       double prob;
  578.       int cnt = 0;
  579.       bool too_close;
  580.       // try to find a location that was segmented as foreground color
  581.       do {
  582.         x = match.left+(rand()%width);
  583.         y = match.top+(rand()%height);
  584.         x = min(x, rgbImage->width-1);
  585.         y = min(y, rgbImage->height-1);
  586.         x = max(x, 0);
  587.         y = max(y, 0);
  588.         ColorBGR* color;
  589.         GetPixel(rgbImage, x, y, &color);
  590.         double color_prob = m_pProbDistrProvider->LookupProb(*color);
  591.         int m_x = cvRound((double)(x-match.left)/scale_x);
  592.         int m_y = cvRound((double)(y-match.top)/scale_y);
  593.         double loc_prob = (*mask).second.GetProb(m_x, m_y);
  594.         prob = color_prob*loc_prob;
  596.         too_close = TooCloseToOthers(features, fcnt, fcnt);
  598.         cnt++;
  599.       } while ((prob<MIN_PROB_FOREGROUND || too_close) && cnt<20);
  600.       features[fcnt].x = (float)x;
  601.       features[fcnt].y = (float)y;
  602.     
  603.     }
  604.   }
  605. }
  606. #pragma warning (default:4786)
  609. /*
  610. * find feature that has the minimum
  611. * cumulative distance from all other features;
  612. * will discard the discard_num_furthest distances in the computation
  613. */
  614. int OpticalFlow::FindCentroid(const CDoubleMatrix& distances,
  615.                               int discard_num_furthest)
  616. {
  617.   double min_cum_dist = DBL_MAX;
  618.   int min_cum_dist_indx = -1;
  619.   int num_elems = (int)distances.size();
  620.   ASSERT(num_elems>discard_num_furthest);
  621.   vector<double> furthest; // will be sorted highest to smallest
  622.   furthest.resize(discard_num_furthest);
  623.   for (int icnt1=0; icnt1<num_elems; icnt1++) {
  624.     double cum_dist=0;
  625.     for (int f=0; f<discard_num_furthest; f++) furthest[f]=0;
  626.     for (int icnt2=0; icnt2<num_elems; icnt2++) {
  627.       double dist;
  628.       if (icnt1<icnt2) {
  629.         dist = distances[icnt1][icnt2];
  630.       } else if (icnt2<icnt1) {
  631.         dist = distances[icnt2][icnt1];
  632.       } else {
  633.         ASSERT(icnt1==icnt2);
  634.         continue;
  635.       }
  636.       // check if it's a far-away feature, update "furthest" vector if so.
  637.       // only add the distance if it's not too far away
  638.       for (int f=0; f<discard_num_furthest; f++) {
  639.         if (dist>furthest[f]) {
  640.           // add smallest "furthest" dist to the sum before we kick it out
  641.           // of the vector
  642.           cum_dist += furthest[discard_num_furthest-1];
  643.           for (int s=f; s<discard_num_furthest-1; s++) {
  644.             furthest[s+1] = furthest[s];
  645.           }
  646.           furthest[f] = dist;
  647.           break;
  648.         }
  649.       }
  650.       cum_dist += dist;
  651.     }
  652.     if (cum_dist<min_cum_dist) {
  653.       min_cum_dist = cum_dist;
  654.       min_cum_dist_indx = icnt1;
  655.     }
  656.   }
  657.   return min_cum_dist_indx;
  658. }
  660. /*
  661. * find average of feature locations
  662. */
  663. void OpticalFlow::GetAverage(const CPointVector& features, CvPoint2D32f& avg)
  664. {
  665.   double sum_x = 0;
  666.   double sum_y = 0;
  667.   int num_elems = (int) features.size();
  668.   for (int icnt=0; icnt<num_elems; icnt++) {
  669.     sum_x += features[icnt].x;
  670.     sum_y += features[icnt].y;
  671.   }
  672.   avg.x = (float)(sum_x/(double)num_elems);
  673.   avg.y = (float)(sum_y/(double)num_elems);
  674. }
  676. /* fill in half of the matrix with the distance measures;
  677. * only the upper half is filled in, i.e. if row<col there's a value
  678. */
  679. void OpticalFlow::DistanceMatrix(const CPointVector& features,
  680.                                  CDoubleMatrix& distances)
  681. {
  682.   int num_features = (int)features.size();
  683.   distances.resize(num_features);
  684.   for (int row=0; row<num_features; row++) {
  685.     distances[row].resize(num_features);
  686.     for (int col=row+1; col<num_features; col++) {
  687.       double dx = features[col].x-features[row].x;
  688.       double dy = features[col].y-features[row].y;
  689.       distances[row][col] = sqrt(dx*dx+dy*dy);
  690.     }
  691.   }
  692. }
  694. bool OpticalFlow::TooCloseToOthers(const CPointVector& features, int indx, int len)
  695. {
  696.   for (int fcnt=0; fcnt<len; fcnt++) {
  697.     if (indx==fcnt) continue;
  698.     double dx = features[fcnt].x-features[indx].x;
  699.     double dy = features[fcnt].y-features[indx].y;
  700.     double dist = sqrt(dx*dx+dy*dy);
  701.     if (dist<m_min_distance) {
  702.       return true;
  703.     }
  704.   }
  705.   return false;
  706. }
  708. /* randomly re-place one of two points that are too close to each other,
  709. * move others that are too far from the centroid towards the centroid
  710. */
  711. void OpticalFlow::ConcentrateFeatures(IplImage* rgbImage,
  712.                                       CPointVector& features,
  713.                                       CStatusVector& status,
  714.                                       int last_width, int last_height,
  715.                                       bool use_prob_distr)
  716. {
  717.   ASSERT(m_pProbDistrProvider);
  719.   int round_or_square = 2; // the area in which we will concentrate the features, 1 is round, 2 is rectangular
  720.   CDoubleMatrix distances;
  721.   DistanceMatrix(features, distances);
  722.   // discard the highest 15percent of distances when computing centroid
  723.   int discard_num_distances = (int)(0.15*(double)distances.size());
  724.   int centroid_index = FindCentroid(distances, discard_num_distances);
  725.   CvPoint2D32f centroid = features[centroid_index];
  726.   double halfwidth = last_width/2.0;
  727.   double halfheight = last_height/2.0;
  728.   double left = centroid.x-halfwidth;
  729.   double right = centroid.x+halfwidth;
  730.   double top = centroid.y-halfheight;
  731.   double bottom = centroid.y+halfheight;
  732.   int num_features = (int)features.size();
  734.   for (int fcnt=0; fcnt<num_features; fcnt++) {
  735.     m_feature_status[fcnt] = 1;
  736.     for (int scnd=fcnt+1; scnd<num_features; scnd++) {
  737.       double dist;
  738.       if (fcnt<scnd) {
  739.         dist = distances[fcnt][scnd];
  740.       } else { // scnd<fcnt
  741.         dist = distances[scnd][fcnt];
  742.       }
  743.       int cnt=0;
  744.       bool too_close = dist<m_min_distance;
  745.       while (too_close && cnt<20) {
  746.         // try to find a location that was segmented as foreground color
  747.         // and one that's far away enough from all other features
  748.         int x, y;
  749.         double prob = 1.0;
  750.         int prob_cnt = 0;
  751.         do {
  752.           x = (int) (centroid.x-last_width/2.0+(rand()%last_width));
  753.           y = (int) (centroid.y-last_height/2.0+(rand()%last_height));
  754.           x = min(x, rgbImage->width-1);
  755.           y = min(y, rgbImage->height-1);
  756.           x = max(x, 0);
  757.           y = max(y, 0);
  758.           ColorBGR* color;
  759.           GetPixel(rgbImage, x, y, &color);
  760.           if (use_prob_distr) {
  761.             prob = m_pProbDistrProvider->LookupProb(*color);
  762.           }
  763.           prob_cnt++;
  764.         } while (prob<MIN_PROB_FOREGROUND && prob_cnt<20);
  765.         features[fcnt].x = (float)x;
  766.         features[fcnt].y = (float)y;
  767.         status[fcnt] = 0;
  769.         too_close = TooCloseToOthers(features, fcnt, (int)features.size());
  771.         cnt++;
  772.       } // end while
  773.     } // end for scnd
  775.     if (centroid_index!=fcnt) {
  776.       if (round_or_square==1) {
  777.         // round
  778.         double dist = -1;
  779.         if (fcnt<centroid_index) {
  780.           dist = distances[fcnt][centroid_index];
  781.         } else if (centroid_index<fcnt) {
  782.           dist = distances[centroid_index][fcnt];
  783.         } else {
  784.           // will not enter the following loop
  785.         }
  786.         while (dist>last_width/2.0) {
  787.           features[fcnt].x = (float)((features[fcnt].x+centroid.x)/2.0);
  788.           features[fcnt].y = (float)((features[fcnt].y+centroid.y)/2.0);
  789.           m_feature_status[fcnt] = 0;
  790.           double dx = features[fcnt].x-centroid.x;
  791.           double dy = features[fcnt].y-centroid.y;
  792.           dist = sqrt(dx*dx+dy*dy);;
  793.         }
  794.       } else {
  795.         // rectangular area
  796.         // move towards horizontal center
  797.         while (features[fcnt].x<left || features[fcnt].x>right) {
  798.           features[fcnt].x = (float)((features[fcnt].x+centroid.x)/2.0);
  799.           status[fcnt] = 0;
  800.         }
  801.         // move towards vertical center
  802.         while (features[fcnt].y<top || features[fcnt].y>bottom) {
  803.           features[fcnt].y = (float)((features[fcnt].y+centroid.y)/2.0);
  804.           status[fcnt] = 0;
  805.         }
  806.       }
  807.     } // end centroid_index!=fcnt
  809.     features[fcnt].x = min(max(features[fcnt].x, .0f), rgbImage->width-1.0f);
  810.     features[fcnt].y = min(max(features[fcnt].y, .0f), rgbImage->height-1.0f);
  811.   }
  812. }
  814. /* randomly re-place one of two points that are too close to each other,
  815. * move others that are too far from the centroid towards the centroid
  816. */
  817. void OpticalFlow::AddNewFeatures(IplImage* rgbImage,
  818.                                  CPointVector& features,
  819.                                  CStatusVector& status,
  820.                                  int last_width, int last_height,
  821.                                  bool use_prob_distr)
  822. {
  823.   ASSERT(m_pProbDistrProvider);
  825.   CDoubleMatrix distances;
  826.   DistanceMatrix(features, distances);
  827.   // discard the highest 15percent of distances when computing centroid
  828.   int discard_num_distances = (int)(0.15*(double)distances.size());
  829.   int centroid_index = FindCentroid(distances, discard_num_distances);
  830.   CvPoint2D32f centroid = features[centroid_index];
  831. //  double halfwidth = last_width/2.0;
  832. //  double halfheight = last_height/2.0;
  833. //  double left = centroid.x-halfwidth;
  834. //  double right = centroid.x+halfwidth;
  835. //  double top = centroid.y-halfheight;
  836. //  double bottom = centroid.y+halfheight;
  838.   for (int fcnt=m_num_features_tracked; fcnt<m_target_num_features; fcnt++) {
  839.     // try to find a location that was segmented as foreground color
  840.     int x, y;
  841.     double prob = 1.0;
  842.     int prob_cnt = 0;
  843.     do {
  844.       x = (int) (centroid.x-last_width/2.0+(rand()%last_width));
  845.       y = (int) (centroid.y-last_height/2.0+(rand()%last_height));
  846.       x = min(x, rgbImage->width-1);
  847.       y = min(y, rgbImage->height-1);
  848.       x = max(x, 0);
  849.       y = max(y, 0);
  850.       ColorBGR* color;
  851.       GetPixel(rgbImage, x, y, &color);
  852.       if (use_prob_distr) {
  853.         prob = m_pProbDistrProvider->LookupProb(*color);
  854.       }
  855.       prob_cnt++;
  856.     } while (prob<MIN_PROB_FOREGROUND && prob_cnt<20);
  858.     features[fcnt].x = (float)x;
  859.     features[fcnt].y = (float)y;
  860.     status[fcnt] = 0;
  861.   } // end for fcnt
  862. }
  864. void OpticalFlow::GetPixel(IplImage* rgbImage, int x, int y, ColorBGR** color)
  865. {
  866.   ASSERT(rgbImage->nChannels==3);
  867.   *color = (ColorBGR*) cvPtr2D(rgbImage, y, x);
  868. }