00001 #include "StereoDataCollector.H"
00002 #include <Logger.H>
00003 #include <string>
00004 #include <opencv/cv.h>
00005 #include <opencv/highgui.h>
00006
00007 using namespace std;
00008 using namespace WURDE;
00009 using namespace WURDEVision;
00010
00011 StereoDataCollector::StereoDataCollector(std::string name) : VisionOperator(name),
00012 m_myRF(STRAT_ASSIGNED, "DataRangeFinderClient"),
00013 m_myEgo(STRAT_ASSIGNED, "DataEgoMotionClient") {
00014
00015 registerConsumer(&m_myRF);
00016 registerConsumer(&m_myEgo);
00017 }
00018
00019 void StereoDataCollector::initOperator() {
00020
00021
00022
00023 m_dolaser=true;
00024 m_doodom=true;
00025 if(!m_dolaser&&!m_doodom){
00026 m_run=true;
00027 }else{
00028 m_run=false;
00029 }
00030
00031 t1state=STATE_RUN;
00032 t2state=STATE_RUN;
00033 t3state=STATE_RUN;
00034
00035
00036
00037 m_newt1image=false;
00038 m_newt2image=false;
00039 m_newt3image=false;
00040
00041
00042
00043 m_t1image=cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
00044 m_t2image=cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
00045 m_t3image=cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
00046
00047
00048
00049 m_t1simage=cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
00050 m_t2simage=cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
00051 m_t3simage=cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
00052
00053
00054
00055 m_stereo=true;
00056 index1=0;
00057 index2=0;
00058 index3=0;
00059
00060
00061
00062 pthread_create(&thread1,NULL,runStereoDataCollectorThread1,(void *)this);
00063 pthread_create(&thread2,NULL,runStereoDataCollectorThread2,(void *)this);
00064 pthread_create(&thread3,NULL,runStereoDataCollectorThread3,(void *)this);
00065
00066
00067
00068 if(g_globalConfiguration.haveModuleOption(m_name,"imageDir")){
00069 m_imageDir=g_globalConfiguration.getModuleOption(m_name,"imageDir");
00070 }else{
00071 m_imageDir=g_globalConfiguration.getDataDirectory();
00072 }
00073
00074 if(g_globalConfiguration.haveModuleOption(m_name,"imagePrefix")){
00075 m_imagePrefix=g_globalConfiguration.getModuleOption(m_name,"imagePrefix");
00076 }else{
00077 m_imagePrefix=m_name;
00078 }
00079
00080 m_state=STATE_RUN;
00081 m_format=FORMAT_CHAR_111_RGB;
00082
00083 next=1;
00084 }
00085
00086 void StereoDataCollector::runOperator(WURDEImage mImage, WURDEImage stereo) {
00087
00088 if(m_dolaser&&m_doodom&&m_myRF.newData()&&m_myEgo.newData()){
00089 m_run=true;
00090 m_myRF.getNextData();
00091 m_myEgo.getNextData();
00092 }
00093
00094 if(m_run){
00095 if(next==1){
00096 if(!m_newt1image){
00097
00098
00099
00100 if(m_t1image->width!=mImage.second->width||
00101 m_t1image->height!=mImage.second->height){
00102 m_t1image=NULL;
00103 m_t1image=cvCreateImage(cvSize(mImage.second->width, mImage.second->height), IPL_DEPTH_8U, 3);
00104 }
00105
00106 if(m_t1simage->width!=stereo.second->width||
00107 m_t1simage->height!=stereo.second->height){
00108 m_t1simage=NULL;
00109 m_t1simage=cvCreateImage(cvSize(stereo.second->width, stereo.second->height), IPL_DEPTH_8U, 3);
00110 }
00111
00112
00113 t1time=mImage.first;
00114
00115 cvConvertImage(mImage.second,m_t1image,CV_CVTIMG_SWAP_RB);
00116 cvConvertImage(stereo.second,m_t1simage,CV_CVTIMG_SWAP_RB);
00117
00118
00119
00120 if(m_dolaser){
00121 ranges1=m_myRF.data;
00122 }
00123 if(m_doodom){
00124 ego1=m_myEgo.data;
00125 }
00126 index1=index;
00127 index++;
00128 next=2;
00129
00130 m_newt1image=true;
00131
00132 }
00133 }else if (next==2){
00134
00135 if(!m_newt2image){
00136
00137
00138
00139 if(m_t2image->width!=mImage.second->width||
00140 m_t2image->height!=mImage.second->height){
00141 m_t2image=NULL;
00142 m_t2image=cvCreateImage(cvSize(mImage.second->width, mImage.second->height), IPL_DEPTH_8U, 3);
00143 }
00144
00145 if(m_t2simage->width!=stereo.second->width||
00146 m_t2simage->height!=stereo.second->height){
00147 m_t2simage=NULL;
00148 m_t2simage=cvCreateImage(cvSize(stereo.second->width, stereo.second->height), IPL_DEPTH_8U, 3);
00149 }
00150
00151
00152 t2time=mImage.first;
00153
00154 cvConvertImage(mImage.second,m_t2image,CV_CVTIMG_SWAP_RB);
00155 cvConvertImage(stereo.second,m_t2simage,CV_CVTIMG_SWAP_RB);
00156
00157
00158
00159 if(m_dolaser){
00160 ranges2=m_myRF.data;
00161 }
00162 if(m_doodom){
00163 ego2=m_myEgo.data;
00164 }
00165 index2=index;
00166 index++;
00167 next=3;
00168
00169 m_newt2image=true;
00170
00171 }
00172
00173 }else if (next==3){
00174 if(!m_newt3image){
00175
00176
00177
00178 if(m_t3image->width!=mImage.second->width||
00179 m_t3image->height!=mImage.second->height){
00180 m_t3image=NULL;
00181 m_t3image=cvCreateImage(cvSize(mImage.second->width, mImage.second->height), IPL_DEPTH_8U, 3);
00182 }
00183
00184 if(m_t3simage->width!=stereo.second->width||
00185 m_t3simage->height!=stereo.second->height){
00186 m_t3simage=NULL;
00187 m_t3simage=cvCreateImage(cvSize(stereo.second->width, stereo.second->height), IPL_DEPTH_8U, 3);
00188 }
00189
00190
00191 t3time=mImage.first;
00192
00193 cvConvertImage(mImage.second,m_t3image,CV_CVTIMG_SWAP_RB);
00194 cvConvertImage(stereo.second,m_t3simage,CV_CVTIMG_SWAP_RB);
00195
00196
00197
00198 if(m_dolaser){
00199 ranges3=m_myRF.data;
00200 }
00201 if(m_doodom){
00202 ego3=m_myEgo.data;
00203 }
00204 index3=index;
00205 index++;
00206 next=1;
00207
00208 m_newt3image=true;
00209
00210 }
00211
00212 }else if (next==4){
00213 if(!m_newt4image){
00214 queueImage(m_t4image,m_t4simage,t4time,mImage,stereo,
00215 index4,ranges4,ego4,m_newt4image);
00216 }
00217 }else if (next==5){
00218 if(!m_newt5image){
00219 queueImage(m_t5image,m_t5simage,t5time,mImage,stereo,
00220 index5,ranges5,ego5,m_newt5image);
00221 }
00222 }
00223 if(next>3){
00224 next=1;
00225 }
00226 m_myRF.getNextData();
00227
00228 }
00229
00230 }
00231
00232 inline void StereoDataCollector::queueImage(IplImage *mainImage,IplImage *stereoImage,Time &theTime,
00233 WURDE::WURDEImage &mImage, WURDE::WURDEImage &stereo,
00234 unsigned int &tindex,WURDE::RangeFinderDataStruct &tranges,
00235 WURDE::EgomotionDataStruct &tego, bool &newtImage){
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252 theTime=mImage.first;
00253
00254 cvConvertImage(mImage.second,mainImage,CV_CVTIMG_SWAP_RB);
00255 cvConvertImage(stereo.second,stereoImage,CV_CVTIMG_SWAP_RB);
00256
00257
00258
00259 if(m_dolaser){
00260 tranges=m_myRF.data;
00261 }
00262 if(m_doodom){
00263 tego=m_myEgo.data;
00264 }
00265
00266 tindex=index;
00267 index++;
00268 next=1;
00269
00270 newtImage=true;
00271
00272 }
00273
00274 void StereoDataCollector::quitOperator() {
00275
00276 t1state=STATE_QUIT;
00277 t2state=STATE_QUIT;
00278 t3state=STATE_QUIT;
00279
00280
00281
00282
00283 pthread_join(thread1,NULL);
00284 pthread_join(thread2,NULL);
00285 pthread_join(thread3,NULL);
00286
00287
00288
00289
00290
00291 cvReleaseImage(&m_t1image);
00292 cvReleaseImage(&m_t1simage);
00293 cvReleaseImage(&m_t2image);
00294 cvReleaseImage(&m_t2simage);
00295 cvReleaseImage(&m_t3image);
00296 cvReleaseImage(&m_t3simage);
00297
00298
00299
00300
00301
00302
00303
00304
00305 m_t1image=NULL;
00306 m_t1simage=NULL;
00307
00308 m_t2image=NULL;
00309 m_t2simage=NULL;
00310
00311 m_t3image=NULL;
00312 m_t3simage=NULL;
00313
00314
00315 m_t4image=NULL;
00316 m_t4simage=NULL;
00317 m_t5image=NULL;
00318 m_t5simage=NULL;
00319
00320 }
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506 inline void StereoDataCollector::fileWriterThread(Time &myTime,IplImage **_mainImage,IplImage **_stereoImage,
00507 RangeFinderDataStruct &myRanges, EgomotionDataStruct &myEgo,
00508 unsigned int &myIndex, bool &myNewImage,RunState &myState){
00509 std::ofstream outfile;
00510 std::stringstream filename;
00511 std::string thestring;
00512 filename.str(thestring);
00513
00514 filename.setf(ios_base::showpoint);
00515 filename.setf(ios_base::fixed,ios_base::floatfield);
00516 double stamp;
00517 vector<double>::iterator rangeiter;
00518
00519 IplImage *mainImage=*_mainImage,*stereoImage=*_stereoImage;
00520
00521 while(myState==STATE_RUN){
00522 if(myNewImage){
00523
00524 stamp=myTime.getSeconds()+myTime.getUSeconds()/1000000.0;
00525
00526
00527 filename << m_imageDir << "/" << m_imagePrefix << "-" << myIndex << "-main-"<< stamp << ".ppm";
00528 g_logdebug << "Filename " << filename.str() << endl;
00529 cvSaveImage((filename.str()).c_str(),mainImage);
00530 thestring.clear();
00531 filename.str(thestring);
00532
00533 filename << m_imageDir << "/" << m_imagePrefix << "-" << myIndex << "-stereo-" << stamp << ".ppm";
00534 g_logdebug << "Filename " << filename.str() << endl;
00535 cvSaveImage((filename.str()).c_str(),stereoImage);
00536 thestring.clear();
00537 filename.str(thestring);
00538
00539 if(m_dolaser){
00540
00541 stamp=myRanges.timestamp.getSeconds()+myRanges.timestamp.getUSeconds()/1000000.0;
00542 filename << m_imageDir << "/" << m_imagePrefix << "-" << myIndex << "-laser-" << stamp << ".txt";
00543 outfile.open(filename.str().c_str());
00544 for(rangeiter=myRanges.ranges.begin();
00545 rangeiter!=myRanges.ranges.end();
00546 rangeiter++){
00547 outfile << (*rangeiter) << " ";
00548 }
00549 outfile.close();
00550
00551 thestring.clear();
00552 filename.str(thestring);
00553 }
00554
00555 if(m_doodom){
00556 stamp=myEgo.timestamp.getSeconds()+myEgo.timestamp.getUSeconds()/1000000.0;
00557 filename << m_imageDir << "/" << m_imagePrefix << "-" << myIndex << "-odometry-" << stamp << ".txt";
00558 outfile.open(filename.str().c_str());
00559 outfile << myEgo.location.x() << " " << myEgo.location.y() << " " << myEgo.location.theta() << " " << myEgo.transVelocity << " " << myEgo.angularVelocity << endl;
00560 outfile.close();
00561
00562 thestring.clear();
00563 filename.str(thestring);
00564 }
00565
00566 myNewImage=false;
00567 }
00568 usleep(50000);
00569 }
00570 }
00571
00572
00573 extern "C" {
00574 void *runStereoDataCollectorThread1(void *ptr){
00575 StereoDataCollector *theObject=(StereoDataCollector *)ptr;
00576
00577 theObject->fileWriterThread(theObject->t1time,&theObject->m_t1image,
00578 &theObject->m_t1simage,theObject->ranges1,
00579 theObject->ego1,theObject->index1,
00580 theObject->m_newt1image,
00581 theObject->t1state);
00582
00583 pthread_exit(NULL);
00584 }
00585
00586 void *runStereoDataCollectorThread2(void *ptr){
00587 StereoDataCollector *theObject=(StereoDataCollector *)ptr;
00588 theObject->fileWriterThread(theObject->t2time,&theObject->m_t2image,
00589 &theObject->m_t2simage,theObject->ranges2,
00590 theObject->ego2,theObject->index2,
00591 theObject->m_newt2image,
00592 theObject->t2state);
00593
00594
00595 pthread_exit(NULL);
00596 }
00597
00598 void *runStereoDataCollectorThread3(void *ptr){
00599 StereoDataCollector *theObject=(StereoDataCollector *)ptr;
00600 theObject->fileWriterThread(theObject->t3time,&theObject->m_t3image,
00601 &theObject->m_t3simage,theObject->ranges3,
00602 theObject->ego3,theObject->index3,
00603 theObject->m_newt3image,
00604 theObject->t3state);
00605
00606
00607 pthread_exit(NULL);
00608 }
00609
00610 void *runStereoDataCollectorThread4(void *ptr){
00611 StereoDataCollector *theObject=(StereoDataCollector *)ptr;
00612 theObject->fileWriterThread(theObject->t4time,&theObject->m_t4image,
00613 &theObject->m_t4simage,theObject->ranges4,
00614 theObject->ego4,theObject->index4,
00615 theObject->m_newt4image,
00616 theObject->t4state);
00617
00618
00619 pthread_exit(NULL);
00620 }
00621
00622 void *runStereoDataCollectorThread5(void *ptr){
00623 StereoDataCollector *theObject=(StereoDataCollector *)ptr;
00624 theObject->fileWriterThread(theObject->t5time,&theObject->m_t5image,
00625 &theObject->m_t5simage,theObject->ranges5,
00626 theObject->ego5,theObject->index5,
00627 theObject->m_newt5image,
00628 theObject->t5state);
00629
00630
00631 pthread_exit(NULL);
00632 }
00633
00634
00635 VisionOperator *StereoDataCollector_factory(std::string name) {
00636 return new StereoDataCollector(name);
00637 }
00638
00639 class StereoDataCollectorProxy {
00640 public:
00641 StereoDataCollectorProxy(){
00642 operatorFactory["StereoDataCollector"] = StereoDataCollector_factory;
00643 }
00644 };
00645
00646 StereoDataCollectorProxy p_imagewriter;
00647 }