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;
}
