void draw_picture(p_draw_buffer_t picture) { for( int i=0 ; i<MUX_WIDTH ; ++i ) for( int j=0 ; j<MUX_HEIGHT ; ++j ) DRAW_POINT( i, j, picture[i][j].r * MUX_COLOR_DEPTH, picture[i][j].g * MUX_COLOR_DEPTH, picture[i][j].b * MUX_COLOR_DEPTH ); }
void DRAW_LINE(int x1,int y1,int x2,int y2,unsigned int color) { int t; int xerr=0,yerr=0,delta_x,delta_y,distance; int incx,incy,uRow,uCol; delta_x=x2-x1; //计算坐标增量 delta_y=y2-y1; uRow=x1; uCol=y1; if(delta_x>0)incx=1; //设置单步方向 else if(delta_x==0)incx=0;//垂直线 else { incx=-1; delta_x=-delta_x; } if(delta_y>0)incy=1; else if(delta_y==0)incy=0;//水平线 else { incy=-1; delta_y=-delta_y; } if( delta_x>delta_y)distance=delta_x; //选取基本增量坐标轴 else distance=delta_y; for(t=0; t<=distance+1; t++ ) //画线输出 { DRAW_POINT(uRow,uCol,color);//画点 xerr+=delta_x ; yerr+=delta_y ; if(xerr>distance) { xerr-=distance; uRow+=incx; } if(yerr>distance) { yerr-=distance; uCol+=incy; } } }
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; }
bool BallDetection::findBallEdge(int xSlope, int ySlope, int &steps) const { Point p; int x = mBallCenterX; int y = mBallCenterY; const YUVImage* image = &getImage(); int imageWidth = getImage().getWidth(); int imageHeight = getImage().getHeight(); int additionalPixelsScanned = 0; bool scanAdditionalPixels = false; bool ballPixelFound = false; bool edgeReached = false; int slopeSteps = abs(xSlope) + abs(ySlope); int maxSteps = 3*mBallRadius / slopeSteps; steps = 0; while (!edgeReached) { x += xSlope; y += ySlope; // bigger than expected maximal size => not found! if (steps > maxSteps) { return false; } // point is outside image => mark point as edge if( x >= imageWidth || x < 0 || y >= imageHeight || y < 0) { if( !scanAdditionalPixels) { p.updatePoint(x, y); } //mIsAtBorder = true; break; } //bool isPossibleEdge = image->getValue(x, y).V < minTreshold; bool isPossibleEdge = image->getValue(x, y).V < mMinV || image->getValue(x, y).U >= mMaxU || image->getValue(x, y).Y < mMinY; // Debugger::DEBUG("BallDetector", "(%i|%i) Sobelvalue: %i", x, y, // sobelValue); if (!scanAdditionalPixels) { if (isPossibleEdge) { if( ballPixelFound) { p.updatePoint(x, y); } scanAdditionalPixels = true; } else if( !ballPixelFound){ ballPixelFound = true; } } else { if (isPossibleEdge) { additionalPixelsScanned += slopeSteps; // if additionalPixels were all negative, save last found edge point and cancel if (additionalPixelsScanned >= mAdditionalScanPixelCount) { edgeReached = true; } } else { // point is valid ball pixel; reset scanAdditionalPixels p.updatePoint(x, y); additionalPixelsScanned = 0; scanAdditionalPixels = false; ballPixelFound = true; } } steps++; } if( !ballPixelFound) { return false; } getBallEdges().addPoint(p); if (mDebugDrawings >= 7) { DRAW_POINT("Ball", p.getX(), p.getY(), DebugDrawer::DARK_RED); } /*Debugger::INFO("BallDetection","Edge (%d, %d) Y: %d, U: %d, V: %d", p.getX(), p.getY(), image->getValue(p.getX(), p.getY()).Y, image->getValue(p.getX(), p.getY()).U, image->getValue(p.getX(), p.getY()).V); */ return true; }
bool GoalFind::execute(){ getGoalPoles().clearList(); #ifdef _DEBUG Debugger::DEBUG("GoalFind","Start Goal Find!"); #endif const YUVImage* yuvImage = &getImage(); int width = yuvImage->getWidth(); int height = yuvImage->getHeight(); int poleWidth = 0; vector< Object > objectMap; //Iterate trough the Picture from left to Right for (int x = 0; x < width; x++){ //Get the Field line Y for pixel x int y = getFieldLines().getY(x) + mPoleWatchBelowLine; //Get Color Value at Point x,y struct YUVImage::YUVData currentColor = yuvImage->getValue(x,y); //check if color not Green and High Y Value(White) if (currentColor.U > mMaxU_Green || currentColor.V > mMaxV_Green) { if (currentColor.Y >= mMinY_Whitegoal && currentColor.V >= mMinV_Whitegoal && currentColor.U >= mMinU_Whitegoal) { #ifdef _DEBUG if (mDebugDrawings) { DRAW_POINT("Goal", x, y, DebugDrawer::CYAN); } #endif //Used for calculate width of GolePole poleWidth++; //If the White Area end start provide it as goal } else { //Check if line is to small to be a goal Pole if (poleWidth >= mMinPoleWidth) { int topY = y; //Now find out the Height of the Goal Beginning at x,y moving to the Top until its not white int middleX = x - (poleWidth/2); /*while (topY > 0) { struct YUVImage::YUVData col = yuvImage->getValue(middleX,topY); if (col.Y < mMinY_Whitegoal) { break; } topY--; }*/ topY =findMaxHeight(middleX,topY); int botY = y; //Go to the Bottom of the Goal while (botY+1 < height) { struct YUVImage::YUVData col = yuvImage->getValue(middleX,botY); if (col.Y < mMinY_Whitegoal) { break; } botY++; } int poleHeight = botY - topY; if (poleHeight >= mMinPoleHeight) { Object obj(x - poleWidth, topY, poleWidth, poleHeight); obj.type = Object::GOAL_POLE_YELLOW; #ifdef _DEBUG if (mDebugDrawings) { DRAW_BOX("Goal", obj.lastImageTopLeftX, obj.lastImageTopLeftY, obj.lastImageWidth, obj.lastImageHeight, DebugDrawer::BLUE); } #endif objectMap.push_back(obj); } else { #ifdef _DEBUG if (mDebugDrawings) { DRAW_BOX("Goal", x - poleWidth, topY, poleWidth, poleHeight, DebugDrawer::LIGHT_GRAY); } #endif } } poleWidth = 0; } } } // use only the outer objects (left and right) vector< Object >::iterator objIt = objectMap.begin(); if( objIt != objectMap.end()) { Object obj = *objIt; getGoalPoles().addObject(obj); // check if there is a right pole (last in list) and then add it objIt = objectMap.end(); if( --objIt != objectMap.begin()) { obj = *objIt; getGoalPoles().addObject(obj); } } return true; }