iCub-main
particleFilter.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Authors: Vadim Tikhanoff, Carlo Ciliberto
4  *
5  * Algorithm taken from R. Hess, A. Fern, "Discriminatively trained particle filters for complex multi-object tracking," cvpr, pp.240-247, 2009 IEEE Conference on Computer Vision and Pattern Recognition, 2009
6  *
7  * email: vadim.tikhanoff@iit.it
8  * website: www.robotcub.org
9  * Permission is granted to copy, distribute, and/or modify this program
10  * under the terms of the GNU General Public License, version 2 or any
11  * later version published by the Free Software Foundation.
12  *
13  * A copy of the license can be found at
14  * http://www.robotcub.org/icub/license/gpl.txt
15  *
16  * This program is distributed in the hope that it will be useful, but
17  * WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
19  * Public License for more details
20  */
21 
22 #include <utility>
23 #include <yarp/cv/Cv.h>
24 #include <iCub/particleFilter.h>
25 
26 using namespace std;
27 using namespace yarp::os;
28 using namespace yarp::sig;
29 using namespace yarp::cv;
30 
31 /**********************************************************/
32 int particle_cmp( const void* p1, const void* p2 )
33 {
36 
37  if( _p1->w > _p2->w )
38  return -1;
39  if( _p1->w < _p2->w )
40  return 1;
41  return 0;
42 }
43 /**********************************************************/
44 
46 {
47  cout << "deleting dynamic objects" << endl;
48  if(regions!=NULL)
49  free_regions ( regions, num_objects );
50 
51  gsl_rng_free ( rng );
52  free_histos ( ref_histos, num_objects);
53  if(particles != NULL)
54  free ( particles);
55 
56  if (temp)
57  {
58  cout << "releasing temp" << endl;
59  cvReleaseImage(&temp);
60  }
61  cout << "finished particle thread" << endl;
62 }
63 /**********************************************************/
65 {
66  firstFrame = true;
67  num_objects = 0;
68  regions = (CvRect **) malloc(sizeof(CvRect*));
69  num_particles = PARTICLES; /* number of particles */
70  rng = gsl_rng_alloc( gsl_rng_mt19937 );
71  gsl_rng_set( rng, (unsigned long)time(NULL) );
72  i = 0;
73  getTemplate = false;
74  gotTemplate = false;
75  sendTarget = false;
76  getImage = false;
77  particles = NULL;
78  new_particles = NULL;
79  temp = NULL;
80  ref_histos = NULL;
81  tpl = NULL;
82  total = 0;
83 }
84 /**********************************************************/
85 void PARTICLEThread::setName(string module)
86 {
87  this->moduleName = module;
88 }
89 
90 /**********************************************************/
92 {
93  /* initialize variables and create data-structures if needed */
94 
95  inputPortName = "/" + moduleName + "/image:i";
96  imageIn.open( inputPortName.c_str() );
97 
98  outputPortName = "/" + moduleName + "/image:o";
99  imageOut.open( outputPortName.c_str() );
100 
101  outputPortNameBlob = "/" + moduleName + "/blob/image:o";
102  imageOutBlob.open( outputPortNameBlob.c_str() );
103 
104  num_objects = 0;
105  init = true;
106 
107  updateNeeded=false;
108  bestTempl.templ=NULL;
109  bestTempl.w=0.0;
110  return true;
111 }
112 /**********************************************************/
114 {
115  while (isStopping() != true) { // the thread continues to run until isStopping() returns true
116 
117  getImage = ( imageIn.getInputCount() > 0 );
118 
119  if ( !getImage )
120  Time::delay(0.1);
121  else{
122 
123  if (init)
124  {//Get first RGB image to establish width, height:
125  cout << "initializing" << endl;
126  initAll();
127  }
128  else
129  {
130  iCubImage = imageIn.read();
131  lock_guard<mutex> lck(templateMutex);
132  imageIn.getEnvelope(targetTemp);
133  }
134 
135  frame = cvCreateImage(cvSize(width,height),IPL_DEPTH_8U, 3 );
136  cv::cvtColor(toCvMat(*iCubImage), cv::cvarrToMat(frame), CV_RGB2BGR);
137  frame_blob = cvCreateImage(cvSize(width,height),IPL_DEPTH_8U, 1 );
138 
139  if ( tpl != NULL )
140  {
141  cv::Mat tplMat=toCvMat(*tpl);
142  tpl_width = tpl->width();
143  tpl_height = tpl->height();
144  res_width = width - tpl_width + 1;
145  res_height = height - tpl_height + 1;
146  cvReleaseImage(&temp);
147  temp = cvCreateImage(cvSize(tplMat.size().width,tplMat.size().height),tplMat.depth(),tplMat.channels() );
148  cv::cvtColor(tplMat, cv::cvarrToMat(temp), CV_RGB2BGR);
149  gotTemplate = true;
150  firstFrame = true;
151  if (num_objects>0)
152  free(*regions);
153  num_objects = 0;
154  delete tpl;
155  tpl = NULL;
156 
157  lock_guard<mutex> lck(templateMutex);
158  while(tempList.size())
159  {
160  delete tempList.back().templ;
161  tempList.pop_back();
162  }
163  if(bestTempl.templ!=NULL)
164  delete bestTempl.templ;
165  bestTempl.w=0.0;
166  updateNeeded=false;
167  }
168 
169  if (gotTemplate)
170  {
171  runAll(frame);
172  }
173 
174  //send it all
175  if (imageOut.getOutputCount()>0)
176  {
177  ImageOf<PixelBgr> &img = imageOut.prepare();
178  img.resize(width,height);
179  cv::cvarrToMat(frame).copyTo(toCvMat(img));
180  imageOut.write();
181  }
182  if (imageOutBlob.getOutputCount()>0)
183  {
184  ImageOf<PixelMono> &imgBlob = imageOutBlob.prepare();
185  imgBlob.resize(width,height);
186  cv::cvarrToMat(frame_blob).copyTo(toCvMat(imgBlob));
187  imageOutBlob.write();
188  }
189  cvReleaseImage(&frame);
190  cvReleaseImage(&frame_blob);
191  }
192  } //while
193 }
194 /**********************************************************/
196 {
197  cout << "cleaning up..." << endl;
198  cout << "attempting to close ports" << endl;
199  imageIn.interrupt();
200  imageOut.interrupt();
201  imageOutBlob.interrupt();
202  imageIn.close();
203  imageOut.close();
204  imageOutBlob.close();
205 
206  lock_guard<mutex> lck(templateMutex);
207  while(tempList.size())
208  {
209  delete tempList.back().templ;
210  tempList.pop_back();
211  }
212 
213  if(bestTempl.templ!=NULL)
214  delete bestTempl.templ;
215  cout << "finished closing ports" << endl;
216 }
217 
218 /**********************************************************/
219 void PARTICLEThread::initAll()
220 {
221  //here some initialization
222  iCubImage = imageIn.read();
223  width = iCubImage->width();
224  height = iCubImage->height();
225  init = false;
226 }
227 /**********************************************************/
228 void PARTICLEThread::runAll(IplImage *img)
229 {
230  img_hsv = bgr2hsv( img );
231  if (firstFrame)
232  {
233  w = img->width;
234  h = img->height;
235 
236  while( num_objects == 0 )
237  {
238  num_objects = get_regionsImage( img, regions );
239  if( num_objects == 0 )
240  fprintf( stderr, "Problem! seg has issues\n" );
241  }
242  if (ref_histos!=NULL)
243  free_histos ( ref_histos, num_objects);
244 
245  ref_histos = compute_ref_histos( img_hsv, *regions, num_objects );
246  if (particles != NULL)
247  free (particles);
248 
249  particles= init_distribution( *regions, ref_histos, num_objects, num_particles );
250  }
251  else
252  {
253  // perform prediction and measurement for each particle
254  for( j = 0; j < num_particles; j++ )
255  {
256  particles[j] = transition( particles[j], w, h, rng );
257  s = particles[j].s;
258  particles[j].w = likelihood( img_hsv, cvRound(particles[j].y),
259  cvRound( particles[j].x ),
260  cvRound( particles[j].width * s ),
261  cvRound( particles[j].height * s ),
262  particles[j].histo );
263  }
264  // normalize weights and resample a set of unweighted particles
265  normalize_weights( particles, num_particles );
266  new_particles = resample( particles, num_particles );
267  free( particles );
268  particles = new_particles;
269  }
270  qsort( particles, num_particles, sizeof( PARTICLEThread::particle ), &particle_cmp );
271 
272  averageMutex.lock();
273  for( j = 0; j < num_particles; j++ )
274  average += particles[j].w;
275 
276  average = average/num_particles;
277  averageMutex.unlock();
278 
279  //display most likely particle ------------------
280  color = cvScalar(255,0,0);
281  targetMutex.lock();
282  targetTemp.clear();
283  //if (imageOut.getOutputCount()>0)
284  display_particle( frame, particles[0], color, targetTemp );
285 
286  if (imageOutBlob.getOutputCount()>0)
287  display_particleBlob( frame_blob, particles[0], targetTemp );
288  targetMutex.unlock();
289  trace_template( frame, particles[0] );
290 
291  cvReleaseImage(&img_hsv);
292 }
293 /**********************************************************/
294 void PARTICLEThread::setTemplate(ImageOf<PixelRgb> *_tpl)
295 {
296  tpl = new ImageOf<PixelRgb> (*_tpl);
297 }
298 /**********************************************************/
299 void PARTICLEThread::pushTarget(Vector &target, Stamp &stamp)
300 {
301  lock_guard<mutex> lck(targetMutex);
302  for (size_t i=0; i< targetTemp.length(); i++ )
303  target.push_back(targetTemp[i]);
304 
305  stamp=targetStamp;
306 }
307 /**********************************************************/
309 {
310  lock_guard<mutex> lck(averageMutex);
311  float tmpAv = 0.0;
312  tmpAv = average;
313  return tmpAv;
314 }
315 /**********************************************************/
316 int PARTICLEThread::get_regionsImage( IplImage* frame, CvRect** regions )
317 {
318  firstFrame = false;
319  params p;
320  CvRect* r;
321  int i; //x1, y1, x2, y2, w, h;
322 
323  p.orig_img = cvCloneImage( frame );
324  p.cur_img = NULL;
325  p.n = 0;
326 
327  res = cvCreateImage( cvSize( res_width, res_height ), IPL_DEPTH_32F, 1 );
328 
329  cvMatchTemplate( frame, temp, res, CV_TM_SQDIFF );
330  cvMinMaxLoc( res, &minval, &maxval, &minloc, &maxloc, 0 );
331 
332  cvReleaseImage( &(p.orig_img) ); //what was the purpose of this image?
333  cvReleaseImage( &res );
334  if( p.cur_img )
335  cvReleaseImage( &(p.cur_img) );
336  p.n = 1;
337  // extract regions defined by user; store as an array of rectangles ----------
338 
339  r = (CvRect*) malloc ( p.n * sizeof( CvRect ) );
340 
341  for( i = 0; i < p.n; i++ )
342  r[i] = cvRect( minloc.x, minloc.y, tpl_width, tpl_height );
343 
344  *regions = r;
345 
346  return p.n;
347 }
348 /**********************************************************/
349 PARTICLEThread::histogram** PARTICLEThread::compute_ref_histos( IplImage* frame, CvRect* regions, int n )
350 {
351  histogram** histos = (histogram**) malloc( n * sizeof( histogram* ) );
352  IplImage* tmp;
353  int i;
354 
355  // extract each region from frame and compute its histogram
356  for( i = 0; i < n; i++ )
357  {
358  cvSetImageROI( frame, regions[i]);
359  tmp = cvCreateImage( cvGetSize(frame), IPL_DEPTH_32F, 3 );
360  cvCopy( frame, tmp, NULL );
361  cvResetImageROI( frame );
362  histos[i] = calc_histogram( &tmp, 1 );
363  normalize_histogram( histos[i] );
364  cvReleaseImage( &tmp );
365  }
366  return histos;
367 }
368 /**********************************************************/
369 PARTICLEThread::histogram* PARTICLEThread::calc_histogram( IplImage** imgs, int n )
370 {
371  IplImage* img;
372  histogram* histo;
373  IplImage* h, * s, * v;
374  float* hist;
375  int i, r, c, bin;
376  histo = (histogram*) malloc( sizeof(histogram) );
377  histo->n = NH*NS + NV;
378  hist = histo->histo;
379  memset( hist, 0, histo->n * sizeof(float) );
380  for( i = 0; i < n; i++ )
381  {
382  // extract individual HSV planes from image
383  img = imgs[i];
384  h = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );
385  s = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );
386  v = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );
387  cvSplit( img, h, s, v, NULL );
388 
389  // increment appropriate histogram bin for each pixel
390  for( r = 0; r < img->height; r++ )
391  for( c = 0; c < img->width; c++ )
392  {
393  bin = histo_bin( pixval32f( h, r, c ),
394  pixval32f( s, r, c ),
395  pixval32f( v, r, c ) );
396  hist[bin] += 1;
397  }
398  cvReleaseImage( &h );
399  cvReleaseImage( &s );
400  cvReleaseImage( &v );
401  }
402  return histo;
403 }
404 /**********************************************************/
405 void PARTICLEThread::free_histos( PARTICLEThread::histogram** histo, int n)
406 {
407  for (int i = 0; i < n; i++)
408  free (histo[i]);
409 
410  free(histo);
411 }
412 /**********************************************************/
413 void PARTICLEThread::free_regions( CvRect** regions, int n)
414 {
415  for (int i = 0; i < n; i++)
416  free (regions[i]);
417 
418  free(regions);
419 }
420 /**********************************************************/
421 void PARTICLEThread::normalize_histogram( PARTICLEThread::histogram* histo )
422 {
423  float* hist;
424  float sum = 0, inv_sum;
425  int i, n;
426 
427  hist = histo->histo;
428  n = histo->n;
429 
430  // compute sum of all bins and multiply each bin by the sum's inverse
431  for( i = 0; i < n; i++ )
432  sum += hist[i];
433  inv_sum = 1.0f / sum;
434  for( i = 0; i < n; i++ )
435  hist[i] *= inv_sum;
436 }
437 /**********************************************************/
438 int PARTICLEThread::histo_bin( float h, float s, float v )
439 {
440  int hd, sd, vd;
441 
442  // if S or V is less than its threshold, return a "colorless" bin
443  vd = MIN( (int)(v * NV / V_MAX), NV-1 );
444  if( s < S_THRESH || v < V_THRESH )
445  return NH * NS + vd;
446 
447  // otherwise determine "colorful" bin
448  hd = MIN( (int)(h * NH / H_MAX), NH-1 );
449  sd = MIN( (int)(s * NS / S_MAX), NS-1 );
450  return sd * NH + hd;
451 }
452 /**********************************************************/
453 PARTICLEThread::particle* PARTICLEThread::init_distribution( CvRect* regions, histogram** histos, int n, int p)
454 {
455  particle* particles;
456  int np;
457  float x, y;
458  int i, j, width, height, k = 0;
459 
460  particles = (particle* )malloc( p * sizeof( particle ) );
461  np = p / n;
462 
463  // create particles at the centers of each of n regions
464  for( i = 0; i < n; i++ ) {
465  width = regions[i].width;
466  height = regions[i].height;
467  x = regions[i].x + (float)(width / 2);
468  y = regions[i].y + (float) (height / 2);
469  for( j = 0; j < np; j++ )
470  {
471  particles[k].x0 = particles[k].xp = particles[k].x = x;
472  particles[k].y0 = particles[k].yp = particles[k].y = y;
473  particles[k].sp = particles[k].s = 1.0;
474  particles[k].width = width;
475  particles[k].height = height;
476  particles[k].histo = histos[i];
477  particles[k++].w = 0;
478  }
479  }
480  // make sure to create exactly p particles
481  i = 0;
482  while( k < p )
483  {
484  width = regions[i].width;
485  height = regions[i].height;
486  x = regions[i].x + (float) (width / 2);
487  y = regions[i].y + (float) (height / 2);
488  particles[k].x0 = particles[k].xp = particles[k].x = x;
489  particles[k].y0 = particles[k].yp = particles[k].y = y;
490  particles[k].sp = particles[k].s = 1.0;
491  particles[k].width = width;
492  particles[k].height = height;
493  particles[k].histo = histos[i];
494  particles[k++].w = 0;
495  i = ( i + 1 ) % n;
496  }
497  return particles;
498 }
499 /**********************************************************/
500 PARTICLEThread::particle PARTICLEThread::transition( const PARTICLEThread::particle &p, int w, int h, gsl_rng* rng )
501 {
502  float x, y, s;
504 
505  // sample new state using second-order autoregressive dynamics
506  x = pfot_A1 * ( p.x - p.x0 ) + pfot_A2 * ( p.xp - p.x0 ) +
507  pfot_B0 * (float)(gsl_ran_gaussian( rng, TRANS_X_STD )) + p.x0;
508  pn.x = MAX( 0.0f, MIN( (float)w - 1.0f, x ) );
509  y = pfot_A1 * ( p.y - p.y0 ) + pfot_A2 * ( p.yp - p.y0 ) +
510  pfot_B0 * (float)(gsl_ran_gaussian( rng, TRANS_Y_STD )) + p.y0;
511  pn.y = MAX( 0.0f, MIN( (float)h - 1.0f, y ) );
512  s = pfot_A1 * ( p.s - 1.0f ) + pfot_A2 * ( p.sp - 1.0f ) +
513  pfot_B0 * (float)(gsl_ran_gaussian( rng, TRANS_S_STD )) + 1.0f;
514  pn.s = MAX( 0.1f, s );
515 
516  pn.xp = p.x;
517  pn.yp = p.y;
518  pn.sp = p.s;
519  pn.x0 = p.x0;
520  pn.y0 = p.y0;
521  pn.width = p.width;
522  pn.height = p.height;
523  pn.histo = p.histo;
524  pn.w = 0;
525 
526  return pn;
527 }
528 /**********************************************************/
529 float PARTICLEThread::likelihood( IplImage* img, int r, int c, int w, int h, histogram* ref_histo )
530 {
531  IplImage* tmp;
532  histogram* histo;
533  float d_sq;
534 
535  // extract region around (r,c) and compute and normalize its histogram
536  cvSetImageROI( img, cvRect( c - w / 2, r - h / 2, w, h ) );
537  tmp = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 3 );
538  cvCopy( img, tmp, NULL );
539  cvResetImageROI( img );
540  histo = calc_histogram( &tmp, 1 );
541  cvReleaseImage( &tmp );
542  normalize_histogram( histo );
543 
544  // compute likelihood as e^{\lambda D^2(h, h^*)}
545  d_sq = histo_dist_sq( histo, ref_histo );
546  free( histo );
547  return exp( -LAMBDA * d_sq );
548 }
549 /**********************************************************/
550 float PARTICLEThread::histo_dist_sq( histogram* h1, histogram* h2 )
551 {
552  float* hist1, * hist2;
553  float sum = 0;
554  int i, n;
555 
556  n = h1->n;
557  hist1 = h1->histo;
558  hist2 = h2->histo;
559  // According the the Battacharyya similarity coefficient,
560  // D = \sqrt{ 1 - \sum_1^n{ \sqrt{ h_1(i) * h_2(i) } } }
561 
562  for( i = 0; i < n; i++ )
563  sum += sqrt( hist1[i]*hist2[i] );
564 
565  if(sum<1.0f)
566  return sqrt(1.0f - sum);
567  else
568  {
569  // should be impossible to get here...
570  fprintf(stdout,"Error in similarity computation!\n");
571  return 1.0f-sum;
572  }
573 }
574 /**********************************************************/
575 void PARTICLEThread::normalize_weights( particle* particles, int n )
576 {
577  float sum = 0;
578  int i;
579 
580  for( i = 0; i < n; i++ )
581  sum += particles[i].w;
582  for( i = 0; i < n; i++ )
583  particles[i].w /= sum;
584 }
585 /**********************************************************/
586 PARTICLEThread::particle* PARTICLEThread::resample( particle* particles, int n )
587 {
588  particle* _new_particles;
589  int i, j, np, k = 0;
590 
591  //new_particles = (particle* )malloc( sizeof( particle ) );
592  qsort( particles, n, sizeof( particle ), &particle_cmp );
593  _new_particles = (particle* ) malloc( n * sizeof( particle ) );
594 
595  for( i = 0; i < n; i++ )
596  {
597  np = cvRound( particles[i].w * n );
598  for( j = 0; j < np; j++ )
599  {
600  _new_particles[k++] = particles[i];
601  if( k == n )
602  goto exit;
603  }
604  }
605  while( k < n )
606  _new_particles[k++] = particles[0];
607 
608  exit:
609  return _new_particles;
610 }
611 /**********************************************************/
612 void PARTICLEThread::display_particle( IplImage* img, const PARTICLEThread::particle &p, CvScalar color, Vector& target )
613 {
614  int x0, y0, x1, y1;
615  x0 = cvRound( p.x - 0.5 * p.s * p.width );
616  y0 = cvRound( p.y - 0.5 * p.s * p.height );
617  x1 = x0 + cvRound( p.s * p.width );
618  y1 = y0 + cvRound( p.s * p.height );
619 
620  cvRectangle( img, cvPoint( x0, y0 ), cvPoint( x1, y1 ), color, 1, 8, 0 );
621 
622  cvCircle (img, cvPoint((x0+x1)/2, (y0+y1)/2), 3, cvScalar(255,0 ,0),1);
623  cvCircle (img, cvPoint( x0, y0), 3, cvScalar(0, 255 ,0),1);
624  cvCircle (img, cvPoint( x1, y1), 3, cvScalar(0, 255 ,0),1);
625 
626  target.push_back((x0+x1)/2);
627  target.push_back((y0+y1)/2);
628  target.push_back(x0);
629  target.push_back(y0);
630  target.push_back(x1);
631  target.push_back(y1);
632 }
633 /**********************************************************/
634 void PARTICLEThread::display_particleBlob( IplImage* img, const PARTICLEThread::particle &p, Vector& target )
635 {
636  int x0, y0, x1, y1;
637  x0 = cvRound( p.x - 0.5 * p.s * p.width );
638  y0 = cvRound( p.y - 0.5 * p.s * p.height );
639  x1 = x0 + cvRound( p.s * p.width );
640  y1 = y0 + cvRound( p.s * p.height );
641 
642  int step = img->widthStep/sizeof(uchar);
643  uchar* data = (uchar *)img->imageData;
644 
645  for (int i=0; i<img->height; i++)
646  {
647  for (int j=0; j<img->width; j++)
648  {
649  data[i*step+j] = 0;
650  }
651  }
652  cvRectangle(img, cvPoint(x0,y0), cvPoint(x1,y1), cvScalar(255,255, 255), CV_FILLED);
653 }
654 /**********************************************************/
655 void PARTICLEThread::trace_template( IplImage* img, const particle &p )
656 {
658  {
659  //create the template image
660  TemplateStruct t;
661  t.w=p.w;
662  t.templ=new ImageOf<PixelRgb>;
663  t.templ->resize(cvRound(p.s*p.width),cvRound(p.s*p.height));
664 
665  cv::Mat imgMat(cv::cvarrToMat(img),cv::Rect(cvRound(p.x-0.5*p.s*p.width),
666  cvRound(p.y-0.5*p.s*p.height),
667  cvRound(p.s*p.width),
668  cvRound(p.s*p.height)));
669 
670  cv::cvtColor(imgMat,toCvMat(*t.templ),CV_BGR2RGB);
671 
672  cvResetImageROI(img);
673 
674  tempList.push_front(t);
675  if (tempList.size()>TEMP_LIST_SIZE)
676  {
677  delete tempList.back().templ;
678  tempList.pop_back();
679  }
680  }
681  //keep in check the best template
682  lock_guard<mutex> lck(templateMutex);
683  //detect if the image tracker should be updated with a template from the stored ones
685  updateNeeded=true;
686 
687  int best_idx=-1;
688  float best_w=0.0;
689  for(unsigned int i=0; i<tempList.size(); i++)
690  {
691  if(tempList[i].w<best_w)
692  {
693  best_idx=i;
694  best_w=tempList[i].w;
695  }
696  }
697 
698  if(best_idx==-1)
699  {
700  updateNeeded=false;
701  }
702  else if(bestTempl.w!=best_w) //avoid releasing and copying the same image
703  {
704  if(bestTempl.templ==NULL)
705  bestTempl.templ=new ImageOf<PixelRgb>;
706 
707  *bestTempl.templ=*tempList[best_idx].templ;
708  bestTempl.w=best_w;
709  }
710 }
711 /**********************************************************/
712 float PARTICLEThread::pixval32f(IplImage* img, int r, int c)
713 {
714  return ( (float*)(img->imageData + img->widthStep*r) )[c];
715 }
716 /**********************************************************/
717 IplImage* PARTICLEThread::bgr2hsv( IplImage* bgr )
718 {
719  IplImage* bgr32f, * hsv;
720 
721  bgr32f = cvCreateImage( cvGetSize(bgr), IPL_DEPTH_32F, 3 );
722  hsv = cvCreateImage( cvGetSize(bgr), IPL_DEPTH_32F, 3 );
723  cvConvertScale( bgr, bgr32f, 1.0 / 255.0, 0 );
724  cvCvtColor( bgr32f, hsv, CV_BGR2HSV );
725  cvReleaseImage( &bgr32f );
726  return hsv;
727 }
728 /**********************************************************/
730 {
731  TemplateStruct best;
732  best.templ=NULL;
733  best.w=0.0;
734 
735  lock_guard<mutex> lck(templateMutex);
736  if(updateNeeded)
737  {
738  best.templ=new ImageOf<PixelRgb>;
739  *best.templ=*bestTempl.templ;
740  best.w=bestTempl.w;
741  }
742  return best;
743 }
744 /**********************************************************/
745 PARTICLEManager::PARTICLEManager() : PeriodicThread(0.02)
746 {
747  tpl = NULL;
748 }
749 /**********************************************************/
751 /**********************************************************/
752 void PARTICLEManager::setName(string module)
753 {
754  this->moduleName = module;
755 }
756 /**********************************************************/
758 {
759  //create all ports
760  inputPortNameTemp = "/" + moduleName + "/template/image:i";
761  imageInTemp.open( inputPortNameTemp.c_str() );
762 
763  outputPortNameTarget = "/" + moduleName + "/target:o";
764  target.open( outputPortNameTarget.c_str() );
765 
766  disparityPort.open(("/"+moduleName+"/triangulation:io").c_str());
767  target3D.open(("/"+moduleName+"/target3d:o").c_str());
768 
769  particleThreadLeft = new PARTICLEThread();
770  particleThreadRight = new PARTICLEThread();
771 
772  particleThreadLeft->setName((moduleName + "/left").c_str());
773  particleThreadRight->setName((moduleName + "/right").c_str());
774 
775  shouldSend = false;
776  particleThreadLeft->start();
777  particleThreadRight->start();
778  return true;
779 }
780 /**********************************************************/
782 {
783  TemplateStruct templLeft,templRight;
784 
785  // if there is no need for a template update, the NULL pointers are returned
786  templLeft=particleThreadLeft->getBestTemplate();
787  templRight=particleThreadRight->getBestTemplate();
788 
789  ImageOf<PixelRgb> *tmp_tpl=templLeft.w>templRight.w?templLeft.templ:templRight.templ;
790 
791  // copy tmp_tpl whether it is NULL or not
792  tpl = tmp_tpl;
793  tpl = imageInTemp.read(false);
794 
795  if (tpl!=NULL)
796  {
797  shouldSend = true;
798  particleThreadLeft->setTemplate(tpl);
799  particleThreadRight->setTemplate(tpl);
800  }
801  Vector targetTemp;
802  Stamp leftStamp,rightStamp;
803 
804  particleThreadLeft->pushTarget(targetTemp,leftStamp);
805  particleThreadRight->pushTarget(targetTemp,rightStamp);
806 
807  float averageTempLeft = 0.0;
808  float averageTempRight = 0.0;
809 
810  averageTempLeft = particleThreadLeft->getAverage();
811  averageTempLeft = averageTempLeft *100;
812  //fprintf(stdout,"the average is %lf \n",averageTempLeft *100);
813 
814 
815  if (shouldSend)
816  {
817  if (target.getOutputCount() > 0 && targetTemp.size() > 4 &&
818  fabs(leftStamp.getTime()-rightStamp.getTime())<0.002)
819  {
820  //fprintf(stdout,"output %lf %lf %lf %lf\n", targetTemp[0], targetTemp[1], targetTemp[2], targetTemp[3]);
821  Stamp propagatedStamp(leftStamp.getCount(),0.5*(leftStamp.getTime()+rightStamp.getTime()));
822  target.setEnvelope(propagatedStamp);
823  target.write(targetTemp);
824  }
825  if (disparityPort.getOutputCount() > 0 && targetTemp.size())
826  {
827  //fprintf(stdout,"getting ready to send\n");
828  Bottle cmd, reply;
829  cmd.addFloat64(targetTemp[0]);
830  cmd.addFloat64(targetTemp[1]);
831  cmd.addFloat64(targetTemp[6]);
832  cmd.addFloat64(targetTemp[7]);
833  fprintf(stdout,"output %lf %lf %lf %lf\n", targetTemp[0], targetTemp[1], targetTemp[6], targetTemp[7]);
834 
835  disparityPort.write(cmd,reply);
836 
837  Bottle targets;
838  targets.addFloat64(reply.get(0).asFloat64());
839  targets.addFloat64(reply.get(1).asFloat64());
840  targets.addFloat64(reply.get(2).asFloat64());
841  targets.addFloat64(0.0);
842  targets.addFloat64(0.0);
843  targets.addFloat64(0.0);
844  targets.addFloat64(averageTempLeft);
845 
846  target3D.write(targets);
847  }
848  }
849  //clear templates
850  if(templLeft.templ!=NULL)
851  delete templLeft.templ;
852  if(templRight.templ!=NULL)
853  delete templRight.templ;
854 }
855 /**********************************************************/
857 {
858  cout << "cleaning up Manager..." << endl;
859  cout << "attempting to close ports Manager" << endl;
860  particleThreadLeft->stop();
861  particleThreadRight->stop();
862 
863  imageInTemp.interrupt();
864  target.interrupt();
865  imageInTemp.close();
866  target.close();
867 
868  disparityPort.interrupt();
869  disparityPort.close();
870 
871  target3D.interrupt();
872  target3D.close();
873 
874  cout << "deleteing thread " << endl;
875  delete particleThreadLeft;
876  delete particleThreadRight;
877  cout << "finished closing ports Manager" << endl;
878 }
879 /**********************************************************/
880 bool PARTICLEModule::configure(yarp::os::ResourceFinder &rf)
881 {
882  /* Process all parameters from both command-line and .ini file */
883 
884  moduleName = rf.check("name",
885  Value("templatePFTracker"),
886  "module name (string)").asString();
887 
888  setName(moduleName.c_str());
889 
890  handlerPortName = "/";
891  handlerPortName += getName(); // use getName() rather than a literal
892 
893  if (!handlerPort.open(handlerPortName.c_str()))
894  {
895  cout << getName() << ": Unable to open port " << handlerPortName << endl;
896  return false;
897  }
898 
899  attach(handlerPort);
900 
901  /* create the thread and pass pointers to the module parameters */
902  particleManager = new PARTICLEManager();
903 
904  /*pass the name of the module in order to create ports*/
905  particleManager->setName(moduleName);
906  /* now start the thread to do the work */
907  particleManager->start();
908 
909 
910  return true ;
911 }
912 /**********************************************************/
914 {
915  handlerPort.interrupt();
916  return true;
917 }
918 /**********************************************************/
920 {
921  handlerPort.close();
922  /* stop the thread */
923  particleManager->stop();
924  cout << "deleteing thread " << endl;
925  delete particleManager;
926  return true;
927 }
928 /**********************************************************/
929 bool PARTICLEModule::respond(const Bottle& command, Bottle& reply)
930 {
931  string helpMessage = string(getName().c_str()) +
932  " commands are: \n" +
933  "help \n" +
934  "quit \n";
935 
936  reply.clear();
937 
938  if (command.get(0).asString()=="quit")
939  {
940  reply.addString("quitting");
941  return false;
942  }
943  else if (command.get(0).asString()=="help")
944  {
945  cout << helpMessage;
946  reply.addString("ok");
947  }
948  else if (command.get(0).asString()=="reset")
949  {
950  cout << "reset has been asked "<< endl;
951  particleManager->shouldSend=false;
952  reply.addString("ok");
953  }
954  else
955  {
956  cout << "command not known - type help for more info" << endl;
957  }
958  return true;
959 }
960 /**********************************************************/
962 {
963  return true;
964 }
965 /**********************************************************/
967 {
968  return 0.1;
969 }
970 
971 
pfot_B0
#define pfot_B0
Definition: particleFilter.h:68
h
h
Definition: compute_ekf_sym.m:23
width
static int width
Definition: iCub_Sim.cpp:53
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
particleFilter.h
NS
#define NS
Definition: particleFilter.h:52
PARTICLEManager::~PARTICLEManager
~PARTICLEManager()
Definition: particleFilter.cpp:750
H_MAX
#define H_MAX
Definition: particleFilter.h:58
PARTICLES
#define PARTICLES
Definition: particleFilter.h:47
PARTICLEThread::histogram::n
int n
Definition: particleFilter.h:93
PARTICLEThread::PARTICLEThread
PARTICLEThread()
Definition: particleFilter.cpp:64
PARTICLEManager::threadInit
bool threadInit()
Definition: particleFilter.cpp:757
PARTICLEThread::particle::x
float x
Definition: particleFilter.h:98
PARTICLEThread::~PARTICLEThread
~PARTICLEThread()
Definition: particleFilter.cpp:45
PARTICLEModule::close
bool close()
Definition: particleFilter.cpp:919
cmd
cmd
Definition: dataTypes.h:30
PARTICLEThread::histogram::histo
float histo[NH *NS+NV]
Definition: particleFilter.h:92
PARTICLEThread::particle::xp
float xp
Definition: particleFilter.h:101
PARTICLEThread::threadRelease
void threadRelease()
Definition: particleFilter.cpp:195
PARTICLEThread::setName
void setName(std::string module)
Definition: particleFilter.cpp:85
PARTICLEModule::interruptModule
bool interruptModule()
Definition: particleFilter.cpp:913
PARTICLEThread::particle::sp
float sp
Definition: particleFilter.h:103
PARTICLEThread::particle::y
float y
Definition: particleFilter.h:99
PARTICLEModule::getPeriod
double getPeriod()
Definition: particleFilter.cpp:966
PARTICLEManager
Definition: particleFilter.h:214
PARTICLEModule::updateModule
bool updateModule()
Definition: particleFilter.cpp:961
PARTICLEThread::particle::y0
float y0
Definition: particleFilter.h:105
PARTICLEThread::particle::w
float w
Definition: particleFilter.h:109
V_MAX
#define V_MAX
Definition: particleFilter.h:60
p
p
Definition: show_eyes_axes.m:23
data
@ data
Definition: ST_M1_dataType.h:64
PARTICLEThread::histogram
Definition: particleFilter.h:90
pfot_A1
#define pfot_A1
Definition: particleFilter.h:66
TEMP_LIST_SIZE
#define TEMP_LIST_SIZE
Definition: particleFilter.h:73
PARTICLEThread
Definition: particleFilter.h:87
TRANS_X_STD
#define TRANS_X_STD
Definition: particleFilter.h:62
TRANS_S_STD
#define TRANS_S_STD
Definition: particleFilter.h:64
TemplateStruct::w
float w
Definition: particleFilter.h:79
PARTICLEThread::pushTarget
void pushTarget(yarp::sig::Vector &target, yarp::os::Stamp &stamp)
Definition: particleFilter.cpp:299
PARTICLEManager::threadRelease
void threadRelease()
Definition: particleFilter.cpp:856
TEMP_LIST_PARTICLE_THRES_HIGH
#define TEMP_LIST_PARTICLE_THRES_HIGH
Definition: particleFilter.h:74
n
int n
Definition: debugFunctions.cpp:58
y
y
Definition: show_eyes_axes.m:21
x
x
Definition: compute_ekf_sym.m:21
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
LAMBDA
#define LAMBDA
Definition: particleFilter.h:70
PARTICLEThread::getAverage
float getAverage()
Definition: particleFilter.cpp:308
S_THRESH
#define S_THRESH
Definition: particleFilter.h:55
PARTICLEThread::getBestTemplate
TemplateStruct getBestTemplate()
Definition: particleFilter.cpp:729
f
f
Definition: compute_ekf_sym.m:22
particle_cmp
int particle_cmp(const void *p1, const void *p2)
Definition: particleFilter.cpp:32
PARTICLEThread::particle
Definition: particleFilter.h:96
x1
x1
Definition: compute_ekf_sym.m:28
NV
#define NV
Definition: particleFilter.h:53
V_THRESH
#define V_THRESH
Definition: particleFilter.h:56
PARTICLEManager::shouldSend
bool shouldSend
Definition: particleFilter.h:236
PARTICLEThread::setTemplate
void setTemplate(yarp::sig::ImageOf< yarp::sig::PixelRgb > *tpl)
Definition: particleFilter.cpp:294
v
static int v
Definition: iCub_Sim.cpp:42
height
static int height
Definition: iCub_Sim.cpp:54
PARTICLEModule::respond
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: particleFilter.cpp:929
PARTICLEThread::particle::s
float s
Definition: particleFilter.h:100
PARTICLEThread::particle::width
int width
Definition: particleFilter.h:106
TemplateStruct::templ
yarp::sig::ImageOf< yarp::sig::PixelRgb > * templ
Definition: particleFilter.h:80
fprintf
fprintf(fid,'\n')
PARTICLEThread::particle::height
int height
Definition: particleFilter.h:107
PARTICLEModule::configure
bool configure(yarp::os::ResourceFinder &rf)
Definition: particleFilter.cpp:880
exp
exp(-x3 *T)]
PARTICLEThread::run
void run()
Definition: particleFilter.cpp:113
TemplateStruct
Definition: particleFilter.h:77
PARTICLEThread::particle::histo
histogram * histo
Definition: particleFilter.h:108
S_MAX
#define S_MAX
Definition: particleFilter.h:59
PARTICLEThread::particle::x0
float x0
Definition: particleFilter.h:104
TRANS_Y_STD
#define TRANS_Y_STD
Definition: particleFilter.h:63
MIN
#define MIN(a, b)
Definition: lmlibindirect.cpp:17
time
degrees time
Definition: sine.m:5
PARTICLEThread::threadInit
bool threadInit()
Definition: particleFilter.cpp:91
PARTICLEManager::PARTICLEManager
PARTICLEManager()
Definition: particleFilter.cpp:745
TEMP_LIST_PARTICLE_THRES_LOW
#define TEMP_LIST_PARTICLE_THRES_LOW
Definition: particleFilter.h:75
PARTICLEManager::setName
void setName(std::string module)
Definition: particleFilter.cpp:752
PARTICLEThread::particle::yp
float yp
Definition: particleFilter.h:102
pfot_A2
#define pfot_A2
Definition: particleFilter.h:67
PARTICLEManager::run
void run()
Definition: particleFilter.cpp:781
NH
#define NH
Definition: particleFilter.h:51