Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

Vision.cc

Go to the documentation of this file.
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   /* White Balance */
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   /* Gain */
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   /* Shutter Speed */
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   // Load color information
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   // Set up threshold
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   // Allocate map structures
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); // +1 to store EncodeRuns terminator value
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(&reg,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   // initialize output stuff
00164   encodeBuf = NULL;
00165   encoder = NULL;
00166   // Always init
00167   //if(config.spoutConfig.dumpVisionRLE!=0) {
00168   //if(config.spoutConfig.dumpVisionRLE) {
00169   // body position (5 floats) + image size*3 chars(value, x, length) + stop chars
00170   // body position (5 floats) + image size*3 chars(y, u, v) + stop chars
00171 #ifdef PLATFORM_APERIOS
00172   NewRegion(5*4+176*144*3+2,(void **)&encodeBuf);
00173   if(encodeBuf==NULL)
00174     printf("Null vision rle encode buf\n");
00175   else
00176     encoder=new SPOutVisionEncoder;
00177   //}
00178 #endif
00179 
00180   visionColorAreaStream = NULL;
00181   visionRawStream       = NULL;
00182   visionRLEStream       = NULL;
00183   visionObjStream       = NULL;
00184 
00185   outCountRaw = 0;
00186   outCountRLE = 0;
00187 
00188   //printf("Done initializing vision.\n");
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; // disable events
00198     vevent_spec[event_idx].filter = 0; // disable noise removal
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;  // disabled
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 #ifdef PLATFORM_APERIOS
00285   DeleteRegion(encodeBuf); // don't care about error
00286 #else
00287   delete[] encodeBuf;
00288 #endif
00289   delete encoder;*/
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; // hand region(s)
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; // pink region
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 /*        printf("%s ball n%d cen (%f,%f) area %d bbox (%d,%d)-(%d,%d) conf %f\n", (ball_color==COLOR_PINK)?"Pink":"Orange",
00454                 n,or_reg->cen_x,or_reg->cen_y,or_reg->area,
00455                 or_reg->x1,or_reg->y1,or_reg->x2,or_reg->y2,
00456                 conf);*/
00457       }
00458 
00459       int sx[2],sy[2]; // scan bounding coordinates;
00460       bool edge_x[2],edge_y[2]; // true if scan coordinate went off edge
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       // do horizontal strips
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       // do vertical strips
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 /*      non_robot_fringe += 3*color_cnts[COLOR_GRAY ];
00500       non_robot_fringe -= 2*color_cnts[COLOR_YELLOW];
00501       non_robot_fringe += 3*color_cnts[COLOR_BLUE  ];*/
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 /*      good_value += 2*color_cnts[COLOR_GRAY ];
00511       good_value -=   color_cnts[COLOR_YELLOW]/2;
00512       good_value += 2*color_cnts[COLOR_BLUE  ];*/
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       if(debug_ball &&
00529          frame_cnt == 0) {
00530         printf(" conf red_v_area %f conf' %f good_value %d g %d w %d r %d b %d y %d scan_pixels %d green_f %f\n",
00531                 conf_red_v_area,conf,good_value,
00532                 color_cnts[COLOR_GREEN],color_cnts[COLOR_WHITE],color_cnts[COLOR_RED],color_cnts[COLOR_BLUE],color_cnts[COLOR_YELLOW],
00533                 scan_pixels,green_f);
00534       }
00535 */
00536     }
00537     
00538     if(conf > ball->confidence) {
00539       /*
00540       d = sqrt((FocalDist * FocalDist * YPixelSize) * (M_PI * BallRadius * BallRadius) /
00541                (or_reg->area));
00542     
00543       vector3d ball_dir; // direction of ball from camera in robot coordinates
00544       ball_dir = getPixelDirection(or_reg->cen_x,or_reg->cen_y);
00545     
00546       // Reject if ball above level plane
00547 //      if(atan2(ball_dir.z,hypot(ball_dir.x,ball_dir.y)) <= (5*(M_PI/180))) {
00548         ball->edge = calcEdgeMask(or_reg);
00549         
00550         vector3d intersect_ball_loc,pixel_size_ball_loc;
00551 
00552         bool intersects=false;
00553         vector3d intersection_pt(0.0,0.0,0.0);
00554         intersects=GVector::intersect_ray_plane(camera_loc,ball_dir,
00555                                                 vector3d(0.0,0.0,BallRadius),vector3d(0.0,0.0,1.0),
00556                                                 intersection_pt);
00557         if(intersects) {
00558           intersect_ball_loc = intersection_pt;
00559         }
00560 
00561         pixel_size_ball_loc = camera_loc + ball_dir * d;
00562 
00563         vector3d ball_loc,alt_ball_loc;
00564         if(ball->edge!=0 && intersects) {
00565           ball_loc     = intersect_ball_loc;
00566           alt_ball_loc = pixel_size_ball_loc;
00567         } else {
00568           alt_ball_loc = intersect_ball_loc;
00569           ball_loc     = pixel_size_ball_loc;
00570         }*/ 
00571 
00572         ball->confidence = conf;
00573         
00574 //        ball->loc = ball_loc;
00575         
00576 //        ball->distance = hypot(ball_loc.x,ball_loc.y);
00577         
00578 //        findSpan(ball->left,ball->right,or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2);
00579 
00580         ball_info->reg = or_reg;
00581 
00582 /*        if(debug_ball &&
00583            frame_cnt==0) {
00584           printf("###found ball, conf %g loc (%g,%g,%g) alt (%g,%g,%g) dist %g left %g right %g edge %d\n",
00585                   ball->confidence,
00586                   ball->loc.x,ball->loc.y,ball->loc.z,
00587                   alt_ball_loc.x,alt_ball_loc.y,alt_ball_loc.z,
00588                   ball->distance,ball->left,ball->right,ball->edge);
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   // bounding boxes <= two pixels apart
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 //          rmap[i].color=COLOR_RED;
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 //          rmap[i].color=COLOR_ORANGE;
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       // hand exists and hand is not holding ball
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 //#define ENABLE_JOIN_NEARBY
00867 
00868 #ifdef ENABLE_JOIN_NEARBY
00869 
00870 //#define DEBUG_JOIN
00871 
00872 #endif /* ENABLE_JOIN_NEARBY */
00873 
00874 //#define CHECK_REGIONS
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   //printf("runs:%6d (%6d) regions:%6d (%6d)\n",
00927   //        num_runs,max_runs,
00928   //        num_regions,max_regions);
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 //   CMVision::CreateRunIndex(yindex,rmap,num_runs);
00948 /*
00949 #ifdef PLATFORM_APERIOS
00950   // output color area if desired
00951   if(config.spoutConfig.dumpVisionColorArea!=0) {
00952     outCountColorArea = (outCountColorArea + 1) % config.spoutConfig.dumpVisionColorArea;
00953     if(outCountColorArea==0) {
00954       sendColorArea();
00955     }
00956   }
00957 #endif
00958 
00959 #ifdef PLATFORM_APERIOS
00960   // output raw frames if desired
00961   if(config.spoutConfig.dumpVisionRaw!=0) {
00962     outCountRaw = (outCountRaw + 1) % config.spoutConfig.dumpVisionRaw;
00963     if(outCountRaw==0) {
00964       sendRawImage();
00965     }
00966   }
00967 #endif
00968 
00969 #ifdef PLATFORM_APERIOS
00970   // output RLE if desired
00971   if(config.spoutConfig.dumpVisionRLE!=0) {
00972     outCountRLE = (outCountRLE + 1) % config.spoutConfig.dumpVisionRLE;
00973     if(outCountRLE==0) {
00974       sendRLEImage();
00975     }
00976   }
00977 #endif
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 void Vision::sendRawImage() {
01046 #ifdef PLATFORM_APERIOS
01047   if(encodeBuf!=NULL && 
01048      encoder  !=NULL && 
01049      visionRawStream!=NULL) {
01050     int out_size=0;
01051     out_size=encoder->encodeVisionRaw(encodeBuf,
01052                                       body_angle,body_height,
01053                                       head_angles[0],head_angles[1],head_angles[2],
01054                                       img);
01055     if(!visionRawStream->writeBinary(encodeBuf,out_size,frameTimestamp))
01056       printf("problem writing raw image\n");
01057   } else {
01058     if(encoder==NULL)
01059       printf("NVEnc\n");
01060     else if(encodeBuf==NULL)
01061       printf("NVEncBuf\n");
01062     else
01063       printf("NVRawStream\n");
01064   }
01065 #endif
01066 
01067   outCountRaw = 0;
01068 }
01069 
01070 void Vision::sendRLEImage() {
01071 #ifdef PLATFORM_APERIOS
01072   if(encodeBuf!=NULL && 
01073      encoder  !=NULL && 
01074      visionRLEStream!=NULL) {
01075     int out_size=0;
01076 #ifdef ENABLE_JOIN_NEARBY
01077     out_size=encoder->encodeVisionRLE(encodeBuf,
01078                                       body_angle,body_height,
01079                                       head_angles[0],head_angles[1],head_angles[2],
01080                                       num_runs-2,rmap+1);
01081 #else
01082     out_size=encoder->encodeVisionRLE(encodeBuf,
01083                                       body_angle,body_height,
01084                                       head_angles[0],head_angles[1],head_angles[2],
01085                                       num_runs,rmap);
01086 #endif
01087     visionRLEStream->writeBinary(encodeBuf,out_size,frameTimestamp);
01088   } else {
01089     if(encoder==NULL)
01090       printf("NVEnc\n");
01091     else if(encodeBuf==NULL)
01092       printf("NVEncBuf\n");
01093     else
01094       printf("NVRLEStream\n");
01095   }
01096 #endif
01097 
01098   outCountRLE = 0;
01099 }
01100 
01101 void Vision::sendColorArea() {
01102   if(encodeBuf!=NULL && 
01103      visionColorAreaStream!=NULL) {
01104     int out_size=0;
01105     uchar *buf;
01106     buf = encodeBuf;
01107     SPOutEncoder::encodeAs<uchar>(&buf,(uchar)num_colors);
01108     for(int color_idx=0; color_idx<num_colors; color_idx++) {
01109       SPOutEncoder::encodeAs<ulong>(&buf,(ulong)color[color_idx].total_area);
01110     }
01111     out_size = buf - encodeBuf;
01112     visionColorAreaStream->writeBinary(encodeBuf,out_size,frameTimestamp);
01113   } else {
01114     if(encodeBuf==NULL)
01115       printf("NVEncBuf\n");
01116     else
01117       printf("NVColorAreaStream\n");
01118   }
01119 }*/
01120 
01121 namespace VisionInterface {
01122 /*  void SendRawImage(Vision *vision) {
01123     vision->sendRawImage();
01124   }
01125 
01126   void SendRLEImage(Vision *vision) {
01127     vision->sendRLEImage();
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 /*! @file
01140  * @brief Does majority of vision processing
01141  * @author CMU RoboSoccer 2001-2002 (Creator)
01142  * @author alokl (ported)
01143  * 
01144  * @verbinclude CMPack_license.txt
01145  *
01146  * $Author: ejt $
01147  * $Name: tekkotsu-1_4_1 $
01148  * $Revision: 1.11 $
01149  * $State: Exp $
01150  * $Date: 2003/06/12 23:41:41 $
01151  */

Tekkotsu v1.4
Generated Sat Jul 19 00:06:32 2003 by Doxygen 1.3.2