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;
00012 typedef SegmentedColorGenerator::color_class_state color_class_state;
00013
00014 BallDetectionGenerator::BallDetectionGenerator(unsigned int mysid, const RegionGenerator * rg, unsigned int colorIdx, unsigned int threshmapChan, unsigned int noiseFiltering, float confidence)
00015 : EventGeneratorBase("BallDetectionGenerator","BallDetectionGenerator",EventBase::visObjEGID,mysid,rg->getGeneratorID(),rg->getSourceID(),EventBase::statusETID), 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 if(segev->getNumColors()<=clrIdx) {
00042 serr->printf("BallDetectionGenerator::clrIdx %d is invalid (only %d available)\n",clrIdx,segev->getNumColors());
00043 return;
00044 }
00045 if(segev->getNumChannels()<=tmIdx) {
00046 serr->printf("BallDetectionGenerator::tmIdx %d is invalid (only %d available)\n",tmIdx,segev->getNumChannels());
00047 return;
00048 }
00049 if(segev->getNumLayers()<=layer) {
00050 serr->printf("BallDetectionGenerator's selected layer %d is invalid (only %d available)\n",layer,segev->getNumLayers());
00051 return;
00052 }
00053 if(segev->getImage(layer,tmIdx)==NULL) {
00054 serr->printf("BallDetectionGenerator source's getImage returned NULL!\n");
00055 return;
00056 }
00057 const color_class_state& ballCCS=reinterpret_cast<const color_class_state*>(segev->getImage(layer,tmIdx))[clrIdx];
00058
00059 ball.confidence = 0;
00060 region * ball_region=NULL;
00061
00062 region * or_reg=ballCCS.list;
00063 if(!or_reg){
00064 count++;
00065
00066 float dim=segev->getWidth(layer)>segev->getHeight(layer) ? segev->getWidth(layer) : segev->getHeight(layer);
00067
00068 float cw=segev->getWidth(layer)/dim;
00069 float ch=segev->getHeight(layer)/dim;
00070 unsigned int frame_number=segev->getFrameNumber();
00071 if (present && count>noiseThreshold) {
00072 count=0;
00073 present=false;
00074 createEvent(EventBase::deactivateETID,0,0,0,0,0,cw,ch,frame_number);
00075 }
00076 return;
00077 }
00078
00079 unsigned int n = 0;
00080 while(or_reg && n<NUM_CHECK) {
00081
00082
00083
00084
00085 int w = or_reg->x2 - or_reg->x1 + 1;
00086 int h = or_reg->y2 - or_reg->y1 + 1;
00087
00088 int edge = calcEdgeMask(or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2,segev->getWidth(layer),segev->getHeight(layer));
00089 float conf0 = (w >= 3) * (h >= 3) * (or_reg->area >= 7);
00090 float conf_square_bbox =
00091 edge ?
00092 gaussian_with_min(pct_from_mean(w,h) / .6f, 1e-3) :
00093 gaussian_with_min(pct_from_mean(w,h) / .2f, 1e-3);
00094 float conf_area =
00095 edge ?
00096 gaussian_with_min(pct_from_mean(((float)M_PI)*w*h/4.0f,or_reg->area) / .6f, 1e-3) :
00097 gaussian_with_min(pct_from_mean(((float)M_PI)*w*h/4.0f,or_reg->area) / .2f, 1e-3);
00098 float conf_area_bonus = or_reg->area / 1000.0f;
00099
00100 float conf = conf0*conf_square_bbox*conf_area + conf_area_bonus;
00101
00102 if(conf > 1.0) conf = 1.0f;
00103
00104 if(debug_conf && frame_cnt == 0) {
00105 printf("conf0 %g conf_square_bbox %g conf_area %g conf_area_bonus %g final %g\n",
00106 conf0,conf_square_bbox,conf_area,conf_area_bonus,conf);
00107 }
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183 if(conf > ball.confidence) {
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218 ball.confidence = conf;
00219
00220
00221
00222
00223
00224
00225
00226 ball_region = or_reg;
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237 }
00238
00239 or_reg = or_reg->next;
00240 n++;
00241 }
00242
00243
00244 testSendEvent(*segev,ball.confidence,(int)ball_region->x1,(int)ball_region->x2,(int)ball_region->y1,(int)ball_region->y2,(int)ball_region->area);
00245 }
00246
00247 void
00248 BallDetectionGenerator::testSendEvent(const FilterBankEvent& ev, float conf, int regX1,int regX2,int regY1,int regY2, int area) {
00249 unsigned int layer=ev.getNumLayers()-config->vision.resolution-1;
00250
00251
00252 float dim=ev.getWidth(layer)>ev.getHeight(layer) ? ev.getWidth(layer) : ev.getHeight(layer);
00253
00254
00255 float cw=config->vision.x_range;
00256 float ch=config->vision.y_range;
00257
00258
00259 float cx1=2.0f*regX1/dim - cw;
00260 float cx2=2.0f*(regX2+1)/dim - cw;
00261 float cy1=2.0f*regY1/dim - ch;
00262 float cy2=2.0f*(regY2+1)/dim - ch;
00263 unsigned int frame_number=ev.getFrameNumber();
00264
00265 if (conf>confidenceThreshold) {
00266 if (present) {
00267 count=0;
00268 createEvent(EventBase::statusETID,cx1,cx2,cy1,cy2,area,cw,ch,frame_number);
00269 } else {
00270 count++;
00271 if (count>noiseThreshold) {
00272 count=0;
00273 present=true;
00274 createEvent(EventBase::activateETID,cx1,cx2,cy1,cy2,area,cw,ch,frame_number);
00275 }
00276 }
00277 } else {
00278 if (!present) {
00279 count=0;
00280 } else {
00281 count++;
00282 if (count>noiseThreshold) {
00283 count=0;
00284 present=false;
00285 createEvent(EventBase::deactivateETID,0,0,0,0,area,cw,ch,frame_number);
00286 } else {
00287 createEvent(EventBase::statusETID,cx1,cx2,cy1,cy2,area,cw,ch,frame_number);
00288 }
00289 }
00290 }
00291 }
00292
00293 void
00294 BallDetectionGenerator::createEvent(EventBase::EventTypeID_t etid, float bbX1,float bbX2,float bbY1,float bbY2,float area,float rx,float ry,unsigned int frame) const {
00295 VisionObjectEvent * vo=new VisionObjectEvent(mySourceID,etid,bbX1,bbX2,bbY1,bbY2,area,rx,ry,frame);
00296 vo->setName(getName());
00297 erouter->postEvent(vo);
00298 }
00299
00300 int
00301 BallDetectionGenerator::calcEdgeMask(int x1,int x2,int y1,int y2, int width, int height) {
00302 static const int boundary_pixel_size=1;
00303
00304 int edge = 0;
00305 if(x1 <= 0 +boundary_pixel_size) edge |= OFF_EDGE_LEFT ;
00306 if(x2 >= width -1-boundary_pixel_size) edge |= OFF_EDGE_RIGHT ;
00307 if(y1 <= 0 +boundary_pixel_size) edge |= OFF_EDGE_TOP ;
00308 if(y2 >= height-1-boundary_pixel_size) edge |= OFF_EDGE_BOTTOM;
00309
00310 return edge;
00311 }
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331