iCub-main
VisuoThread.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Carlo Ciliberto, Vadim Tikhanoff
4 * email: carlo.ciliberto@iit.it vadim.tikhanoff@iit.it
5 * website: www.robotcub.org
6 * Permission is granted to copy, distribute, and/or modify this program
7 * under the terms of the GNU General Public License, version 2 or any
8 * later version published by the Free Software Foundation.
9 *
10 * A copy of the license can be found at
11 * http://www.robotcub.org/icub/license/gpl.txt
12 *
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16 * Public License for more details
17 */
18 #include <utility>
19 #include <yarp/os/Time.h>
20 #include <yarp/cv/Cv.h>
21 #include <iCub/VisuoThread.h>
22 
23 
24 void VisuoThread::close()
25 {
26  if (closed)
27  return;
28 
29  interrupt();
30 
31  outPort[LEFT].close();
32  outPort[RIGHT].close();
33 
34  imgPort[LEFT].close();
35  imgPort[RIGHT].close();
36 
37  mCUTPort[LEFT].close();
38  mCUTPort[RIGHT].close();
39 
40  pftInPort.close();
41  pftOutPort.close();
42 
43  boundMILPort.close();
44  cmdMILPort.close();
45  recMILPort.close();
46 
47  cmdMSRPort.close();
48  recMSRPort.close();
49 
50  if(img[LEFT]!=NULL)
51  {
52  delete img[LEFT];
53  img[LEFT]=NULL;
54  }
55 
56  if(img[RIGHT]!=NULL)
57  {
58  delete img[RIGHT];
59  img[RIGHT]=NULL;
60  }
61 
62  closed=true;
63 }
64 
65 
66 bool VisuoThread::checkTracker(Vector *vec)
67 {
68  bool track=false;
69 
70  trackBuffer.push_back(*vec);
71 
72  if(isTracking() && trackBuffer.size()>minTrackBufSize)
73  {
74  double speed_avg[2];
75  double speed_std[2];
76  speed_avg[LEFT]=speed_avg[RIGHT]=0.0;
77  speed_std[LEFT]=speed_std[RIGHT]=0.0;
78 
79  for(unsigned int i=1; i<trackBuffer.size(); i++)
80  {
81  double speed[2];
82  speed[LEFT]=(trackBuffer[i-1][0]-trackBuffer[i][0])*(trackBuffer[i-1][0]-trackBuffer[i][0])+
83  (trackBuffer[i-1][1]-trackBuffer[i][1])*(trackBuffer[i-1][1]-trackBuffer[i][1]);
84  speed_avg[LEFT]+=sqrt(speed[LEFT]);
85  speed_std[LEFT]+=speed[LEFT];
86 
87  speed[RIGHT]=(trackBuffer[i-1][0]-trackBuffer[i][0])*(trackBuffer[i-1][0]-trackBuffer[i][0])+
88  (trackBuffer[i-1][1]-trackBuffer[i][1])*(trackBuffer[i-1][1]-trackBuffer[i][1]);
89  speed_avg[RIGHT]+=sqrt(speed[RIGHT]);
90  speed_std[RIGHT]+=speed[RIGHT];
91  }
92 
93  double n=1.0/(trackBuffer.size()-1);
94  speed_avg[LEFT]*=n;
95  speed_std[LEFT]=n*speed_std[LEFT]-speed_avg[LEFT]*speed_avg[LEFT];
96  speed_avg[RIGHT]*=n;
97  speed_std[RIGHT]=n*speed_std[RIGHT]-speed_avg[RIGHT]*speed_avg[RIGHT];
98 
99  track=true;
100 
101  // check that the speeds are not varying too much
102  if(speed_avg[LEFT]>speedStdThresh || speed_avg[RIGHT]>speedStdThresh)
103  {
104  track=false;
105  }
106 
107  double dist=(trackBuffer.back()[0]-trackBuffer.back()[6])*(trackBuffer.back()[0]-trackBuffer.back()[6])+
108  (trackBuffer.back()[1]-trackBuffer.back()[7])*(trackBuffer.back()[1]-trackBuffer.back()[7]);
109 
110  // avoid strabicity
111  if(sqrt(dist)>stereoDistThresh)
112  {
113  track=false;
114  }
115  // check that both images are tracking similar things
116  }
117 
118  while(trackBuffer.size()>maxTrackBufSize)
119  trackBuffer.pop_front();
120 
121  return track;
122 }
123 
124 
125 void VisuoThread::startTracker(const Vector &stereo, const int &side)
126 {
127  int eye_in_use;
128 
129  if(stereo[2*dominant_eye]==0.0 && stereo[2*dominant_eye+1]==0.0)
130  {
131  if(stereo[2*(1-dominant_eye)]==0.0 && stereo[2*(1-dominant_eye)+1]==0.0)
132  return;
133 
134  eye_in_use=1-dominant_eye;
135  }
136  else
137  eye_in_use=dominant_eye;
138 
139  cv::Mat imgMat;
140  imgMutex.lock();
141  if (img[eye_in_use]!=nullptr)
142  imgMat=yarp::cv::toCvMat(*img[eye_in_use]).clone();
143  imgMutex.unlock();
144 
145  if (!imgMat.empty())
146  {
147  tpl.resize(side,side);
148  cv::Mat tplMat=yarp::cv::toCvMat(tpl);
149 
150  int x=stereo[2*eye_in_use]-0.5*side<0?0:cvRound(stereo[2*eye_in_use]-0.5*side);
151  int y=stereo[2*eye_in_use+1]-0.5*side<0?0:cvRound(stereo[2*eye_in_use+1]-0.5*side);
152  int width=stereo[2*eye_in_use]+0.5*side>=imgMat.cols?imgMat.cols-x:side;
153  int height=stereo[2*eye_in_use+1]+0.5*side>=imgMat.rows?imgMat.rows-y:side;
154 
155  imgMat(cv::Rect(x,y,width,height)).copyTo(tplMat);
156  cv::cvtColor(tplMat,tplMat,CV_BGR2RGB);
157 
158  pftOutPort.write(tpl);
159  }
160 
161  lock_guard<mutex> lck(trackMutex);
162  stereoTracker.side=side;
163  tracking=true;
164  trackMode=MODE_TRACK_TEMPLATE;
165 }
166 
167 
169 {
170  pftOutPort.write(tpl);
171 
172  lock_guard<mutex> lck(trackMutex);
173  tracking=true;
174 }
175 
176 void VisuoThread::updateImages()
177 {
178  ImageOf<PixelRgb> *iL=imgPort[LEFT].read(false);
179  ImageOf<PixelRgb> *iR=imgPort[RIGHT].read(false);
180 
181  lock_guard<mutex> lck(imgMutex);
182  if(iL!=NULL)
183  {
184  if(img[LEFT]!=NULL)
185  delete img[LEFT];
186 
187  img[LEFT]=new ImageOf<PixelRgb>(*iL);
188 
189  newImage[LEFT]=true;
190  }
191 
192  if(iR!=NULL)
193  {
194  if(img[RIGHT]!=NULL)
195  delete img[RIGHT];
196 
197  img[RIGHT]=new ImageOf<PixelRgb>(*iR);
198 
199  newImage[RIGHT]=true;
200  }
201 }
202 
203 void VisuoThread::updateLocationsMIL()
204 {
205  Bottle *bLocations=recMILPort.read(false);
206 
207  if(bLocations!=NULL)// && bLocations->size()==1)
208  {
209  lock_guard<mutex> lck(MILMutex);
210  locations.clear();
211 
212  for(int i=0; i<bLocations->get(0).asList()->size(); i++)
213  {
214  Bottle *b=bLocations->get(0).asList()->get(i).asList();
215  locations[b->get(0).asString()]=cvPoint(b->get(1).asInt32(),b->get(2).asInt32());
216  }
217  }
218 }
219 
220 
221 void VisuoThread::updateMotionCUT()
222 {
223  // Detect motion
224  Bottle *bMotion[2];
225  bMotion[LEFT]=mCUTPort[LEFT].read(false);
226  bMotion[RIGHT]=mCUTPort[RIGHT].read(false);
227 
228  double tNow=Time::now();
229 
230  bool detected=true;
231 
232  lock_guard<mutex> lck(motMutex);
233 
234  Vector stereo(4); stereo=0.0;
235  for(int cam=0; cam<2; cam++)
236  {
237  // if even the first element of the buffer is far (in time)
238  // clear the whole buffer
239  if(buffer[cam].size()>0 && tNow-buffer[cam].back().t>timeTol)
240  buffer[cam].clear();
241 
242  // If only a single moving blob has been detected
243  if (bMotion[cam] && bMotion[cam]->size()==1)
244  {
245  Item item;
246  item.t=Time::now();
247  item.size=bMotion[cam]->get(0).asList()->get(2).asFloat64();
248  item.p=cvPoint(bMotion[cam]->get(0).asList()->get(0).asInt32(),bMotion[cam]->get(0).asList()->get(1).asInt32());
249  buffer[cam].push_back(item);
250 
251  stereo[2*cam]=bMotion[cam]->get(0).asList()->get(0).asFloat64();
252  stereo[2*cam+1]=bMotion[cam]->get(0).asList()->get(1).asFloat64();
253  }
254 
255  // Avoid time unconsistencies
256  while (buffer[cam].size() && (tNow-buffer[cam].front().t)>timeTol)
257  buffer[cam].pop_front();
258  }
259 
260  if(trackMode==MODE_TRACK_MOTION)
261  stereo_target.set(stereo);
262 }
263 
264 
265 
266 void VisuoThread::updatePFTracker()
267 {
268  Vector *trackVec=pftInPort.read(false);
269 
270  Vector stereo;
271  if(trackVec!=NULL && trackVec->size()==12)
272  {
273  //must check if the tracker has gone mad.
274  if(checkTracker(trackVec))
275  {
276  trackMutex.lock();
277  stereoTracker.vec=*trackVec;
278  trackMutex.unlock();
279 
280  stereo.resize(4);
281  stereo[0]=stereoTracker.vec[0];
282  stereo[1]=stereoTracker.vec[1];
283  stereo[2]=stereoTracker.vec[6];
284  stereo[3]=stereoTracker.vec[7];
285 
286  if(trackMode==MODE_TRACK_TEMPLATE)
287  stereo_target.set(stereo);
288  }
289  else
290  {
291  lock_guard<mutex> lck(trackMutex);
292  stereoTracker.vec.clear();
293  stereoTracker.side=0;
294  }
295  }
296 
297  lock_guard<mutex> lck(imgMutex);
298  if(img[LEFT]!=NULL && img[RIGHT]!=NULL)
299  {
300  ImageOf<PixelRgb> drawImg[2];
301  drawImg[LEFT]=*img[LEFT];
302  drawImg[RIGHT]=*img[RIGHT];
303 
304  if(stereoTracker.vec.size()==12)
305  {
306  cv::Mat tmpL=yarp::cv::toCvMat(drawImg[LEFT]);
307  cv::circle(tmpL,cv::Point(cvRound(stereoTracker.vec[0]),cvRound(stereoTracker.vec[1])),3,cv::Scalar(0,255),3);
308  cv::rectangle(tmpL,cv::Point(cvRound(stereoTracker.vec[2]),cvRound(stereoTracker.vec[3])),
309  cv::Point(cvRound(stereoTracker.vec[4]),cvRound(stereoTracker.vec[5])),cv::Scalar(0,255),3);
310 
311  cv::Mat tmpR=yarp::cv::toCvMat(drawImg[RIGHT]);
312  cv::circle(tmpR,cv::Point(cvRound(stereoTracker.vec[6]),cvRound(stereoTracker.vec[7])),3,cv::Scalar(0,255),3);
313  cv::rectangle(tmpR,cv::Point(cvRound(stereoTracker.vec[8]),cvRound(stereoTracker.vec[9])),
314  cv::Point(cvRound(stereoTracker.vec[10]),cvRound(stereoTracker.vec[11])),cv::Scalar(0,255),3);
315 
316  Bottle v;
317  v.clear();
318  Bottle &vl=v.addList();
319  vl.addInt32(cvRound(stereoTracker.vec[0]));
320  vl.addInt32(cvRound(stereoTracker.vec[1]));
321  vl.addInt32(stereoTracker.side);
322  Bottle &vr=v.addList();
323  vr.addInt32(cvRound(stereoTracker.vec[6]));
324  vr.addInt32(cvRound(stereoTracker.vec[7]));
325  vr.addInt32(stereoTracker.side);
326 
327  boundMILPort.write(v);
328  }
329 
330  if(newImage[LEFT])
331  outPort[LEFT].write(drawImg[LEFT]);
332 
333  if(newImage[RIGHT])
334  outPort[RIGHT].write(drawImg[RIGHT]);
335 
336  //avoid writing multiple times the same image
337  newImage[LEFT]=false;
338  newImage[RIGHT]=false;
339  }
340 }
341 
342 
343 
344 VisuoThread::VisuoThread(ResourceFinder &_rf, Initializer *initializer)
345  :PeriodicThread(0.02),rf(_rf),stereo_target(initializer->stereo_target),opcPort(initializer->port_opc)
346 {
347  buffer[LEFT].clear();
348  buffer[RIGHT].clear();
349 
350  locations.clear();
351 
352  img[LEFT]=NULL;
353  img[RIGHT]=NULL;
354 
355  stereoTracker.vec.clear();
356  stereoTracker.side=0;
357 
358  trackMode=MODE_TRACK_TEMPLATE;
359  closed=false;
360 }
361 
363 {
364  string name=rf.find("name").asString();
365 
366  Bottle bVision=rf.findGroup("vision");
367 
368  setPeriod((double)bVision.check("period",Value(20)).asInt32()/1000.0);
369 
370  minMotionBufSize=bVision.check("minMotionBufSize",Value(10)).asInt32();
371  minTrackBufSize=bVision.check("minTrackBufSize",Value(1)).asInt32();
372  maxTrackBufSize=bVision.check("maxTrackBufSize",Value(2)).asInt32();
373  timeTol=bVision.check("timeTol",Value(0.5)).asFloat64();
374  motionStdThresh=bVision.check("motionStdThresh",Value(5.0)).asFloat64();
375  speedStdThresh=bVision.check("speedStdThresh",Value(700.0)).asFloat64();
376  stereoDistThresh=bVision.check("stereoDistThresh",Value(300.0)).asFloat64();
377  dominant_eye=bVision.check("dominant_eye",Value("left")).asString()=="left"?LEFT:RIGHT;
378 
379  rawWaitThresh=bVision.check("raw_detection_wait_thresh",Value(15.0)).asFloat64();
380  motionWaitThresh=bVision.check("motion_detection_wait_thresh",Value(5.0)).asFloat64();
381  objectWaitThresh=bVision.check("object_detection_wait_thresh",Value(5.0)).asFloat64();
382 
383  // open ports
384  outPort[LEFT].open("/"+name+"/left/img:o");
385  outPort[RIGHT].open("/"+name+"/right/img:o");
386 
387  imgPort[LEFT].open("/"+name+"/left/img:i");
388  imgPort[RIGHT].open("/"+name+"/right/img:i");
389 
390  mCUTPort[LEFT].open("/"+name+"/left/blobs:i");
391  mCUTPort[RIGHT].open("/"+name+"/right/blobs:i");
392 
393  rawInPort[LEFT].open("/"+name+"/left/raw:i");
394  rawInPort[RIGHT].open("/"+name+"/right/raw:i");
395 
396  boundMILPort.open("/"+name+"/MIL/window:o");
397  cmdMILPort.open("/"+name+"/MIL/cmd:o");
398  recMILPort.open("/"+name+"/MIL/rec:i");
399 
400  cmdMSRPort.open("/"+name+"/MSR/cmd:o");
401  recMSRPort.open("/"+name+"/MSR/rec:i");
402 
403  pftInPort.open("/"+name+"/tracker:i");
404  pftOutPort.open("/"+name+"/tracker:o");
405 
406  segPort.open("/"+name+"/seg:o");
407 
408  newImage[LEFT]=false;
409  newImage[RIGHT]=false;
410 
411  show=false;
412 
413  interrupted=false;
414 
415  trackMode=MODE_TRACK_TEMPLATE;
416 
417  closed=false;
418  return true;
419 }
420 
422 {
423  updateImages();
424  updatePFTracker();
425  updateLocationsMIL();
426  updateMotionCUT();
427 }
428 
429 
431 {
432  close();
433 }
434 
435 
436 
437 //get the action target point in the images reference frame
438 bool VisuoThread::getTarget(Value &type, Bottle &options)
439 {
440  bool ok=false;
441 
442  Bottle &bNewTarget=options.addList();
443  bNewTarget.addString("target");
444 
445  Bottle &bTarget=bNewTarget.addList();
446  Bottle &bNewStereo=bTarget.addList();
447  bNewStereo.addString("stereo");
448 
449  Bottle &bStereo=bNewStereo.addList();
450 
451  if(type.isList())
452  {
453  Bottle *list=type.asList();
454  if(list->size()>2)
455  {
456  if(list->get(0).asString()=="right")
457  {
458  bStereo.addFloat64(0.0);
459  bStereo.addFloat64(0.0);
460  bStereo.addFloat64(list->get(1).asFloat64());
461  bStereo.addFloat64(list->get(2).asFloat64());
462  }
463  else if(list->get(0).asString()=="left")
464  {
465  bStereo.addFloat64(list->get(1).asFloat64());
466  bStereo.addFloat64(list->get(2).asFloat64());
467  bStereo.addFloat64(0.0);
468  bStereo.addFloat64(0.0);
469  }
470  else if(list->get(0).asString()=="cartesian")
471  {
472  Bottle &bNewCartesian=bTarget.addList();
473  bNewCartesian.addString("cartesian");
474 
475  Bottle &bCartesian=bNewCartesian.addList();
476 
477  for(int i=1; i<list->size(); i++)
478  bCartesian.addFloat64(list->get(i).asFloat64());
479  }
480  else
481  {
482  Bottle &bNewCartesian=bTarget.addList();
483  bNewCartesian.addString("cartesian");
484 
485  Bottle &bCartesian=bNewCartesian.addList();
486 
487  for(int i=0; i<list->size(); i++)
488  bCartesian.addFloat64(list->get(i).asFloat64());
489  }
490 
491  ok=true;
492  }
493  else if(list->size()==2)
494  {
495  bStereo.addFloat64(list->get(0).asFloat64());
496  bStereo.addFloat64(list->get(1).asFloat64());
497  bStereo.addFloat64(0.0);
498  bStereo.addFloat64(0.0);
499 
500  ok=true;
501  }
502  }
503  else switch(type.asVocab32())
504  {
505  case PARAM_FIXATION:
506  {
507  ok=getFixation(bStereo);
508  break;
509  }
510  case PARAM_MOTION:
511  {
512  ok=getMotion(bStereo);
513  break;
514  }
515  case PARAM_TRACK:
516  {
517  ok=getTrack(bStereo);
518  break;
519  }
520  case PARAM_RAW:
521  {
522  ok=getRaw(bStereo);
523  break;
524  }
525 
526  default:
527  {
528  Bottle &bName=bTarget.addList();
529  bName.addString("name");
530  bName.addString(type.asString());
531 
532  getObject(type.asString(),bStereo);
533  break;
534  }
535  }
536 
537 
538  return ok;
539 }
540 
541 
542 bool VisuoThread::getFixation(Bottle &bStereo)
543 {
544  Vector stereo(4);
545 
546  imgMutex.lock();
547  if(img[LEFT]!=NULL)
548  {
549  stereo[0]=stereo[2]=0.5*img[LEFT]->width();
550  stereo[1]=stereo[3]=0.5*img[LEFT]->height();
551  }
552  else
553  {
554  stereo[0]=stereo[2]=160;
555  stereo[1]=stereo[3]=120;
556  }
557  imgMutex.unlock();
558 
559  for(size_t i=0; i<stereo.size(); i++)
560  bStereo.addFloat64(stereo[i]);
561 
562  int side=40;
563  startTracker(stereo,side);
564 
565  return true;
566 }
567 
568 bool VisuoThread::getMotion(Bottle &bStereo)
569 {
570  Vector stereo;
571 
572  bool ok=false;
573 
574  double t=Time::now();
575 
576  while(Time::now()-t<motionWaitThresh && !interrupted)
577  {
578  lock_guard<mutex> lck(motMutex);
579  // If the buffers are sufficently dense and not so small, return true.
580  double size=0.0;
581  if (buffer[LEFT].size()>minMotionBufSize && buffer[RIGHT].size()>minMotionBufSize)
582  {
583  Vector p[2];
584  for (int cam=0; cam<2; cam++)
585  {
586  double size_cam,u,v,n;
587  double u_std,v_std;
588  size_cam=u=v=0.0;
589  u_std=v_std=0.0;
590  n=1.0/buffer[cam].size();
591 
592  for (unsigned int i=0; i<buffer[cam].size(); i++)
593  {
594  size_cam+=buffer[cam][i].size;
595  u+=buffer[cam][i].p.x;
596  v+=buffer[cam][i].p.y;
597  u_std+=buffer[cam][i].p.x*buffer[cam][i].p.x;
598  v_std+=buffer[cam][i].p.y*buffer[cam][i].p.y;
599  }
600 
601  size_cam*=n;
602  u*=n;
603  v*=n;
604  u_std=sqrt(n*u_std-u*u);
605  v_std=sqrt(n*v_std-v*v);
606 
607  //check if the motion detected point is not wildly moving
608  if (u_std<motionStdThresh && v_std<motionStdThresh)
609  {
610  p[cam].resize(2);
611  p[cam][0]=u;
612  p[cam][1]=v;
613  }
614  else
615  break;
616 
617  size+=size_cam;
618  }
619 
620  int side=cvRound(2*sqrt(size/3.1415)*2);
621 
622  if (p[LEFT].size()==2 && p[RIGHT].size()==2)
623  {
624  stereo.resize(4);
625  stereo[0]=p[LEFT][0];
626  stereo[1]=p[LEFT][1];
627  stereo[2]=p[RIGHT][0];
628  stereo[3]=p[RIGHT][1];
629 
630  startTracker(stereo,cvRound(side));
631 
632  ok=true;
633  }
634  }
635  }
636 
637  for(size_t i=0; i<stereo.size(); i++)
638  bStereo.addFloat64(stereo[i]);
639 
640  return ok;
641 }
642 
643 
644 bool VisuoThread::getTrack(Bottle &bStereo)
645 {
646  lock_guard<mutex> lck(trackMutex);
647 
648  Vector stereo;
649  bool ok=false;
650 
651  if(stereoTracker.vec.size()==12)
652  {
653  stereo.resize(4);
654  stereo[0]=stereoTracker.vec[0];
655  stereo[1]=stereoTracker.vec[1];
656  stereo[2]=stereoTracker.vec[6];
657  stereo[3]=stereoTracker.vec[7];
658  }
659 
660  for(size_t i=0; i<stereo.size(); i++)
661  bStereo.addFloat64(stereo[i]);
662 
663  tracking=true;
664  trackMode=MODE_TRACK_TEMPLATE;
665 
666  return ok;
667 }
668 
669 
670 
671 bool VisuoThread::getRaw(Bottle &bStereo)
672 {
673  Vector stereo;
674 
675  bool ok=false;
676 
677  //empty the buffers
678  Bottle *bL=rawInPort[LEFT].read(false);
679  Bottle *bR=rawInPort[RIGHT].read(false);
680 
681  double t=Time::now();
682 
683  while(stereo.size()!=4 && Time::now()-t<rawWaitThresh && !interrupted)
684  {
685  Bottle *bL=rawInPort[LEFT].read(false);
686  Bottle *bR=rawInPort[RIGHT].read(false);
687 
688  if(bL!=NULL || bR!=NULL)
689  stereo.resize(4,0.0);
690 
691  if(bL!=NULL)
692  {
693  stereo[0]=bL->get(0).asInt32();
694  stereo[1]=bL->get(1).asInt32();
695  }
696 
697  if(bR!=NULL)
698  {
699  stereo[2]=bR->get(0).asInt32();
700  stereo[3]=bR->get(1).asInt32();
701  }
702  }
703 
704  if(stereo.size()==4)
705  {
706  int side=40;
707  startTracker(stereo,side);
708  ok=true;
709  }
710 
711  for(size_t i=0; i<stereo.size(); i++)
712  bStereo.addFloat64(stereo[i]);
713 
714  return ok;
715 }
716 
717 // if the object is located in both cameras, return its stereo position
718 bool VisuoThread::getObject(const std::string &object_name, Bottle &bStereo)
719 {
720  Vector stereo;
721 
722  bool ok=opcPort.getStereoPosition(object_name,stereo);
723 
724  if(!ok)
725  {
726  double t=Time::now();
727  while(Time::now()-t<objectWaitThresh && !interrupted)
728  {
729  lock_guard<mutex> lck(MILMutex);
730  if(locations.count(object_name)>0)
731  {
732  stereo.resize(4);
733  stereo[0]=locations[object_name].x;
734  stereo[1]=locations[object_name].y;
735  stereo[2]=160.0;
736  stereo[3]=120.0;
737 
738  ok=true;
739  }
740  }
741  }
742 
743  for(size_t i=0; i<stereo.size(); i++)
744  bStereo.addFloat64(stereo[i]);
745 
746  return ok;
747 }
748 
749 
750 Bottle VisuoThread::recogMSR(string &obj_name)
751 {
752  Bottle bDetect;
753  Bottle *bMSR=recMSRPort.read(false);
754  if(bMSR!=NULL)
755  {
756  bDetect=*bMSR;
757  obj_name=bDetect.get(0).asString();
758  }
759  return bDetect;
760 }
761 
762 
763 
765 {
766  if(!isTracking())
767  return false;
768 
769  Bottle command,reply;
770  command.fromString("learn " + obj_name + " template");
771 
772  cmdMILPort.write(command,reply);
773 
774  return (reply.get(0).toString()=="ack");
775 }
776 
777 
779 {
780  Bottle command,reply;
781  command.fromString("set label 0");
782 
783  cmdMILPort.write(command,reply);
784 
785  return (reply.get(0).toString()=="ack");
786 }
787 
789 {
790  Bottle command,reply;
791  command.fromString("set label template");
792 
793  cmdMILPort.write(command,reply);
794 
795  return (reply.get(0).toString()=="ack");
796 }
797 
799 {
800  Bottle command,reply;
801  command.fromString("train");
802 
803  cmdMILPort.write(command,reply);
804 
805  return (reply.get(0).toString()=="ack");
806 }
807 
808 
809 
811 {
812  outPort[LEFT].interrupt();
813  outPort[RIGHT].interrupt();
814 
815  imgPort[LEFT].interrupt();
816  imgPort[RIGHT].interrupt();
817 
818  mCUTPort[LEFT].interrupt();
819  mCUTPort[RIGHT].interrupt();
820 
821  rawInPort[LEFT].interrupt();
822  rawInPort[RIGHT].interrupt();
823 
824  pftInPort.interrupt();
825  pftOutPort.interrupt();
826 
827  boundMILPort.interrupt();
828  cmdMILPort.interrupt();
829  recMILPort.interrupt();
830 
831  cmdMSRPort.interrupt();
832  recMSRPort.interrupt();
833 
834  opcPort.interrupt();
835 
836  interrupted=true;
837 }
838 
840 {
841  outPort[LEFT].resume();
842  outPort[RIGHT].resume();
843 
844  imgPort[LEFT].resume();
845  imgPort[RIGHT].resume();
846 
847  mCUTPort[LEFT].resume();
848  mCUTPort[RIGHT].resume();
849 
850  rawInPort[LEFT].resume();
851  rawInPort[RIGHT].resume();
852 
853  pftInPort.resume();
854  pftOutPort.resume();
855 
856  boundMILPort.resume();
857  cmdMILPort.resume();
858  recMILPort.resume();
859 
860  cmdMSRPort.resume();
861  recMSRPort.resume();
862 
863  opcPort.resume();
864 
865  interrupted=false;
866 }
867 
868 
PARAM_MOTION
#define PARAM_MOTION
Definition: VisuoThread.h:41
width
static int width
Definition: iCub_Sim.cpp:53
VisuoThread::checkTracker
bool checkTracker(Vector *vec)
Definition: VisuoThread.cpp:66
MODE_TRACK_MOTION
#define MODE_TRACK_MOTION
Definition: VisuoThread.h:46
VisuoThread::isTracking
bool isTracking()
Definition: VisuoThread.h:155
VisuoThread::recogMSR
Bottle recogMSR(string &obj_name)
Definition: VisuoThread.cpp:750
VisuoThread::startTracker
void startTracker(const Vector &stereo, const int &side)
Definition: VisuoThread.cpp:125
MODE_TRACK_TEMPLATE
#define MODE_TRACK_TEMPLATE
Definition: VisuoThread.h:45
VisuoThread::restartTracker
void restartTracker()
Definition: VisuoThread.cpp:168
VisuoThread::interrupt
void interrupt()
Definition: VisuoThread.cpp:810
VisuoThread::VisuoThread
VisuoThread(ResourceFinder &_rf, Initializer *initializer)
Definition: VisuoThread.cpp:344
PARAM_TRACK
#define PARAM_TRACK
Definition: VisuoThread.h:42
VisuoThread::startLearningMIL
bool startLearningMIL(const string &obj_name)
Definition: VisuoThread.cpp:764
Initializer
Definition: utils.h:113
VisuoThread.h
p
p
Definition: show_eyes_axes.m:23
StereoTracker::vec
Vector vec
Definition: VisuoThread.h:55
n
int n
Definition: debugFunctions.cpp:58
VisuoThread::threadInit
virtual bool threadInit()
Definition: VisuoThread.cpp:362
StereoTracker::side
int side
Definition: VisuoThread.h:56
y
y
Definition: show_eyes_axes.m:21
x
x
Definition: compute_ekf_sym.m:21
state::ok
@ ok
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
VisuoThread::resumeLearningMIL
bool resumeLearningMIL()
Definition: VisuoThread.cpp:788
LEFT
#define LEFT
Definition: utils.h:33
VisuoThread::run
virtual void run()
Definition: VisuoThread.cpp:421
VisuoThread::getTarget
bool getTarget(Value &type, Bottle &target)
Definition: VisuoThread.cpp:438
v
static int v
Definition: iCub_Sim.cpp:42
height
static int height
Definition: iCub_Sim.cpp:54
PARAM_FIXATION
#define PARAM_FIXATION
Definition: VisuoThread.h:40
PARAM_RAW
#define PARAM_RAW
Definition: VisuoThread.h:43
VisuoThread::reinstate
void reinstate()
Definition: VisuoThread.cpp:839
RIGHT
#define RIGHT
Definition: utils.h:34
ObjectPropertiesCollectorPort::getStereoPosition
bool getStereoPosition(const string &obj_name, Vector &stereo)
Definition: utils.cpp:29
VisuoThread::suspendLearningMIL
bool suspendLearningMIL()
Definition: VisuoThread.cpp:778
VisuoThread::trainMIL
bool trainMIL()
Definition: VisuoThread.cpp:798
VisuoThread::threadRelease
virtual void threadRelease()
Definition: VisuoThread.cpp:430
StereoTarget::set
void set(const Vector &stereo)
Definition: utils.h:91