iCub-main
Loading...
Searching...
No Matches
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
24void 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
66bool 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
125void 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
168void VisuoThread::restartTracker()
169{
170 pftOutPort.write(tpl);
171
172 lock_guard<mutex> lck(trackMutex);
173 tracking=true;
174}
175
176void 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
203void 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
221void 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
266void 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
344VisuoThread::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
362bool VisuoThread::threadInit()
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
421void VisuoThread::run()
422{
423 updateImages();
424 updatePFTracker();
425 updateLocationsMIL();
426 updateMotionCUT();
427}
428
429
430void VisuoThread::threadRelease()
431{
432 close();
433}
434
435
436
437//get the action target point in the images reference frame
438bool 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
542bool 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
568bool 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
644bool 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
671bool 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
718bool 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
750Bottle 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
764bool VisuoThread::startLearningMIL(const std::string &obj_name)
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
778bool VisuoThread::suspendLearningMIL()
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
788bool VisuoThread::resumeLearningMIL()
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
798bool VisuoThread::trainMIL()
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
810void VisuoThread::interrupt()
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
839void VisuoThread::reinstate()
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_TRACK
#define PARAM_TRACK
Definition VisuoThread.h:42
PARAM_MOTION
#define PARAM_MOTION
Definition VisuoThread.h:41
PARAM_RAW
#define PARAM_RAW
Definition VisuoThread.h:43
PARAM_FIXATION
#define PARAM_FIXATION
Definition VisuoThread.h:40
MODE_TRACK_MOTION
#define MODE_TRACK_MOTION
Definition VisuoThread.h:46
MODE_TRACK_TEMPLATE
#define MODE_TRACK_TEMPLATE
Definition VisuoThread.h:45
ObjectPropertiesCollectorPort::getStereoPosition
bool getStereoPosition(const string &obj_name, Vector &stereo)
Definition utils.cpp:29
StereoTarget::set
void set(const Vector &stereo)
Definition utils.h:91
VisuoThread::suspendLearningMIL
bool suspendLearningMIL()
Definition VisuoThread.cpp:778
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
VisuoThread::threadRelease
virtual void threadRelease()
Definition VisuoThread.cpp:430
VisuoThread::run
virtual void run()
Definition VisuoThread.cpp:421
VisuoThread::checkTracker
bool checkTracker(Vector *vec)
Definition VisuoThread.cpp:66
VisuoThread::isTracking
bool isTracking()
Definition VisuoThread.h:155
VisuoThread::startLearningMIL
bool startLearningMIL(const string &obj_name)
Definition VisuoThread.cpp:764
VisuoThread::restartTracker
void restartTracker()
Definition VisuoThread.cpp:168
VisuoThread::threadInit
virtual bool threadInit()
Definition VisuoThread.cpp:362
VisuoThread::VisuoThread
VisuoThread(ResourceFinder &_rf, Initializer *initializer)
Definition VisuoThread.cpp:344
VisuoThread::getTarget
bool getTarget(Value &type, Bottle &target)
Definition VisuoThread.cpp:438
VisuoThread::resumeLearningMIL
bool resumeLearningMIL()
Definition VisuoThread.cpp:788
x
x
Definition compute_ekf_sym.m:21
n
int n
Definition debugFunctions.cpp:58
LEFT
#define LEFT
Definition utils.h:33
RIGHT
#define RIGHT
Definition utils.h:34
y
y
Definition show_eyes_axes.m:21
p
p
Definition show_eyes_axes.m:23
Generated on Wed Dec 18 2024 16:34:48 for iCub-main by doxygen 1.9.8