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(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
00022 if(!erouter->hasListeners(getGeneratorID(),getSourceID()))
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;
00047 if(!or_reg) return;
00048 unsigned int n = 0;
00049 while(or_reg && n<NUM_CHECK) {
00050
00051
00052
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
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
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 if(conf > ball.confidence) {
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
00184
00185
00186
00187 ball.confidence = conf;
00188
00189
00190
00191
00192
00193
00194
00195 ball_region = or_reg;
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206 }
00207
00208 or_reg = or_reg->next;
00209 n++;
00210 }
00211
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
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283