Homepage Demos Overview Downloads Tutorials Reference
Credits

BallDetectionGenerator.cc

Go to the documentation of this file.
00001 #include "BallDetectionGenerator.h"
00002 #include "Events/EventRouter.h"
00003 #include "Shared/Util.h"
00004 #include "Shared/WorldState.h"
00005 #include "Wireless/Wireless.h"
00006 #include "Shared/Config.h"
00007 #include "Events/VisionObjectEvent.h"
00008 
00009 #include "Vision/SegmentedColorGenerator.h"
00010 #include "Vision/RegionGenerator.h"
00011 typedef RegionGenerator::region region; //!< shorthand - using CMVision regions
00012 typedef SegmentedColorGenerator::color_class_state color_class_state; //!< shorthand - using CMVision color structs
00013 
00014 BallDetectionGenerator::BallDetectionGenerator(EventBase::EventGeneratorID_t gid, unsigned int sid, unsigned int mysid, unsigned int colorIdx, unsigned int threshmapChan, unsigned int noiseFiltering, float confidence)
00015   : EventGeneratorBase("BallDetectionGenerator",EventBase::visObjEGID,mysid,gid,sid), clrIdx(colorIdx), tmIdx(threshmapChan), ball(), present(false), count(0), noiseThreshold(noiseFiltering), confidenceThreshold(confidence)
00016 {}
00017 
00018 void
00019 BallDetectionGenerator::processEvent(const EventBase& e) {
00020   PROFSECTION("BallDetection::processEvent()",state->mainProfile);
00021   EventGeneratorBase::processEvent(e);
00022   if(e.getGeneratorID()!=getListenGeneratorID() || e.getSourceID()!=getListenSourceID())
00023     return;
00024 
00025   const SegmentedColorFilterBankEvent * segev=dynamic_cast<const SegmentedColorFilterBankEvent*>(&e);
00026   if(NULL==segev) {
00027     serr->printf("BallDetectionGenerator's event %s was not a SegmentedColorFilterBankEvent",e.getName().c_str());
00028     return;
00029   }
00030   
00031   static const bool debug_ball = false;
00032   static const bool debug_conf = false;
00033 
00034   static int frame_cnt=0;
00035   static const int print_period=1;
00036 
00037   if(debug_ball)
00038     frame_cnt = (frame_cnt + 1) % print_period;
00039 
00040   unsigned int layer=segev->getNumLayers()-config->vision.resolution-1;
00041   const color_class_state& ballCCS=reinterpret_cast<const color_class_state*>(segev->getImage(layer,tmIdx))[clrIdx];
00042 
00043   ball.confidence = 0;
00044   region * ball_region=NULL;
00045 
00046   region * or_reg=ballCCS.list; //head of linked list of regions of target color
00047   if(!or_reg) return;
00048   unsigned int n = 0;
00049   while(or_reg && n<NUM_CHECK) {
00050     //float conf,conf0,conf_square_bbox,conf_area,conf_green,conf_area_bonus;
00051     //float conf_red_v_area;
00052     //int edge;
00053 
00054     int w = or_reg->x2 - or_reg->x1 + 1;
00055     int h = or_reg->y2 - or_reg->y1 + 1;
00056     
00057     int edge = calcEdgeMask(or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2,segev->getWidth(layer),segev->getHeight(layer));
00058     float conf0 = (w >= 3) * (h >= 3) * (or_reg->area >= 7);
00059     float conf_square_bbox = 
00060       edge ?
00061       gaussian_with_min(pct_from_mean(w,h) / .6f, 1e-3) :
00062       gaussian_with_min(pct_from_mean(w,h) / .2f, 1e-3);
00063     float conf_area =
00064       edge ?
00065       gaussian_with_min(pct_from_mean(((float)M_PI)*w*h/4.0f,or_reg->area) / .6f, 1e-3) :
00066       gaussian_with_min(pct_from_mean(((float)M_PI)*w*h/4.0f,or_reg->area) / .2f, 1e-3);
00067     float conf_area_bonus = or_reg->area / 1000.0f;
00068 
00069     float conf = conf0*conf_square_bbox*conf_area + conf_area_bonus;
00070 
00071     if(conf > 1.0) conf = 1.0f;
00072     
00073     if(debug_conf && frame_cnt == 0) {
00074       printf("conf0 %g conf_square_bbox %g conf_area %g conf_area_bonus %g final %g\n",
00075              conf0,conf_square_bbox,conf_area,conf_area_bonus,conf);
00076     }
00077     
00078     /*
00079       float green_f;
00080       if(conf > ball->confidence) {
00081       if(debug_ball &&
00082       (frame_cnt == 0 ||
00083       frame_cnt == 1)) {
00084       printf("%s ball n%d cen (%f,%f) area %d bbox (%d,%d)-(%d,%d) conf %f\n", (ball_color==COLOR_PINK)?"Pink":"Orange",
00085       n,or_reg->cen_x,or_reg->cen_y,or_reg->area,
00086       or_reg->x1,or_reg->y1,or_reg->x2,or_reg->y2,
00087       conf);
00088       }
00089 
00090       int sx[2],sy[2]; // scan bounding coordinates;
00091       bool edge_x[2],edge_y[2]; // true if scan coordinate went off edge
00092       static const int scan_distance = 3;
00093       static int color_cnts[num_colors];
00094       int scan_pixels;
00095       int good_value;
00096     
00097       sx[0] = or_reg->x1 - scan_distance;
00098       edge_x[0] = (sx[0] < 0);
00099       if(edge_x[0]) sx[0] = 0;
00100       sx[1] = or_reg->x2 + scan_distance;
00101       edge_x[1] = (sx[1] > width-1);
00102       if(edge_x[1]) sx[1] = width-1;
00103       sy[0] = or_reg->y1 - scan_distance;
00104       edge_y[0] = (sy[0] < 0);
00105       if(edge_y[0]) sy[0] = 0;
00106       sy[1] = or_reg->y2 + scan_distance;
00107       edge_y[1] = (sy[1] > height-1);
00108       if(edge_y[1]) sy[1] = height-1;
00109 
00110       scan_pixels = 0;
00111       for(int color_idx=0; color_idx<MAX_COLORS; color_idx++)
00112       color_cnts[color_idx]=0;
00113     
00114       // do horizontal strips
00115       for(int side=0; side<2; side++) {
00116       if(!edge_y[side]) {
00117       scan_pixels+=addToHistHorizStrip(sy[side],sx[0],sx[1],color_cnts);
00118       }
00119       }
00120     
00121       // do vertical strips
00122       for(int side=0; side<2; side++) {
00123       if(!edge_x[side]) {
00124       scan_pixels+=addToHistVertStrip(sx[side],sy[0],sy[1],color_cnts);
00125       }
00126       }
00127 
00128       int non_robot_fringe=0;
00129       non_robot_fringe += 5*color_cnts[getColor("blue")];
00130       non_robot_fringe -=   color_cnts[getColor("green")];
00131 
00132       conf_red_v_area = 1 + non_robot_fringe / or_reg->area;
00133       conf_red_v_area = bound(conf_red_v_area,0.0,1.0);
00134       
00135       good_value = 0;
00136       good_value += 2*color_cnts[getColor("blue")];
00137       good_value += 3*color_cnts[getColor("green")]/2;
00138 
00139       green_f = std::max(good_value+1,1) / (scan_pixels + 1.0);
00140       green_f = bound(green_f,0.0,1.0);
00141       conf_green = green_f;
00142     
00143       conf = 
00144       conf0 *
00145       conf_square_bbox *
00146       conf_area *
00147       + conf_area_bonus;
00148     
00149       if(conf > 1.0) conf = 1.0;
00150       }*/
00151     
00152     if(conf > ball.confidence) {
00153       /*
00154         d = sqrt((FocalDist * FocalDist * YPixelSize) * (M_PI * BallRadius * BallRadius) /
00155         (or_reg->area));
00156     
00157         vector3d ball_dir; // direction of ball from camera in robot coordinates
00158         ball_dir = getPixelDirection(or_reg->cen_x,or_reg->cen_y);
00159     
00160         // Reject if ball above level plane
00161         //      if(atan2(ball_dir.z,hypot(ball_dir.x,ball_dir.y)) <= (5*(M_PI/180))) {
00162         ball->edge = calcEdgeMask(or_reg);
00163         
00164         vector3d intersect_ball_loc,pixel_size_ball_loc;
00165 
00166         bool intersects=false;
00167         vector3d intersection_pt(0.0,0.0,0.0);
00168         intersects=GVector::intersect_ray_plane(camera_loc,ball_dir,
00169         vector3d(0.0,0.0,BallRadius),vector3d(0.0,0.0,1.0),
00170         intersection_pt);
00171         if(intersects) {
00172         intersect_ball_loc = intersection_pt;
00173         }
00174 
00175         pixel_size_ball_loc = camera_loc + ball_dir * d;
00176 
00177         vector3d ball_loc,alt_ball_loc;
00178         if(ball->edge!=0 && intersects) {
00179         ball_loc     = intersect_ball_loc;
00180         alt_ball_loc = pixel_size_ball_loc;
00181         } else {
00182         alt_ball_loc = intersect_ball_loc;
00183         ball_loc     = pixel_size_ball_loc;
00184         }
00185       */ 
00186 
00187       ball.confidence = conf;
00188         
00189       //        ball->loc = ball_loc;
00190         
00191       //        ball->distance = hypot(ball_loc.x,ball_loc.y);
00192         
00193       //        findSpan(ball->left,ball->right,or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2);
00194 
00195       ball_region = or_reg;
00196 
00197       /*        if(debug_ball &&
00198                 frame_cnt==0) {
00199                 printf("###found ball, conf %g loc (%g,%g,%g) alt (%g,%g,%g) dist %g left %g right %g edge %d\n",
00200                 ball->confidence,
00201                 ball->loc.x,ball->loc.y,ball->loc.z,
00202                 alt_ball_loc.x,alt_ball_loc.y,alt_ball_loc.z,
00203                 ball->distance,ball->left,ball->right,ball->edge);
00204                 }*/
00205       // }
00206     }
00207 
00208     or_reg = or_reg->next;
00209     n++;
00210   }
00211   //return (ball_color==getColor("red"))?generateEvent (VisionObjectEvent::RedBallSID, ball->confidence, ball_region->cen_x, ball_region->cen_y):generateEvent(VisionObjectEvent::PinkBallSID,ball->confidence,ball_region->cen_x,ball_region->cen_y);
00212   testSendEvent(*segev,ball.confidence,(int)ball_region->cen_x,(int)ball_region->cen_y);
00213 }
00214 
00215 void
00216 BallDetectionGenerator::testSendEvent(const FilterBankEvent& ev, float conf, int regcenX, int regcenY) {
00217   unsigned int layer=ev.getNumLayers()-config->vision.resolution-1;
00218   float cx=2.0f*regcenX/ev.getWidth(layer)-1;
00219   float cy=2.0f*regcenY/ev.getHeight(layer)-1;
00220   if (conf>confidenceThreshold) {
00221     if (present) {
00222       count=0;
00223       createEvent(EventBase::statusETID,cx,cy);
00224     } else {
00225       count++;
00226       if (count>noiseThreshold) {
00227         count=0;
00228         present=true;
00229         createEvent(EventBase::activateETID,cx,cy);
00230       }
00231     }
00232   } else {
00233     if (!present) {
00234       count=0;
00235     } else {
00236       count++;
00237       if (count>noiseThreshold) {
00238         count=0;
00239         present=false;
00240         createEvent(EventBase::deactivateETID,0,0);
00241       } else {
00242         createEvent(EventBase::statusETID,cx,cy);
00243       }
00244     }
00245   }
00246 }
00247 
00248 void
00249 BallDetectionGenerator::createEvent(EventBase::EventTypeID_t etid, float centerX, float centerY) const {
00250   erouter->postEvent(new VisionObjectEvent(etid,mySourceID,centerX,centerY));
00251 }
00252 
00253 int
00254 BallDetectionGenerator::calcEdgeMask(int x1,int x2,int y1,int y2, int width, int height) {
00255   static const int boundary_pixel_size=1;
00256 
00257   int edge = 0;
00258   if(x1 <= 0       +boundary_pixel_size) edge |= OFF_EDGE_LEFT  ;
00259   if(x2 >= width -1-boundary_pixel_size) edge |= OFF_EDGE_RIGHT ;
00260   if(y1 <= 0       +boundary_pixel_size) edge |= OFF_EDGE_TOP   ;
00261   if(y2 >= height-1-boundary_pixel_size) edge |= OFF_EDGE_BOTTOM;
00262 
00263   return edge;
00264 }
00265 
00266 /*! @file 
00267  * @brief Implements BallDetectionGenerator, which uses segmented color region information to detect round objects
00268  * @author alokl (Creator)
00269  * @author ejt (reorganized)
00270  *
00271  * History is old, may have grown from CMPack (CMU Robosoccer) roots?
00272  * I think if there's any of their code left, it's probably *mostly*
00273  * the commented out stuff I (ejt) left for posterity when
00274  * reorganizing.  But alokl didn't flag this as CMPack's prior to
00275  * inital release, and they didn't request credit for it when they
00276  * reviewed the code, so I guess it's all ours...
00277  *
00278  * $Author: ejt $
00279  * $Name: tekkotsu-2_2_1 $
00280  * $Revision: 1.6 $
00281  * $State: Exp $
00282  * $Date: 2004/11/09 20:01:49 $
00283  */

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:37 2004 by Doxygen 1.3.9.1