Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

BallDetectionGenerator.cc Go to the documentation of this file. #include "BallDetectionGenerator.h" #include "Events/EventRouter.h" #include "Shared/Util.h" #include "Shared/WorldState.h" #include "Wireless/Wireless.h" #include "Shared/Config.h" #include "Events/VisionObjectEvent.h" #include "Vision/SegmentedColorGenerator.h" #include "Vision/RegionGenerator.h" typedef RegionGenerator::region region; //!< shorthand - using CMVision regions typedef SegmentedColorGenerator::color_class_state color_class_state; //!< shorthand - using CMVision color structs BallDetectionGenerator::BallDetectionGenerator(EventBase::EventGeneratorID_t gid, unsigned int sid, unsigned int mysid, unsigned int colorIdx, unsigned int threshmapChan, unsigned int noiseFiltering, float confidence) : EventGeneratorBase("BallDetectionGenerator",EventBase::visObjEGID,mysid,gid,sid), clrIdx(colorIdx), tmIdx(threshmapChan), ball(), present(false), count(0), noiseThreshold(noiseFiltering), confidenceThreshold(confidence) {} void BallDetectionGenerator::processEvent(const EventBase& e) { PROFSECTION("BallDetection::processEvent()",state->mainProfile); EventGeneratorBase::processEvent(e); if(e.getGeneratorID()!=getListenGeneratorID() || e.getSourceID()!=getListenSourceID()) return; const SegmentedColorFilterBankEvent * segev=dynamic_cast<const SegmentedColorFilterBankEvent*>(&e); if(NULL==segev) { serr->printf("BallDetectionGenerator's event %s was not a SegmentedColorFilterBankEvent",e.getName().c_str()); return; } static const bool debug_ball = false; static const bool debug_conf = false; static int frame_cnt=0; static const int print_period=1; if(debug_ball) frame_cnt = (frame_cnt + 1) % print_period; unsigned int layer=segev->getNumLayers()-config->vision.resolution-1; const color_class_state& ballCCS=reinterpret_cast<const color_class_state*>(segev->getImage(layer,tmIdx))[clrIdx]; ball.confidence = 0; region * ball_region=NULL; region * or_reg=ballCCS.list; //head of linked list of regions of target color. or_reg= original region (without noise removal) if(!or_reg) return; unsigned int n = 0; while(or_reg && n<NUM_CHECK) { //float conf,conf0,conf_square_bbox,conf_area,conf_green,conf_area_bonus; //float conf_red_v_area; //int edge; int w = or_reg->x2 - or_reg->x1 + 1; int h = or_reg->y2 - or_reg->y1 + 1; int edge = calcEdgeMask(or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2,segev->getWidth(layer),segev->getHeight(layer)); float conf0 = (w >= 3) * (h >= 3) * (or_reg->area >= 7); float conf_square_bbox = edge ? gaussian_with_min(pct_from_mean(w,h) / .6f, 1e-3) : gaussian_with_min(pct_from_mean(w,h) / .2f, 1e-3); float conf_area = edge ? gaussian_with_min(pct_from_mean(((float)M_PI)*w*h/4.0f,or_reg->area) / .6f, 1e-3) : gaussian_with_min(pct_from_mean(((float)M_PI)*w*h/4.0f,or_reg->area) / .2f, 1e-3); float conf_area_bonus = or_reg->area / 1000.0f; float conf = conf0*conf_square_bbox*conf_area + conf_area_bonus; if(conf > 1.0) conf = 1.0f; if(debug_conf && frame_cnt == 0) { printf("conf0 %g conf_square_bbox %g conf_area %g conf_area_bonus %g final %g\n", conf0,conf_square_bbox,conf_area,conf_area_bonus,conf); } /* float green_f; if(conf > ball->confidence) { if(debug_ball && (frame_cnt == 0 || frame_cnt == 1)) { printf("%s ball n%d cen (%f,%f) area %d bbox (%d,%d)-(%d,%d) conf %f\n", (ball_color==COLOR_PINK)?"Pink":"Orange", n,or_reg->cen_x,or_reg->cen_y,or_reg->area, or_reg->x1,or_reg->y1,or_reg->x2,or_reg->y2, conf); } int sx[2],sy[2]; // scan bounding coordinates; bool edge_x[2],edge_y[2]; // true if scan coordinate went off edge static const int scan_distance = 3; static int color_cnts[num_colors]; int scan_pixels; int good_value; sx[0] = or_reg->x1 - scan_distance; edge_x[0] = (sx[0] < 0); if(edge_x[0]) sx[0] = 0; sx[1] = or_reg->x2 + scan_distance; edge_x[1] = (sx[1] > width-1); if(edge_x[1]) sx[1] = width-1; sy[0] = or_reg->y1 - scan_distance; edge_y[0] = (sy[0] < 0); if(edge_y[0]) sy[0] = 0; sy[1] = or_reg->y2 + scan_distance; edge_y[1] = (sy[1] > height-1); if(edge_y[1]) sy[1] = height-1; scan_pixels = 0; for(int color_idx=0; color_idx<MAX_COLORS; color_idx++) color_cnts[color_idx]=0; // do horizontal strips for(int side=0; side<2; side++) { if(!edge_y[side]) { scan_pixels+=addToHistHorizStrip(sy[side],sx[0],sx[1],color_cnts); } } // do vertical strips for(int side=0; side<2; side++) { if(!edge_x[side]) { scan_pixels+=addToHistVertStrip(sx[side],sy[0],sy[1],color_cnts); } } int non_robot_fringe=0; non_robot_fringe += 5*color_cnts[getColor("blue")]; non_robot_fringe -= color_cnts[getColor("green")]; conf_red_v_area = 1 + non_robot_fringe / or_reg->area; conf_red_v_area = bound(conf_red_v_area,0.0,1.0); good_value = 0; good_value += 2*color_cnts[getColor("blue")]; good_value += 3*color_cnts[getColor("green")]/2; green_f = std::max(good_value+1,1) / (scan_pixels + 1.0); green_f = bound(green_f,0.0,1.0); conf_green = green_f; conf = conf0 * conf_square_bbox * conf_area * + conf_area_bonus; if(conf > 1.0) conf = 1.0; }*/ if(conf > ball.confidence) { /* d = sqrt((FocalDist * FocalDist * YPixelSize) * (M_PI * BallRadius * BallRadius) / (or_reg->area)); vector3d ball_dir; // direction of ball from camera in robot coordinates ball_dir = getPixelDirection(or_reg->cen_x,or_reg->cen_y); // Reject if ball above level plane // if(atan2(ball_dir.z,hypot(ball_dir.x,ball_dir.y)) <= (5*(M_PI/180))) { ball->edge = calcEdgeMask(or_reg); vector3d intersect_ball_loc,pixel_size_ball_loc; bool intersects=false; vector3d intersection_pt(0.0,0.0,0.0); intersects=GVector::intersect_ray_plane(camera_loc,ball_dir, vector3d(0.0,0.0,BallRadius),vector3d(0.0,0.0,1.0), intersection_pt); if(intersects) { intersect_ball_loc = intersection_pt; } pixel_size_ball_loc = camera_loc + ball_dir * d; vector3d ball_loc,alt_ball_loc; if(ball->edge!=0 && intersects) { ball_loc = intersect_ball_loc; alt_ball_loc = pixel_size_ball_loc; } else { alt_ball_loc = intersect_ball_loc; ball_loc = pixel_size_ball_loc; } */ ball.confidence = conf; // ball->loc = ball_loc; // ball->distance = hypot(ball_loc.x,ball_loc.y); // findSpan(ball->left,ball->right,or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2); ball_region = or_reg; /* if(debug_ball && frame_cnt==0) { printf("###found ball, conf %g loc (%g,%g,%g) alt (%g,%g,%g) dist %g left %g right %g edge %d\n", ball->confidence, ball->loc.x,ball->loc.y,ball->loc.z, alt_ball_loc.x,alt_ball_loc.y,alt_ball_loc.z, ball->distance,ball->left,ball->right,ball->edge); }*/ // } } or_reg = or_reg->next; n++; } //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); testSendEvent(*segev,ball.confidence,(int)ball_region->x1,(int)ball_region->x2,(int)ball_region->y1,(int)ball_region->y2); } void BallDetectionGenerator::testSendEvent(const FilterBankEvent& ev, float conf, int regX1,int regX2,int regY1,int regY2) { unsigned int layer=ev.getNumLayers()-config->vision.resolution-1; //pick the larger dimension for scaling, one dimension will be [-1,1], the other will depend on aspect ratio float dim=ev.getWidth(layer)>ev.getHeight(layer) ? ev.getWidth(layer) : ev.getHeight(layer); //the max width and height of scaled dimensions float cw=ev.getWidth(layer)/dim; float ch=ev.getHeight(layer)/dim; //scale the dimensions float cx1=2.0f*regX1/dim - cw; float cx2=2.0f*(regX2+1)/dim - cw; float cy1=2.0f*regY1/dim - ch; float cy2=2.0f*(regY2+1)/dim - ch; if (conf>confidenceThreshold) { if (present) { count=0; createEvent(EventBase::statusETID,cx1,cx2,cy1,cy2,cw,ch); } else { count++; if (count>noiseThreshold) { count=0; present=true; createEvent(EventBase::activateETID,cx1,cx2,cy1,cy2,cw,ch); } } } else { if (!present) { count=0; } else { count++; if (count>noiseThreshold) { count=0; present=false; createEvent(EventBase::deactivateETID,0,0,0,0,cw,ch); } else { createEvent(EventBase::statusETID,cx1,cx2,cy1,cy2,cw,ch); } } } } void BallDetectionGenerator::createEvent(EventBase::EventTypeID_t etid, float bbX1,float bbX2,float bbY1,float bbY2,float rx,float ry) const { VisionObjectEvent * vo=new VisionObjectEvent(etid,mySourceID,bbX1,bbX2,bbY1,bbY2,rx,ry); vo->setName(getName()); erouter->postEvent(vo); } int BallDetectionGenerator::calcEdgeMask(int x1,int x2,int y1,int y2, int width, int height) { static const int boundary_pixel_size=1; int edge = 0; if(x1 <= 0 +boundary_pixel_size) edge |= OFF_EDGE_LEFT ; if(x2 >= width -1-boundary_pixel_size) edge |= OFF_EDGE_RIGHT ; if(y1 <= 0 +boundary_pixel_size) edge |= OFF_EDGE_TOP ; if(y2 >= height-1-boundary_pixel_size) edge |= OFF_EDGE_BOTTOM; return edge; } /*! @file * @brief Implements BallDetectionGenerator, which uses segmented color region information to detect round objects * @author alokl (Creator) * @author ejt (reorganized) * @author Ignacio Herrero Reder < nhr at dte uma es > (VisionObjectInfo Boundary Box - bug 74) * * History is old, may have grown from CMPack (CMU Robosoccer) roots? * I think if there's any of their code left, it's probably *mostly* * the commented out stuff I (ejt) left for posterity when * reorganizing. But alokl didn't flag this as CMPack's prior to * inital release, and they didn't request credit for it when they * reviewed the code, so I guess it's all ours... * * $Author: ejt $ * $Name: tekkotsu-2_3 $ * $Revision: 1.9 $ * $State: Exp $ * $Date: 2005/01/14 22:24:51 $ */

Tekkotsu v2.3

Generated Sat Jan 29 02:25:21 2005 by Doxygen 1.4.0