/******************************************************************************* * * Helper functions * ******************************************************************************/ static void draw_basis_circles (struct app* app, const float pos[3], const float size, const float col_x[3], const float col_y[3], const float col_z[3], const uint32_t pick_id_x, const uint32_t pick_id_y, const uint32_t pick_id_z) { assert(app && pos); #define DRAW_CIRCLE(pitch, yaw, roll, color, pick) \ APP(imdraw_ellipse \ (app, \ APP_IMDRAW_FLAG_UPPERMOST_LAYER | APP_IMDRAW_FLAG_FIXED_SCREEN_SIZE, \ pick, \ pos, \ (float[]){size, size}, \ (float[]){(pitch), (yaw), (roll)}, \ (color))) DRAW_CIRCLE(PI * 0.5f, 0.f, 0.f, col_x, pick_id_x); DRAW_CIRCLE(0.f, 0.f, 0.f, col_y, pick_id_y); DRAW_CIRCLE(0.f, PI * 0.5f, 0.f, col_z, pick_id_z); #undef DRAW_CIRCLE }
bool BallDetection::execute() { mBallClouds.clear(); mBallCloudDataList.clear(); getRemovedBallCandidates().clearList(); mColor = DebugDrawer::RED; const list<Point>* cloud = getBallPoints().getCloud(); list<Point>::const_iterator it = cloud->begin(); for( ; it != cloud->end(); ++it) { addBallPointToNearestCloud( (*it)); } //mergeOverlappingClouds(); BoundingBox bestCompareBox(Point(0,0),0,0); double bestCompareResult = 0.0; vector<PointCloud>::const_iterator pointCloudIt; for( pointCloudIt = mBallClouds.begin(); pointCloudIt != mBallClouds.end(); ++pointCloudIt) { BoundingBox box = pointCloudIt->calculateBoundingBox(); Point point = pointCloudIt->getCenterPoint(); mBallCenterX = point.getX(); mBallCenterY = point.getY(); mBallRadius = (box.width + box.height) / 4; //3 // 4 //Debugger::INFO("BallDetection", "Cloud-Radius: %d", ballRadius); //Debugger::DEBUG("BallDetection", "maxValue: %i", maxValue); if (filterFieldLines(Point(mBallCenterX, mBallCenterY)) && filterHorizon(Point(mBallCenterX, mBallCenterY))) { // todo: filter out all possible balls with too small pixel to size ratio // filter out all balls with wrong expected size (tilt angle) getBallEdges().clearCloud(); //Debugger::DEBUG("BallDetection", "-----------------------------"); //Debugger::DEBUG("BallDetection", "BallRadius 1: %d", mBallRadius); mIsAtBorder = false; BallCompareData ballCompareData; if( !findAllBallEdges(ballCompareData)) { if (mDebugDrawings >= 2) { DRAW_CIRCLE("Ball", box.topLeft.getX(), box.topLeft.getY(), 2*mBallRadius, DebugDrawer::GREEN); } //break; } ballCompareData.centerX = mBallCenterX; ballCompareData.centerY = mBallCenterY; ballCompareData.numCloudPixels = pointCloudIt->getSize(); if (mDebugDrawings >= 6) { DRAW_POINT("Ball", mBallCenterX, mBallCenterY, DebugDrawer::YELLOW); } int countEdges = (int)getBallEdges().getSize(); int sumX = 0; int sumY = 0; list<Point>::const_iterator it2; for (it2 = getBallEdges().getCloud()->begin(); it2 != getBallEdges().getCloud()->end(); ++it2) { Point p = (*it2); sumX += p.getX(); sumY += p.getY(); } if( countEdges > 1) { mBallCenterX = sumX / countEdges; mBallCenterY = sumY / countEdges; if (mDebugDrawings >= 6) { DRAW_POINT("Ball", mBallCenterX, mBallCenterY, DebugDrawer::BLUE); } } double sumDistances = 0.0; vector<double> radiusList; int i = 0; list<Point>::const_iterator pointIt; for (pointIt = getBallEdges().getCloud()->begin(); pointIt != getBallEdges().getCloud()->end(); ++pointIt) { Point p = (*pointIt); double dx = (double)mBallCenterX - p.getX(); double dy = (double)mBallCenterY - p.getY(); double rad = sqrt((dx * dx) + (dy * dy)); radiusList.push_back(rad); sumDistances += rad; /*if( mDebugInfoEnabled) { Debugger::DEBUG("BallDetection", "%d) Radius point: %.2f (%d; %d)", i, rad, p.getX(), p.getY()); }*/ ++i; } if( countEdges >= 4) { mBallRadius = (int)(sumDistances / (double)countEdges); //Debugger::INFO("Ball", "Mean-Radius: %d", ballRadius); } //Debugger::DEBUG("BallDetection", "BallRadius 2: %d", mBallRadius); if (filterFieldLines(Point(mBallCenterX, mBallCenterY)) && filterHorizon(Point(mBallCenterX, mBallCenterY))) { //Debugger::DEBUG("BallDetection", "ballCenter: %d, %d", ballCenterX, ballCenterY); //Debugger::DEBUG("BallDetection", "radius: %d", ballRadius); ballCompareData.meanRadius = mBallRadius; ballCompareData.circleAnomaly = 0.0; int numCorrectEdges = 0; double maxAnomaly = (mMaxAnomalyRadiusFactor * mBallRadius) * (mMaxAnomalyRadiusFactor * mBallRadius); //double maxAnomaly = 25; // 5 pixel //Debugger::DEBUG("BallDetection", "maxAnomaly: %.2f", maxAnomaly); i = 0; vector<double>::const_iterator doubleIt; for (doubleIt = radiusList.begin(); doubleIt != radiusList.end(); ++doubleIt) { double anomaly = (double)mBallRadius - (*doubleIt); anomaly *= anomaly; if( mDebugInfoEnabled) { Debugger::DEBUG("BallDetection", "%d) Anomaly point: %.2f", i, anomaly); } if( anomaly > maxAnomaly) { ballCompareData.circleAnomaly += anomaly; //Debugger::DEBUG("BallDetection", "CircleAnomaly: %.2f",ballCompareData.circleAnomaly); } else { numCorrectEdges++; } ++i; } if( numCorrectEdges > 0) { //Debugger::DEBUG("BallDetection", "CircleAnomaly1: %.2f (%d)",ballCompareData.circleAnomaly, numCorrectEdges); ballCompareData.circleAnomaly /= (double)numCorrectEdges; } //Debugger::DEBUG("BallDetection", "CircleAnomaly2: %.2f",ballCompareData.circleAnomaly); if( countEdges > 0) { ballCompareData.correctEdgesRatio = (double)numCorrectEdges / (double)countEdges; } box.topLeft = Point(mBallCenterX - mBallRadius, mBallCenterY - mBallRadius); box.width = 2 * mBallRadius; box.height = 2 * mBallRadius; double compareResult = calculateCompareResult(ballCompareData); if( mDebugInfoEnabled) { Debugger::INFO("BallDetection", "CompareResult: %f", compareResult); } if( compareResult > bestCompareResult) { bestCompareResult = compareResult; bestCompareBox = box; } if (mDebugDrawings >= 3) { DRAW_CIRCLE("Ball", box.topLeft.getX() +1, box.topLeft.getY() +1, box.width, DebugDrawer::BROWN); } } else { if (mDebugDrawings >= 4) { DRAW_CIRCLE("Ball", box.topLeft.getX(), box.topLeft.getY(), box.width, DebugDrawer::ORANGE); } } } else { Object obj(box.topLeft.getX(), box.topLeft.getY(), box.width, box.height); obj.type = Object::BALL; getRemovedBallCandidates().addObject(obj); if (mDebugDrawings >= 5) { DRAW_CIRCLE("Ball", box.topLeft.getX(), box.topLeft.getY(), box.width, DebugDrawer::LIGHT_GRAY); } } } if( bestCompareResult > 0.0) { int x = bestCompareBox.topLeft.getX(); int y = bestCompareBox.topLeft.getY(); getBall().updateObject( x, y, bestCompareBox.width, bestCompareBox.height); //x = getBall().lastImageX; //y = getBall().lastImageY; // todo add angle from position in image double panAngle = DEGREE_TO_RADIANS * getBodyStatus().getPan(); double tiltAngle = DEGREE_TO_RADIANS * getBodyStatus().getTilt(); double length = getRobotConfiguration().cameraHeight * tan(tiltAngle); length += getRobotConfiguration().cameraOffsetX; getBall().lastVector.updateVector(panAngle, length); Debugger::INFO("BallDetection", "BallVector %.1f; %.1f : %.1f", panAngle*RADIANS_TO_DEGREE, tiltAngle*RADIANS_TO_DEGREE, length); if( mDebugDrawings >= 1) { x = bestCompareBox.topLeft.getX(); y = bestCompareBox.topLeft.getY(); DRAW_CIRCLE("Ball", x, y, bestCompareBox.width, DebugDrawer::RED); if( mDebugDrawings >= 2) { DRAW_TEXT("Ball", x + 2, y + 10, DebugDrawer::YELLOW, "%.0f mm (%d px; %.2f deg)", length, bestCompareBox.width, tiltAngle*RADIANS_TO_DEGREE); DRAW_TEXT("Ball", x + 2, y + 24, DebugDrawer::YELLOW, "Pan: %.2f deg", panAngle*RADIANS_TO_DEGREE); } } } else { getBall().notSeen(); } return true; }
//--------------------------------------------------------- void CWKSP_Shapes_Point::Draw_Symbol(wxDC &dc, int x, int y, int size) { switch( m_Symbol_Type ) { default: case 0: // circle DRAW_CIRCLE(1.0); break; case 1: // square DRAW_SQUARE; break; case 2: // rhombus DRAW_RHOMBUS; break; case 3: // triangle up DRAW_TRIANGLE_UP; break; case 4: // triangle down DRAW_TRIANGLE_DOWN; break; case 5: // square in circle DRAW_CIRCLE(1.0); DRAW_SQUARE; break; case 6: // rhombus in circle DRAW_CIRCLE(1.1); DRAW_RHOMBUS; break; case 7: // triangle up in circle DRAW_CIRCLE(1.0); DRAW_TRIANGLE_UP; break; case 8: // triangle down in circle DRAW_CIRCLE(1.0); DRAW_TRIANGLE_DOWN; break; case 9: // circle in square DRAW_SQUARE; DRAW_CIRCLE(0.7); break; case 10: // circle in rhombus DRAW_RHOMBUS; DRAW_CIRCLE(0.5 * sqrt(2.0)); break; case 11: // circle in triangle up DRAW_TRIANGLE_UP; DRAW_CIRCLE(0.5); break; case 12: // circle in triangle down DRAW_TRIANGLE_DOWN; DRAW_CIRCLE(0.5); break; case 13: // image { double d = (double)m_Symbol.GetWidth() / (double)m_Symbol.GetHeight(); int sx, sy; if( d > 1.0 ) { sx = size; sy = (int)(0.5 + size / d); } else { sx = (int)(0.5 + size * d); sy = size; } if( sx > 0 && sy > 0 ) { dc.DrawBitmap(wxBitmap(m_Symbol.Scale(2 * sx, 2 * sy)), x - sx, y - sy, true); } } break; } }