00001 #include <stdio.h>
00002 #include <math.h>
00003
00004 #include <MCOOP.h>
00005
00006 #include "Shared/Config.h"
00007 #include "Shared/mathutils.h"
00008 #include "Events/EventRouter.h"
00009 #include "cmv_region.h"
00010 #include "Vision.h"
00011 #include "Shared/WorldState.h"
00012 #include "Events/VisionEvent.h"
00013 #include "Shared/get_time.h"
00014 #include "Shared/SystemUtility.h"
00015 #include "VisionSerializer.h"
00016 #include "Wireless/Wireless.h"
00017
00018 Vision *vision=NULL;
00019
00020 using namespace mathutils;
00021 using namespace VisionEventNS;
00022
00023 Vision::Vision()
00024 {
00025 initialize();
00026 setCameraParam();
00027 vser=new VisionSerializer();
00028 }
00029
00030 void Vision::setCameraParam()
00031 {
00032 static OPrimitiveID fbkID = 0;
00033
00034 if(fbkID == 0){
00035 if(OPENR::OpenPrimitive("PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1", &fbkID) != oSUCCESS){
00036 cout << "Open FbkImageSensor failure." << endl;
00037 }
00038 }
00039
00040 longword wb=config->vision.white_balance;
00041 longword gain=config->vision.gain;
00042 longword shutter=config->vision.shutter_speed;
00043
00044
00045 OPrimitiveControl_CameraParam owb(wb);
00046 if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_WHITE_BALANCE, &owb, sizeof(owb), 0, 0) != oSUCCESS){
00047 cout << "CAM_SET_WHITE_BALANCE : Failed!" << endl;
00048 }
00049
00050
00051 OPrimitiveControl_CameraParam ogain(gain);
00052 if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &ogain, sizeof(ogain), 0, 0) != oSUCCESS){
00053 cout << "CAM_SET_GAIN : Failed!" << endl;
00054 }
00055
00056
00057 OPrimitiveControl_CameraParam oshutter(shutter);
00058 if(OPENR::ControlPrimitive(fbkID,oprmreqCAM_SET_SHUTTER_SPEED, &oshutter, sizeof(oshutter), 0, 0) != oSUCCESS){
00059 cout << "CAM_SET_SHUTTER_SPEED : Failed!" << endl;
00060 }
00061 }
00062
00063 void Vision::initialize()
00064 {
00065 int num_y,num_u,num_v;
00066 int size;
00067 num_tmaps=1;
00068 int result;
00069
00070 obj_info=new ObjectInfo;
00071
00072 switch(config->vision.resolution) {
00073 case 1:
00074 width=344;
00075 height=288;
00076 break;
00077 case 3:
00078 width=86;
00079 height=72;
00080 break;
00081 default:
00082 width=172;
00083 height=144;
00084 break;
00085 }
00086
00087 max_width = width;
00088 max_height = height;
00089
00090
00091 num_colors =
00092 CMVision::LoadColorInformation(color,MAX_COLORS,config->vision.colors);
00093 if(num_colors > 0){
00094 printf(" Loaded %d colors.\n",num_colors);
00095 }else{
00096 printf(" ERROR: Could not load color information.\n");
00097 }
00098
00099
00100 size = 1 << (bits_y + bits_u + bits_v);
00101 num_y = 1 << bits_y;
00102 num_u = 1 << bits_u;
00103 num_v = 1 << bits_v;
00104
00105 cur_tmap = 0;
00106 memset(tmap,0,sizeof(tmap));
00107 for(int i=0; i<num_tmaps; i++) {
00108 #ifdef PLATFORM_APERIOS
00109 result = NewRegion(size, reinterpret_cast<void**>(&tmap[i]));
00110 if (result != sSUCCESS)
00111 printf("Unable to allocate tmap buffer %d of size %d\n",i,size);
00112 #else
00113 tmap[i] = new cmap_t[size];
00114 #endif
00115 memset(tmap[i],0,size*sizeof(cmap_t));
00116 }
00117
00118 for(int i=0; i<num_tmaps; i++) {
00119 char tmapfile[256];
00120 if(i==0)
00121 sprintf(tmapfile,"%s",config->vision.thresh);
00122 else
00123 sprintf(tmapfile,"%s%d",config->vision.thresh,i);
00124 if(!CMVision::LoadThresholdFile(tmap[i],num_y,num_u,num_v,tmapfile)) {
00125 printf(" ERROR: Could not load threshold file '%s'.\n",tmapfile);
00126 }
00127 int remapped=CMVision::CheckTMapColors(tmap[i],num_y,num_u,num_v,num_colors,0);
00128 printf("remapped %d colors in threshold file '%s'\n",remapped,tmapfile);
00129 }
00130
00131
00132 max_width = width;
00133 max_height = height;
00134 size = width * height;
00135 max_runs = size / MIN_EXP_RUN_LENGTH;
00136 max_regions = size / MIN_EXP_REGION_SIZE;
00137 size = max_width * max_height;
00138 NewLarge(&cmap,size+1);
00139 if(!cmap) {
00140 cout << "Couldn't allocate cmap\n\xFF" << endl;
00141 *((long *)0xDEADCC44)=1;
00142 }
00143 NewLarge(&rmap,max_runs);
00144 if(!rmap) {
00145 cout << "Couldn't allocate rmap\n\xFF" << endl;
00146 *((long *)0xDEAD7744)=1;
00147 }
00148 NewLarge(&rmap2,max_runs);
00149 if(!rmap2) {
00150 cout << "Couldn't allocate rmap2\n\xFF" << endl;
00151 *((long *)0xDEAD7745)=1;
00152 }
00153 NewLarge(®,max_regions);
00154 if(!reg) {
00155 cout << "Couldn't allocate reg\n" << endl;
00156 *((long *)0xDEAD7E66)=1;
00157 }
00158
00159 body_angle=0.0;
00160 body_height=100.0;
00161 head_angles[0]=head_angles[1]=head_angles[2]=0.0;
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190 frameTimestamp = 0UL;
00191 frame_count=0;
00192 initializeEventSpecs();
00193 }
00194
00195 void Vision::initializeEventSpecs() {
00196 for(int event_idx=0; event_idx<NUM_VEVENTS; event_idx++) {
00197 vevent_spec[event_idx].listeners = 0;
00198 vevent_spec[event_idx].filter = 0;
00199 vevent_spec[event_idx].count = 0;
00200 vevent_spec[event_idx].cx = 0;
00201 vevent_spec[event_idx].cy = 0;
00202 vevent_spec[event_idx].present = false;
00203 }
00204 vevent_spec[RedBallSID].confidence=0.7;
00205 vevent_spec[PinkBallSID].confidence=0.7;
00206 vevent_spec[HandSID].confidence=0.5;
00207 vevent_spec[ThumbsupSID].confidence=0.45;
00208 vevent_spec[ThingSID].confidence=0.5;
00209 vevent_spec[MarkersSID].confidence=1.0001;
00210 }
00211
00212 void Vision::enableEvents(int vevent) {
00213 vevent_spec[vevent].listeners++;
00214 }
00215
00216 void Vision::enableEvents(int vevent, int noise) {
00217 enableEvents(vevent);
00218 setNoiseThreshold(vevent, noise);
00219 }
00220
00221 void Vision::disableEvents(int vevent) {
00222 if (vevent_spec[vevent].listeners>0)
00223 vevent_spec[vevent].listeners--;
00224 }
00225
00226 void Vision::setNoiseThreshold(int vevent, int noise) {
00227 vevent_spec[vevent].filter=noise;
00228 }
00229
00230 bool Vision::generateEvent(int vevent, double conf, int cenX, int cenY) {
00231 float cx, cy;
00232 if (conf>vevent_spec[vevent].confidence) {
00233 cx=(cenX-88.0)/88.0;
00234 cy=(cenY-72.0)/72.0;
00235 vevent_spec[vevent].cx=cx;
00236 vevent_spec[vevent].cy=cy;
00237
00238 if (vevent_spec[vevent].present) {
00239 vevent_spec[vevent].count=0;
00240 createEvent(EventBase::statusETID,vevent,cx,cy);
00241 } else {
00242 vevent_spec[vevent].count++;
00243 if (vevent_spec[vevent].count>vevent_spec[vevent].filter) {
00244 vevent_spec[vevent].count=0;
00245 vevent_spec[vevent].present=true;
00246 createEvent(EventBase::activateETID,vevent,cx,cy);
00247 }
00248 }
00249 return true;
00250 } else {
00251 if (!vevent_spec[vevent].present) {
00252 vevent_spec[vevent].count=0;
00253 } else {
00254 vevent_spec[vevent].count++;
00255 if (vevent_spec[vevent].count>vevent_spec[vevent].filter) {
00256 vevent_spec[vevent].count=0;
00257 vevent_spec[vevent].present=false;
00258 createEvent(EventBase::deactivateETID,vevent,0,0);
00259 } else {
00260 createEvent(EventBase::statusETID,vevent,cx,cy);
00261 }
00262 }
00263 return false;
00264 }
00265 }
00266
00267 bool Vision::close()
00268 {
00269 for(int i=0; i<num_tmaps; i++) {
00270 delete[] tmap[i];
00271 }
00272 delete(cmap);
00273 delete(rmap);
00274 delete(reg);
00275
00276 memset(tmap,0,sizeof(tmap));
00277 cmap = NULL;
00278 rmap = NULL;
00279 reg = NULL;
00280
00281 max_width = 0;
00282 max_height = 0;
00283
00284
00285
00286
00287
00288
00289
00290
00291 return(true);
00292 }
00293
00294 inline double pct_from_mean(double a,double b) {
00295 double s = (a - b) / (a + b);
00296 return fabs(s);
00297 }
00298
00299 int Vision::calcEdgeMask(int x1,int x2,int y1,int y2)
00300 {
00301 static const int boundary_pixel_size=1;
00302
00303 int edge;
00304
00305 edge = 0;
00306 if(x1 <= 0 +boundary_pixel_size) edge |= OFF_EDGE_LEFT ;
00307 if(x2 >= width -1-boundary_pixel_size) edge |= OFF_EDGE_RIGHT ;
00308 if(y1 <= 0 +boundary_pixel_size) edge |= OFF_EDGE_TOP ;
00309 if(y2 >= height-1-boundary_pixel_size) edge |= OFF_EDGE_BOTTOM;
00310
00311 return edge;
00312 }
00313
00314 int Vision::addToHistHorizStrip(int y, int x1, int x2, int *color_cnt)
00315 {
00316 int x;
00317
00318 y = bound(y ,0,height-1);
00319 x1 = bound(x1,0,width -1);
00320 x2 = bound(x2,0,width -1);
00321
00322 for(x=x1; x<=x2; x++) {
00323 color_cnt[getColorUnsafe(x,y)]++;
00324 }
00325
00326 return x2-x1+1;
00327 }
00328
00329 int Vision::addToHistVertStrip(int x, int y1, int y2, int *color_cnt)
00330 {
00331 int y;
00332
00333 x = bound(x ,0,width -1);
00334 y1 = bound(y1,0,height-1);
00335 y2 = bound(y2,0,height-1);
00336
00337 for(y=y1; y<=y2; y++) {
00338 color_cnt[getColorUnsafe(x,y)]++;
00339 }
00340
00341 return y2-y1+1;
00342 }
00343
00344 bool Vision::findHand(VObject *hand, VisionObjectInfo *hand_info)
00345 {
00346 if (vevent_spec[HandSID].listeners==0) return false;
00347
00348 color_class_state *skin;
00349 int n;
00350 double conf;
00351
00352 skin=&color[COLOR_SKIN];
00353
00354 region *or_reg;
00355
00356 or_reg=skin->list;
00357
00358 hand->confidence = 0.0;
00359
00360 if(!or_reg) return false;
00361
00362 n = 0;
00363 while(or_reg && n<10) {
00364 conf = ((double)or_reg->area/500);
00365
00366 if(conf > 1.0) conf = 1.0;
00367
00368 if(conf > hand->confidence) {
00369 hand->confidence = conf;
00370 hand_info->reg = or_reg;
00371 }
00372
00373 or_reg = or_reg->next;
00374 n++;
00375 }
00376
00377 return generateEvent (HandSID, hand->confidence, hand_info->reg->cen_x, hand_info->reg->cen_y);
00378 }
00379
00380 bool Vision::findBall(int ball_color, VObject *ball,VisionObjectInfo *ball_info) {
00381 if ((ball_color==COLOR_RED && vevent_spec[RedBallSID].listeners==0)
00382 ||(ball_color==COLOR_PINK && vevent_spec[PinkBallSID].listeners==0)) return false;
00383
00384 static const bool debug_ball = false;
00385 static const bool debug_conf = false;
00386
00387 static int frame_cnt=0;
00388 static const int print_period=1;
00389
00390 if(debug_ball)
00391 frame_cnt = (frame_cnt + 1) % print_period;
00392
00393 color_class_state *pink;
00394 int n;
00395 int w,h;
00396 double conf;
00397 double green_f;
00398
00399 pink=&color[ball_color];
00400
00401 region *or_reg;
00402
00403 or_reg=pink->list;
00404
00405 ball->confidence = 0.0;
00406
00407 if(!or_reg) return false;
00408
00409 n = 0;
00410 while(or_reg && n<10) {
00411 double conf0,conf_square_bbox,conf_area,conf_green,conf_area_bonus;
00412 double conf_red_v_area;
00413 int edge;
00414
00415 w = or_reg->x2 - or_reg->x1 + 1;
00416 h = or_reg->y2 - or_reg->y1 + 1;
00417
00418 edge = calcEdgeMask(or_reg);
00419 conf0 = (w >= 3) * (h >= 3) * (or_reg->area >= 7);
00420 conf_square_bbox =
00421 edge ?
00422 gaussian_with_min(pct_from_mean(w,h) / .6, 1e-3) :
00423 gaussian_with_min(pct_from_mean(w,h) / .2, 1e-3);
00424 conf_area =
00425 edge ?
00426 gaussian_with_min(pct_from_mean(M_PI*w*h/4.0,or_reg->area) / .6, 1e-3) :
00427 gaussian_with_min(pct_from_mean(M_PI*w*h/4.0,or_reg->area) / .2, 1e-3);
00428 conf_red_v_area = 1.0;
00429 conf_green = 1.0;
00430 conf_area_bonus = ((double)or_reg->area / 1000);
00431
00432 conf =
00433 conf0 *
00434 conf_square_bbox *
00435 conf_area *
00436 conf_red_v_area *
00437 conf_green +
00438 conf_area_bonus;
00439
00440 if(conf > 1.0) conf = 1.0;
00441
00442 if(debug_conf &&
00443 frame_cnt == 0) {
00444 printf("conf0 %g conf_square_bbox %g conf_area %g conf_area_bonus %g final %g\n",
00445 conf0,conf_square_bbox,conf_area,conf_area_bonus,conf);
00446 }
00447
00448
00449 if(conf > ball->confidence) {
00450 if(debug_ball &&
00451 (frame_cnt == 0 ||
00452 frame_cnt == 1)) {
00453
00454
00455
00456
00457 }
00458
00459 int sx[2],sy[2];
00460 bool edge_x[2],edge_y[2];
00461 static const int scan_distance = 3;
00462 static int color_cnts[MAX_COLORS];
00463 int scan_pixels;
00464 int good_value;
00465
00466 sx[0] = or_reg->x1 - scan_distance;
00467 edge_x[0] = (sx[0] < 0);
00468 if(edge_x[0]) sx[0] = 0;
00469 sx[1] = or_reg->x2 + scan_distance;
00470 edge_x[1] = (sx[1] > width-1);
00471 if(edge_x[1]) sx[1] = width-1;
00472 sy[0] = or_reg->y1 - scan_distance;
00473 edge_y[0] = (sy[0] < 0);
00474 if(edge_y[0]) sy[0] = 0;
00475 sy[1] = or_reg->y2 + scan_distance;
00476 edge_y[1] = (sy[1] > height-1);
00477 if(edge_y[1]) sy[1] = height-1;
00478
00479 scan_pixels = 0;
00480 for(int color_idx=0; color_idx<MAX_COLORS; color_idx++)
00481 color_cnts[color_idx]=0;
00482
00483
00484 for(int side=0; side<2; side++) {
00485 if(!edge_y[side]) {
00486 scan_pixels+=addToHistHorizStrip(sy[side],sx[0],sx[1],color_cnts);
00487 }
00488 }
00489
00490
00491 for(int side=0; side<2; side++) {
00492 if(!edge_x[side]) {
00493 scan_pixels+=addToHistVertStrip(sx[side],sy[0],sy[1],color_cnts);
00494 }
00495 }
00496
00497 int non_robot_fringe=0;
00498 non_robot_fringe += 5*color_cnts[COLOR_BLUE ];
00499
00500
00501
00502 non_robot_fringe -= color_cnts[COLOR_GREEN ];
00503
00504 conf_red_v_area = 1 + non_robot_fringe / or_reg->area;
00505 conf_red_v_area = bound(conf_red_v_area,0.0,1.0);
00506
00507 good_value = 0;
00508 good_value += 2*color_cnts[COLOR_BLUE ];
00509 good_value += 3*color_cnts[COLOR_GREEN]/2;
00510
00511
00512
00513
00514 green_f = std::max(good_value+1,1) / (scan_pixels + 1.0);
00515 green_f = bound(green_f,0.0,1.0);
00516 conf_green = green_f;
00517
00518 conf =
00519 conf0 *
00520 conf_square_bbox *
00521 conf_area *
00522 conf_red_v_area *
00523 conf_green +
00524 conf_area_bonus;
00525
00526 if(conf > 1.0) conf = 1.0;
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536 }
00537
00538 if(conf > ball->confidence) {
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572 ball->confidence = conf;
00573
00574
00575
00576
00577
00578
00579
00580 ball_info->reg = or_reg;
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591 }
00592
00593 or_reg = or_reg->next;
00594 n++;
00595 }
00596
00597 return (ball_color==COLOR_RED)?generateEvent (RedBallSID, ball->confidence, ball_info->reg->cen_x, ball_info->reg->cen_y):generateEvent(PinkBallSID,ball->confidence,ball_info->reg->cen_x,ball_info->reg->cen_y);
00598 }
00599
00600 bool Vision::findThing(VObject *thing, VisionObjectInfo *thing_info) {
00601 if (vevent_spec[ThingSID].listeners==0) return false;
00602 thing->confidence=0;
00603 return generateEvent (ThingSID, thing->confidence, thing_info->reg->cen_x, thing_info->reg->cen_y);
00604 }
00605
00606 int Vision::isIn(region *r1, region *r2) {
00607 int r1w=r1->x2-r1->x1;
00608 int r1h=r1->y2-r1->y1;
00609
00610 if ((((r2->x1 < r1->x1) && (r2->x2 > r1->x1+r1w*0.75)) ||
00611 ((r2->x2 > r1->x2) && (r2->x1 < r1->x2-r1w*0.75)))
00612 && (((r2->y1 < r1->y1) && (r2->y2 > r1->y1+r1h*0.75)) ||
00613 ((r2->y2 > r1->y2) && (r2->y1 < r1->y2-r1h*0.75))))
00614 return 1;
00615 return 0;
00616 }
00617
00618 int Vision::isAdjacent(region *r1, region *r2) {
00619
00620 if (r1->x1 < r2->x1)
00621 return r1->x2>(r2->x1-2);
00622 else if (r1->x2 > r2->x2)
00623 return r1->x1<(r2->x2+2);
00624 return 0;
00625 }
00626
00627 int Vision::identifyMarker(int color1, int color2, int color3) {
00628 static const int marker_colors[27]={
00629 -1, -1, -1, MARKER_GOG, -1, MARKER_GOP, MARKER_GPG, MARKER_GPO, -1,
00630 -1, MARKER_OGO, MARKER_OGP, -1, -1, -1, MARKER_OPG, MARKER_OPO, -1,
00631 -1, MARKER_PGO, MARKER_PGP, MARKER_POG, -1, MARKER_POP, -1, -1, -1 };
00632 int markeridx=0;
00633 switch (color1) {
00634 case COLOR_ORANGE:
00635 markeridx+=9;
00636 break;
00637 case COLOR_PURPLE:
00638 markeridx+=18;
00639 break;
00640 }
00641 switch (color2) {
00642 case COLOR_ORANGE:
00643 markeridx+=3;
00644 break;
00645 case COLOR_PURPLE:
00646 markeridx+=6;
00647 break;
00648 }
00649 switch (color3) {
00650 case COLOR_ORANGE:
00651 markeridx+=1;
00652 break;
00653 case COLOR_PURPLE:
00654 markeridx+=2;
00655 break;
00656 }
00657 return marker_colors[markeridx];
00658 }
00659
00660 bool Vision::findMarkers() {
00661 if (vevent_spec[MarkersSID].listeners==0) return false;
00662
00663 const bool debug_markers = false;
00664 static int frame_cnt=0;
00665
00666 static const int print_period=30;
00667
00668 if(debug_markers)
00669 frame_cnt = (frame_cnt + 1) % print_period;
00670
00671 region *or_reg, *gr_reg, *pu_reg;
00672
00673 or_reg = color[COLOR_ORANGE].list;
00674 gr_reg = color[COLOR_BGREEN].list;
00675 pu_reg = color[COLOR_PURPLE].list;
00676
00677 region* marker_regs[9];
00678 int num_regs=0;
00679
00680 for (int i=0; i<9; i++) marker_regs[i]=NULL;
00681
00682 while (num_regs<9) {
00683 int rarea=0;
00684
00685 if (or_reg) {
00686 marker_regs[num_regs]=or_reg;
00687 rarea=or_reg->area;
00688 }
00689 if (gr_reg && gr_reg->area>rarea) {
00690 marker_regs[num_regs]=gr_reg;
00691 rarea=gr_reg->area;
00692 }
00693 if (pu_reg && pu_reg->area>rarea) {
00694 marker_regs[num_regs]=pu_reg;
00695 rarea=pu_reg->area;
00696 }
00697 if (rarea<30) break;
00698
00699 switch(marker_regs[num_regs]->color) {
00700 case COLOR_ORANGE:
00701 or_reg=or_reg->next;
00702 break;
00703 case COLOR_BGREEN:
00704 gr_reg=gr_reg->next;
00705 break;
00706 case COLOR_PURPLE:
00707 pu_reg=pu_reg->next;
00708 break;
00709 }
00710 num_regs++;
00711 }
00712
00713 vis_markers=0;
00714
00715 for (int i=0; i<num_regs/3; i++) {
00716 int j;
00717 for (j=0; j<3; j++) markers[vis_markers].regs[j]=NULL;
00718 for (j=i; j<num_regs; j++)
00719 if (marker_regs[j]) break;
00720 markers[vis_markers].regs[0]=marker_regs[j];
00721 marker_regs[j]=NULL;
00722 for (j++; j<num_regs; j++) {
00723 if (marker_regs[j]) {
00724 if (isAdjacent(markers[vis_markers].regs[0],marker_regs[j])) {
00725 markers[vis_markers].regs[1]=marker_regs[j];
00726 marker_regs[j]=NULL;
00727 break;
00728 }
00729 }
00730 }
00731 if (!markers[vis_markers].regs[1]) continue;
00732 for (j=i+1; j<num_regs; j++) {
00733 if (marker_regs[j]) {
00734 if (isAdjacent(markers[vis_markers].regs[0],marker_regs[j])
00735 && isAdjacent(markers[vis_markers].regs[1],marker_regs[j])) {
00736 markers[vis_markers].regs[2]=markers[vis_markers].regs[1];
00737 markers[vis_markers].regs[1]=marker_regs[j];
00738 marker_regs[j]=NULL;
00739 break;
00740 } else if (isAdjacent(markers[vis_markers].regs[0],marker_regs[j])) {
00741 markers[vis_markers].regs[2]=markers[vis_markers].regs[1];
00742 markers[vis_markers].regs[1]=markers[vis_markers].regs[0];
00743 markers[vis_markers].regs[0]=marker_regs[j];
00744 marker_regs[j]=NULL;
00745 break;
00746 } else if (isAdjacent(markers[vis_markers].regs[1],marker_regs[j])) {
00747 markers[vis_markers].regs[2]=marker_regs[j];
00748 marker_regs[j]=NULL;
00749 break;
00750 }
00751 }
00752 }
00753 if (!markers[vis_markers].regs[2]) continue;
00754 if (markers[vis_markers].regs[0]->cen_x>markers[vis_markers].regs[2]->cen_x) {
00755 region* t_marker=markers[vis_markers].regs[0];
00756 markers[vis_markers].regs[0]=markers[vis_markers].regs[2];
00757 markers[vis_markers].regs[2]=t_marker;
00758 }
00759 markers[vis_markers].cen_x=markers[vis_markers].regs[1]->cen_x;
00760 markers[vis_markers].cen_y=markers[vis_markers].regs[1]->cen_y;
00761 markers[vis_markers].marker=identifyMarker(markers[vis_markers].regs[0]->color, markers[vis_markers].regs[1]->color,markers[vis_markers].regs[2]->color);
00762 if (markers[vis_markers].marker<0) continue;
00763
00764 if (debug_markers && frame_cnt==0)
00765 printf("marker colors: %d%d%d, id: %d\n",markers[vis_markers].regs[0]->color, markers[vis_markers].regs[1]->color, markers[vis_markers].regs[2]->color,markers[vis_markers].marker);
00766 vis_markers++;
00767 }
00768 for (int i=0; i<vis_markers; i++) {
00769 #ifdef PLATFORM_APERIOS
00770 VisionEvent *mevent=new VisionEvent(EventBase::statusETID,MarkersSID,
00771 markers[i].cen_x, markers[i].cen_y);
00772 mevent->setProperty(markers[i].marker);
00773 erouter->postEvent(mevent);
00774 #endif
00775 }
00776 return (vis_markers>0);
00777 }
00778
00779 bool Vision::findGesture(VisionObjectInfo *hand_info) {
00780 if (vevent_spec[ThumbsupSID].listeners==0) return false;
00781 if (hand_info->reg->cen_y>=144) return false;
00782
00783 int thumbsup=0, thumbx=0;
00784 int idx=hand_info->reg->run_start;
00785 int parent=rmap[idx].parent;
00786
00787 int i=0, y=0;
00788
00789 while (y<=hand_info->reg->cen_y) {
00790 int r=0;
00791 int xl=0;
00792 while (rmap[i].y<=y) {
00793 if (r>0) {
00794 if (rmap[i].parent==parent
00795 && rmap[i].width>5
00796 && rmap[i].x > xl+10) {
00797
00798 thumbsup++;
00799 thumbx+=xl+(rmap[i].x-xl)/2;
00800 } else {
00801 xl=rmap[i].x+rmap[i].width;
00802 }
00803 } else if (r==0) {
00804 if (rmap[i].parent==parent
00805 && rmap[i].width>5) {
00806 r=i;
00807
00808 xl=rmap[i].x+rmap[i].width;
00809 }
00810 }
00811 i++;
00812 if (i>=num_runs) break;
00813 }
00814 if (i>=num_runs) break;
00815 y=rmap[i].y;
00816 }
00817
00818 return generateEvent (ThumbsupSID, limitRange(thumbsup/20.00,0.0,1.0), hand_info->reg->cen_x, hand_info->reg->cen_y);
00819 }
00820
00821 void Vision::createEvent(unsigned int tid, unsigned int sid,
00822 float cenX, float cenY) {
00823 #ifdef PLATFORM_APERIOS
00824 erouter->postEvent(new VisionEvent((EventBase::EventTypeID_t)tid,sid,cenX,cenY));
00825 #endif
00826 }
00827
00828 bool Vision::runHighLevelVision(ObjectInfo *obj_info) {
00829 for(int obj_idx=0; obj_idx<NUM_VISION_OBJECTS; obj_idx++) {
00830 vobj_info[obj_idx].reg = NULL;
00831 }
00832
00833 findMarkers();
00834 bool rball_exist=findBall(COLOR_RED,&obj_info->rball,&vobj_info[RBALL]);
00835 bool pball_exist=findBall(COLOR_PINK,&obj_info->pball,&vobj_info[PBALL]);
00836 bool hand_exist=findHand(&obj_info->hand,&vobj_info[HAND]);
00837 if (hand_exist
00838 && !(rball_exist && isIn(vobj_info[RBALL].reg,vobj_info[HAND].reg))
00839 && !(pball_exist && isIn(vobj_info[PBALL].reg,vobj_info[HAND].reg)))
00840
00841 findGesture(&vobj_info[HAND]);
00842 return true;
00843 }
00844
00845 bool Vision::thresholdImage(CMVision::image_idx<rgb> &img) {
00846 static rgb rgb_colors[MAX_COLORS];
00847 static bool initted=false;
00848
00849 if(!initted) {
00850 for(int color_idx=0; color_idx<MAX_COLORS; color_idx++)
00851 rgb_colors[color_idx]=color[color_idx].color;
00852 initted=true;
00853 }
00854
00855 CMVision::RgbToIndex(cmap,img.buf,width,height,rgb_colors,MAX_COLORS);
00856
00857 return true;
00858 }
00859
00860 bool Vision::thresholdImage(CMVision::image_yuv<const uchar> &img) {
00861 CMVision::ThresholdImageYUVPlanar<cmap_t,CMVision::image_yuv<const uchar>,const uchar,bits_y,bits_u,bits_v>(cmap,img,tmap[cur_tmap]);
00862
00863 return true;
00864 }
00865
00866
00867
00868 #ifdef ENABLE_JOIN_NEARBY
00869
00870
00871
00872 #endif
00873
00874
00875
00876 template<class image>
00877 bool Vision::runLowLevelVision(image &img)
00878 {
00879 static int frame_cnt=-1;
00880 frame_cnt++;
00881
00882 #ifdef ENABLE_JOIN_NEARBY
00883 int num_runs_old;
00884 #endif
00885 int max_area;
00886
00887 width = img.width;
00888 height = img.height;
00889
00890 thresholdImage(img);
00891 num_runs = CMVision::EncodeRuns(rmap,cmap,img.width,img.height,max_runs);
00892
00893 #ifdef ENABLE_JOIN_NEARBY
00894 num_runs_old = num_runs;
00895 #endif
00896
00897 #ifdef ENABLE_JOIN_NEARBY
00898 num_runs = CMVision::JoinNearbyRuns(rmap,rmap2,num_runs,width,height);
00899 #ifdef DEBUG_JOIN_PARANOID
00900 {
00901 bool ok=CMVision::CheckRuns(rmap2+1,num_runs-2,width,height);
00902 if(!ok) {
00903 printf("error in run data, consistency check failed\n");
00904 }
00905 }
00906 #endif
00907 #endif
00908
00909 #ifdef ENABLE_JOIN_NEARBY
00910 memcpy(rmap,rmap2,sizeof(*rmap2)*max_runs);
00911 #endif
00912
00913 #ifdef DEBUG_JOIN
00914 {
00915 bool ok=CMVision::CheckRuns(rmap+1,num_runs-2,width,height);
00916 if(!ok) {
00917 printf("error in run data, consistency check failed\n");
00918 }
00919 }
00920 #endif
00921
00922 CMVision::ConnectComponents(rmap,num_runs);
00923
00924 num_regions = CMVision::ExtractRegions(reg,max_regions,rmap,num_runs);
00925
00926
00927
00928
00929
00930 max_area = CMVision::SeparateRegions(color,num_colors,reg,num_regions);
00931 CMVision::SortRegions(color,num_colors,max_area);
00932
00933 CMVision::MergeRegions(color,num_colors,rmap);
00934
00935 for(int i=0; i<num_colors; i++)
00936 color[i].total_area = -1;
00937
00938 #ifdef CHECK_REGIONS
00939 {
00940 bool ok=CMVision::CheckRegions(color,num_colors,rmap);
00941 if(!ok) {
00942 printf("CheckRegions failed\n");
00943 }
00944 }
00945 #endif
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972
00973
00974
00975
00976
00977
00978
00979 return(true);
00980 }
00981
00982 bool Vision::processFrame(const uchar *data_y, const uchar *data_u, const uchar *data_v, int width, int height) {
00983 frameTimestamp = get_time();
00984 frame_count++;
00985
00986 img.buf_y=data_y;
00987 img.buf_u=data_u;
00988 img.buf_v=data_v;
00989
00990 img.width=width;
00991 img.height=height;
00992 img.row_stride=3*width;
00993
00994 bool ok=true;
00995
00996 if(ok) ok=runLowLevelVision(img);
00997 if(ok) ok=runHighLevelVision(obj_info);
00998
00999 vser->serialize();
01000
01001 return ok;
01002 }
01003
01004 int WritePPM(char *filename,rgb *img,int width,int height)
01005 {
01006 FILE *out;
01007 int wrote;
01008
01009 out = fopen(filename,"wb");
01010 if(!out) return(0);
01011
01012 fprintf(out,"P6\n%d %d\n%d\n",width,height,255);
01013 wrote = fwrite(img,sizeof(rgb),width*height,out);
01014 fclose(out);
01015
01016 return(wrote);
01017 }
01018
01019 int Vision::setThreshold(int threshold_id) {
01020 if(threshold_id < 0 || threshold_id >= num_tmaps) {
01021 return -1;
01022 } else {
01023 int old_threshold;
01024 old_threshold = cur_tmap;
01025 cur_tmap = threshold_id;
01026 return old_threshold;
01027 }
01028 }
01029
01030 bool Vision::saveThresholdImage(char *filename)
01031 {
01032 rgb *buf;
01033 int wrote;
01034
01035 buf = new rgb[width * height];
01036 if(!buf) return(false);
01037
01038 CMVision::IndexToRgb<cmap_t,color_class_state>(buf,cmap,width,height,color,num_colors);
01039 wrote = WritePPM(filename,buf,width,height);
01040 delete(buf);
01041
01042 return(wrote > 0);
01043 }
01044
01045
01046
01047
01048
01049
01050
01051
01052
01053
01054
01055
01056
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066
01067
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081
01082
01083
01084
01085
01086
01087
01088
01089
01090
01091
01092
01093
01094
01095
01096
01097
01098
01099
01100
01101
01102
01103
01104
01105
01106
01107
01108
01109
01110
01111
01112
01113
01114
01115
01116
01117
01118
01119
01120
01121 namespace VisionInterface {
01122
01123
01124
01125
01126
01127
01128
01129
01130 void WriteThresholdImage(Vision *vision, char *filename) {
01131 vision->saveThresholdImage(filename);
01132 }
01133
01134 int SetThreshold(Vision *vision,int threshold_id) {
01135 return vision->setThreshold(threshold_id);
01136 }
01137 }
01138
01139
01140
01141
01142
01143
01144
01145
01146
01147
01148
01149
01150
01151