/* As part of our roundness checking we scan the midlines of the blob.
In principle every pixel should be the right color.
@param b candidate blob
@return a pair containing the number of good and bad pixels
*/
pair<int, int> Ball::scanMidlinesForRoundnessInformation(Blob b) {
int w = b.width();
int h = b.height();
int x = b.getLeftTopX();
int y = b.getLeftTopY();
int pix;
int goodPix = 0, badPix = 0;
for (int i = 0; i < h; i++) {
pix = thresh->getThresholded(y+i,x + w/2);
if (pix == ORANGE || pix == ORANGERED || pix == ORANGEYELLOW) {
goodPix++;
} else if (pix != GREY)
badPix++;
}
for (int i = 0; i < w; i++) {
pix = thresh->getThresholded(y+h/2,x + i);
if (pix == ORANGE || pix == ORANGERED || pix == ORANGEYELLOW) {
goodPix++;
} else if (pix != GREY) {
badPix++;
}
}
pair<int, int> info;
info.first = goodPix;
info.second = badPix;
return info;
}
/* Returns whether true when the size of the cross is reasonable when tested
against the pixEstimated distance.
@param b the candidate cross
@return if the size is reasonable
*/
bool Cross::checkSizeAgainstPixEstimate(Blob b) {
int x = b.getLeftTopX();
int y = b.getLeftTopY();
int w = b.width();
int h = b.height();
// before we spend a lot of time processing, let's see if it is a reasonable size
if (w > 3 * h || h > 2 * w) {
return false;
}
estimate e = vision->pose->pixEstimate(x, y, 0.0);
if (CROSSDEBUG) {
cout << "Distance check: " << e.dist << " " << w << endl;
}
if (e.dist < 100.0f && w < 20) {
return false;
} else if (e.dist < 150.0f && w < 12) {
return false;
} else if (e.dist < 200.0f && w < 8) {
return false;
}
if (e.dist > 200.0f && w > 20) {
return false;
}
return true;
}
/* Draws the outline of a blob in the specified color.
* @param b the blob
* @param c the color to paint
*/
void Ball::drawBlob(Blob b, int c) {
#ifdef OFFLINE
thresh->drawLine(b.getLeftTopX(), b.getLeftTopY(),
b.getRightTopX(), b.getRightTopY(),
c);
thresh->drawLine(b.getLeftTopX(), b.getLeftTopY(),
b.getLeftBottomX(), b.getLeftBottomY(),
c);
thresh->drawLine(b.getLeftBottomX(), b.getLeftBottomY(),
b.getRightBottomX(), b.getRightBottomY(),
c);
thresh->drawLine(b.getRightTopX(), b.getRightTopY(),
b.getRightBottomX(), b.getRightBottomY(),
c);
#endif
}
/* Checks out how much of the blob is of the right color.
* If it is enough returns true, if not false.
* @param tempobj the blob we're checking (usually a post)
* @param minpercent how good it needs to be
* @return was it good enough?
*/
bool Ball::rightBlobColor(Blob tempobj, float minpercent) {
int x = tempobj.getLeftTopX();
int y = tempobj.getLeftTopY();
int spanX = tempobj.width();
int spanY = tempobj.height();
if (spanX < 2 || spanY < 2) return false;
int ny, nx, starty, startx;
int good = 0, total = 0;
for (int i = 0; i < spanY; i++) {
starty = y + i;
startx = x;
for (int j = 0; j < spanX; j++) {
nx = startx + j;
ny = starty;
if (ny > -1 && nx > -1 && ny < IMAGE_HEIGHT && nx < IMAGE_WIDTH) {
total++;
if (thresh->thresholded[ny][nx] == color) {
good++;
}
}
}
}
float percent = (float)good / (float) (total);
if (percent > minpercent) {
return true;
}
return false;
}
/* Determines if a line intersects the candidate cross, if so it
is thrown out.
@param b the candidate cross
@return whether a line intersects
*/
bool Cross::checkForLineIntersection(Blob b) {
int x = b.getLeftTopX();
int y = b.getLeftTopY();
int w = b.width();
int h = b.height();
point <int> plumbLineTop, plumbLineBottom, line1start, line1end;
plumbLineTop.x = x + w / 2; plumbLineTop.y = y;
plumbLineBottom.x = x; plumbLineBottom.y = y + h;
const vector <boost::shared_ptr<VisualLine> >* lines =
vision->fieldLines->getLines();
for (vector <boost::shared_ptr<VisualLine> >::const_iterator k =
lines->begin();
k != lines->end(); k++) {
pair<int, int> foo = Utility::plumbIntersection(plumbLineTop,
plumbLineBottom,
(*k)->getStartpoint(),
(*k)->getEndpoint());
if (foo.first != Utility::NO_INTERSECTION && foo.second !=
Utility::NO_INTERSECTION) {
if (CROSSDEBUG)
cout << "Throwing out blob that is part of a line" << endl;
return true;
}
}
return false;
}
/* Print debugging information for a blob.
* @param b the blob
*/
void Ball::printBlob(Blob b) {
#if defined OFFLINE
cout << "Blob Top Left Corner " << b.getLeftTopX() << " " << b.getLeftTopY()
<< endl;
cout << "Width/height " << b.width() << " " << b.height();
cout << " Amount of orange " << b.getPixels() << endl;
#endif
}
/* Scans around the outside of the blob looking for green. Ideally the
cross will have only green around it. White is a big problem, so is
having the cross near the edge. This is because our version of the
cross is so simple - we don't look at its shape at all! Instead we
just look for white blobs with the right general properties.
@param b the candidate cross
@return the amount of green in the perimeter (white gives a huge
negative penalty)
*/
bool Cross::scanAroundPerimeter(Blob b) {
const float greenThreshold = 0.75f;
int x = b.getLeftTopX();
int y = b.getLeftTopY();
int w = b.width();
int h = b.height();
int counter = 0, count = 0;
// first scan the sides
for (int i = max(0, y - 2); i < min(IMAGE_HEIGHT - 1, y + h + 2); i++) {
if (x > 3) {
if (Utility::isGreen(thresh->getThresholded(i,x - 4))) {
count++;
} else if (Utility::isWhite(thresh->getThresholded(i,x - 4))) {
count-=3;
}
counter++;
} else return false;
if (x + w + 4 < IMAGE_WIDTH) {
if (Utility::isGreen(thresh->getThresholded(i,x + w+ 4))) {
count++;
} else if (Utility::isWhite(thresh->getThresholded(i,x + w+ 4))) {
count-=3;
}
counter++;
} else return false;
}
// now scan above and below
for (int i = max(0, x - 2); i < min(IMAGE_WIDTH - 1, x + w + 2); i++) {
if (y > 1) {
if (Utility::isGreen(thresh->getThresholded(y - 2,i))) {
count++;
} else if (Utility::isUndefined(thresh->getThresholded(y - 2,i))) {
count--;
} else if (Utility::isWhite(thresh->getThresholded(y - 2,i))) {
count-=3;
}
counter++;
} else return false;
if (y + h + 2 < IMAGE_HEIGHT) {
if (Utility::isGreen(thresh->getThresholded(y+h+2,i))) {
count++;
} else if (Utility::isWhite(thresh->getThresholded(y+h+2,i))) {
count-=3;
}
counter++;
} else return false;
}
if (count > (float)counter * greenThreshold) {
if (CROSSDEBUG) {
cout << "White stats: " << count << " " << counter << endl;
}
return true;
}
return false;
}
/* Print debugging information for a blob.
* @param b the blob
*/
void Robots::printBlob(Blob b) {
#if defined OFFLINE
cout << "Outputting blob" << endl;
cout << b.getLeftTopX() << " " << b.getLeftTopY() << " " << b.getRightTopX() << " "
<< b.getRightTopY() << endl;
cout << b.getLeftBottomX() << " " << b.getLeftBottomY() << " " << b.getRightBottomX()
<< " " << b.getRightBottomY() << endl;
#endif
}
/**
* Update the robot values from the blob
*
* @param b The blob to update our object from.
*/
void VisualRobot::updateRobot(Blob b)
{
setLeftTopX(b.getLeftTopX());
setLeftTopY(b.getLeftTopY());
setLeftBottomX(b.getLeftBottomX());
setLeftBottomY(b.getLeftBottomY());
setRightTopX(b.getRightTopX());
setRightTopY(b.getRightTopY());
setRightBottomX(b.getRightBottomX());
setRightBottomY(b.getRightBottomY());
setX(b.getLeftTopX());
setY(b.getLeftTopY());
setWidth(dist(b.getRightTopX(), b.getRightTopY(), b.getLeftTopX(),
b.getLeftTopY()));
setHeight(dist(b.getLeftTopX(), b.getLeftTopY(), b.getLeftBottomX(),
b.getLeftBottomY()));
setCenterX(getLeftTopX() + ROUND2(getWidth() / 2));
setCenterY(getRightTopY() + ROUND2(getHeight() / 2));
setDistance(1);
}
// only called on really big orange blobs
float Ball::rightHalfColor(Blob tempobj)
{
const float COLOR_THRESH = 0.15f;
const float POOR_VALUE = 0.10f;
int x = tempobj.getLeftTopX();
int y = tempobj.getLeftTopY();
int spanY = tempobj.height() - 1;
int spanX = tempobj.width() - 1;
int good = 0, good1 = 0, good2 = 0;
int pix;
if (rightColor(tempobj, ORANGE) < COLOR_THRESH) return POOR_VALUE;
for (int i = spanY / 2; i < spanY; i++) {
for (int j = 0; j < spanX; j++) {
pix = thresh->thresholded[y + i][x + j];
if (y + i > -1 && x + j > -1 && (y + i) < IMAGE_HEIGHT &&
x + j < IMAGE_WIDTH && (pix == ORANGE || pix == ORANGERED ||
pix == ORANGEYELLOW)) {
good++;
}
}
}
for (int i = 0; i < spanY; i++) {
for (int j = 0; j < spanX / 2; j++) {
pix = thresh->thresholded[y + i][x + j];
if (y + i > -1 && x + j > -1 && (y + i) < IMAGE_HEIGHT &&
x + j < IMAGE_WIDTH && (pix == ORANGE || pix == ORANGERED ||
pix == ORANGEYELLOW)) {
good1++;
}
}
}
for (int i = 0; i < spanY; i++) {
for (int j = spanX / 2; j < spanX; j++) {
pix = thresh->thresholded[y + i][x + j];
if (y + i > -1 && x + j > -1 && (y + i) < IMAGE_HEIGHT &&
x + j < IMAGE_WIDTH && (pix == ORANGE || pix == ORANGERED ||
pix == ORANGEYELLOW)) {
good2++;
}
}
}
if (BALLDEBUG) {
cout << "Checking half color " << good << " " << good1 << " " <<
good2 << " " << (spanX * spanY / 2) << endl;
}
float percent = (float)max(max(good, good1), good2) /
(float) (spanX * spanY / 2);
return percent;
}
/* As part of roundness checking we scan the diagonals of the blob.
We know that there is a predictible transition point from no ball
to ball and categorize each pixel accordingly
@param b the blob
@return a pair containing the number of good pixels, and bad ones
*/
pair<int, int> Ball::scanDiagonalsForRoundnessInformation(Blob b) {
const float CORNER_CHUNK_DIV = 6.0f;
int w = b.width();
int h = b.height();
int x = b.getLeftTopX();
int y = b.getLeftTopY();
int pix;
int goodPix = 0, badPix = 0;
int d = ROUND2(static_cast<float>(std::max(w, h)) /
CORNER_CHUNK_DIV);
int d3 = min(w, h);
pair<int, int> info;
for (int i = 0; i < d3; i++) {
pix = thresh->getThresholded(y+i,x+i);
if (i < d || (i > d3 - d)) {
if (pix == ORANGE || pix == ORANGERED) {
//drawPoint(x+i, y+i, BLACK);
badPix++;
} else {
goodPix++;
}
} else {
if (pix == ORANGE || pix == ORANGERED || pix == ORANGEYELLOW) {
goodPix++;
} else if (pix != GREY) {
badPix++;
//drawPoint(x+i, y+i, PINK);
}
}
pix = thresh->getThresholded(y+i,x+w-i);
if (i < d || (i > d3 - d)) {
if (pix == ORANGE || pix == ORANGERED) {
//drawPoint(x+w-i, y+i, BLACK);
badPix++;
}
else {
goodPix++;
}
} else if (pix == ORANGE || pix == ORANGERED ||
pix == ORANGEYELLOW) {
goodPix++;
} else if (pix != GREY) {
badPix++;
}
}
info.first = goodPix;
info.second = badPix;
return info;
}
/* Prints a bunch of ball information about the best ball candidate (or any one).
* @param b the candidate ball
* @param c how confident we are its a ball
* @param p how many occlusions
* @param o what the occlusions are if any
* @param bg where around the ball there is green
*/
void Ball::printBall(Blob b, int c, float p, int o) {
#ifdef OFFLINE
if (BALLDEBUG) {
cout << "Ball info: " << b.getLeftTopX() << " " << b.getLeftTopY()
<< " " << b.width() << " " << b.height() << endl;
cout << "Confidence: " << c << " Orange Percent: " << p
<< " Occlusions: ";
if (o == NOOCCLUSION) cout << "none";
if (o % LEFTOCCLUSION == 0) cout << "left ";
if (o % RIGHTOCCLUSION == 0) cout << "right ";
if (o % TOPOCCLUSION == 0) cout << "top ";
if (o % BOTTOMOCCLUSION == 0) cout << "bottom ";
cout << endl;
}
#endif
}
int Ball::ballNearGreen(Blob b)
{
const int EXTRA_LINES = 6;
// first check the bottom
int w = b.width();
int h = b.height();
int where = NOGREEN;
if (greenCheck(b))
where = where * GREENBELOW;
// now try the sides - happily the ball is round so we don't have to worry
// about scan angles
int x = b.getLeftTopX();
int y = b.getLeftTopY();
for (int i = 0; i < h && y + i < IMAGE_HEIGHT && where % GREENLEFT != 0;
i= i+2) {
for (int j =-1; j < EXTRA_LINES && x + j > -1 && where % GREENLEFT != 0;
j++) {
if (thresh->thresholded[i+y][x - j] == GREEN) {
where = where * GREENLEFT;
}
}
}
for (int i = 0; i < w && x + i < IMAGE_WIDTH && where % GREENABOVE != 0;
i= i+2) {
for (int j = 0; j < EXTRA_LINES && y - j > 0 && where % GREENABOVE != 0;
j++) {
if (thresh->thresholded[i+y][j+x] == GREEN) {
where = where * GREENABOVE;
}
}
}
x = b.getRightTopX();
y = b.getRightTopY();
for (int i = 0; i < h && y + i < IMAGE_HEIGHT && where % GREENRIGHT != 0;
i= i+2) {
for (int j = 0; j < EXTRA_LINES && x + j < IMAGE_WIDTH &&
where % GREENRIGHT != 0; j++) {
if (thresh->thresholded[i+y][j+x] == GREEN) {
where = where * GREENRIGHT;
}
}
}
// put in the case where we don't have any, but want to check the corners
return where;
}
/* When we process blobs we start them with BAD_VALUE such that we can easily
* tell if whatever processing we did worked out. Here we make that check.
* @param b the blob we worked on.
* @return true when the processing worked, false otherwise
*/
bool Ball::blobOk(Blob b) {
if (b.getLeftTopX() > BAD_VALUE && b.getLeftBottomX() > BAD_VALUE &&
b.width() > 2)
return true;
return false;
}
float Ball::rightColor(Blob tempobj, int col)
{
const int MIN_BLOB_SIZE = 1000;
const float RED_PERCENT = 0.10f;
const float ORANGE_PERCENT = 0.20f;
const float ORANGEYELLOW_PERCENT = 0.40f;
const float GOOD_PERCENT = 0.65f;
int x = tempobj.getLeftTopX();
int y = tempobj.getLeftTopY();
int spanY = tempobj.height();
int spanX = tempobj.width();
if (spanX < 2 || spanY < 2) return false;
int good = 0;
int ogood = 0;
int orgood = 0;
int oygood = 0;
int red = 0;
for (int i = 0; i < spanY; i++) {
for (int j = 0; j < spanX; j++) {
int pix = thresh->thresholded[y + i][x + j];
if (y + i > -1 && x + j > -1 && (y + i) < IMAGE_HEIGHT &&
x + j < IMAGE_WIDTH && (pix == ORANGE || pix == ORANGERED ||
pix == ORANGEYELLOW)) {
good++;
if (pix == ORANGE)
ogood++;
else if (pix == ORANGEYELLOW)
oygood++;
else
orgood++;
} else if (pix == RED)
red++;
}
}
// here's a big hack - if we have a ton of orange, let's say it is enough
// unless the percentage is really low
if (BALLDEBUG) {
cout << "Orange " << ogood << " " << orgood << " " << red << " "
<< tempobj.getArea() << endl;
}
if (tempobj.getArea() > MIN_BLOB_SIZE) return (float) good /
(float) tempobj.getArea();
//if (ogood < 2 * orgood) return 0.1; // at least two thirds of the "orange"
// pixels should be orange
if (red > static_cast<float>(spanX * spanY) * RED_PERCENT) {
if (BALLDEBUG)
cout << "Too much red" << endl;
// before giving up let's try and salvage this one
if (ogood < static_cast<float>(spanX * spanY) * ORANGE_PERCENT)
return RED_PERCENT;
if (greenCheck(tempobj) && greenSide(tempobj) && roundness(tempobj) !=
BAD_VALUE) {
return GOOD_PERCENT;
}
return RED_PERCENT;
}
/*if (ogood < static_cast<float>(spanX * spanY) * ORANGE_PERCENT) {
if (BALLDEBUG)
cout << "Not enough pure orange" << endl;
return RED_PERCENT;
}*/
if (tempobj.getArea() > MIN_BLOB_SIZE &&
ogood + oygood > (static_cast<float>(spanX * spanY) * ORANGEYELLOW_PERCENT)
&& good < ( static_cast<float>(spanX * spanY) * GOOD_PERCENT))
return GOOD_PERCENT;
float percent = (float)good / (float) (spanX * spanY);
if (col == GREEN)
return (float)good;
return percent;
}
/* Is the ball at the boundary of the screen?
* @param b the ball
* @return whether or not it borders a boundary
*/
bool Ball::atBoundary(Blob b) {
return b.getLeftTopX() == 0 || b.getRightTopX() >= IMAGE_WIDTH -1 ||
b.getLeftTopY() == 0
|| b.getLeftBottomY() >= IMAGE_HEIGHT - 1;
}
bool Ball::badSurround(Blob b) {
// basically check around the blob and see if it is ok - ideally we'd have
// some green, worrisome would be lots of RED
static const int SURROUND = 12;
const float GREEN_PERCENT = 0.1f;
int x = b.getLeftTopX();
int y = b.getLeftTopY();
int w = b.width();
int h = b.height();
int surround = min(SURROUND, w/2);
int greens = 0, orange = 0, red = 0, borange = 0, pix, realred = 0,
yellows = 0;
// now collect information on the area surrounding the ball and the ball
x = max(0, x - surround);
y = max(0, y - surround);
w = w + surround * 2;
h = h + surround * 2;
for (int i = 0; i < w && x + i < IMAGE_WIDTH; i++) {
for (int j = 0; j < h && y + j < IMAGE_HEIGHT; j++) {
pix = thresh->getThresholded(y + j,x + i);
if (pix == ORANGE || pix == ORANGEYELLOW) {
orange++;
if (x + i >= b.getLeft() && x + i <= b.getRight() &&
y + j >= b.getTop() && y + j <= b.getBottom()) {
borange++;
}
} else if (pix == RED) {
realred++;
} else if (pix == ORANGERED) {
red++;
} else if (pix == GREEN) {
greens++;
} else if (pix == YELLOW && j < surround) {
yellows++;
}
}
}
if (BALLDEBUG) {
cout << "Surround information " << red << " " << realred << " "
<< orange << " " << borange << " " << greens << " "
<< yellows << endl;
}
if (realred > borange) {
if (BALLDEBUG) {
cout << "Too much real red" << endl;
}
return true;
}
if (realred > greens && w * h < 2000) {
if (BALLDEBUG) {
cout << "Too much real red versus green" << endl;
}
return true;
}
if (realred > borange && realred > orange) {
if (BALLDEBUG) {
cout << "Too much real red vs borange" << endl;
}
return true;
}
if (orange - borange > borange * 0.3 && orange - borange > 10) {
if (BALLDEBUG) {
cout << "Too much orange outside of the ball" << endl;
}
// We can run into this problem with reflections - let's see if
// we're up against a post or something
if (yellows > w * 3) {
if (BALLDEBUG) {
cout << "But lots of yellow, doing nothing " << endl;
}
return false;
} else {
return true;
}
}
if ((red > orange) &&
(static_cast<float>(greens) <
(static_cast<float>(w * h) * GREEN_PERCENT))) {
if (BALLDEBUG) {
cout << "Too much real orangered without enough green" << endl;
}
return true;
}
if (red > orange || (realred > greens && realred > 2 * w &&
realred > borange * 0.1)) {
if (BALLDEBUG) {
cout << "Too much real red - doing more checking" << endl;
}
x = b.getLeftTopX();
y = b.getLeftBottomY();
if (nearImageEdgeX(x, 2) || nearImageEdgeX(x+b.width(), 2) ||
nearImageEdgeY(y, 2)) {
if (BALLDEBUG) {
cout << "Dangerous corner location detected " << x << " "
<< y << " " << w << endl;
}
return true;
}
return roundness(b) == BAD_VALUE;
}
return false;
}
void Cross::checkForX(Blob b) {
int x = b.getLeftTopX();
int y = b.getLeftTopY();
int w = b.width();
int h = b.height();
int count = 0, counter = 0;
// before we spend a lot of time processing, let's see if it is a reasonable size
if (!checkSizeAgainstPixEstimate(b)) {
return;
}
if (CROSSDEBUG) {
cout << "Have a candidate white blob " << x << " " << y << endl;
}
// First we scan the outside of the blob. It should basically be all
// green. What we don't want are line fragments or robot fragments
// so finding white is very bad.
if (!scanAroundPerimeter(b)) {
return;
}
const int HORIZONCHECK = 15;
// make sure we aren't too close to the horizon
if (y - HORIZONCHECK < field->horizonAt(x) && field->horizonAt(x) > 0) return;
if (y - HORIZONCHECK < field->horizonAt(x+w) && field->horizonAt(x+w) > 0) return;
if (CROSSDEBUG) {
cout << "Passed Horizon checks " << endl;
}
// first make sure this isn't really a line
if (checkForLineIntersection(b)) {
return;
}
// Is the cross white enough? At least half the pixels must be white.
if (!rightBlobColor(b, 0.5f)) {
if (CROSSDEBUG) {
cout << "Tossing a blob for not being white enough " << endl;
}
return;
}
// passed all of our current sanity checks
if (CROSSDEBUG) {
cout << "Found a cross " << endl;
b.printBlob();
}
// Make sure we don't have more than one candidate cross. Note: we
// actually can see two crosses at some places on the field, but for
// now we just will ID one of them.
// TODO: allow seeing two crosses - but must test whether they are
// correctly aligned with each other, etc.
if (vision->cross->getWidth() > 0) {
if (w * h > vision->cross->getWidth() * vision->cross->getHeight()) {
vision->cross->updateCross(&b);
if (CROSSDEBUG) {
cout << "Larger than previous cross." << endl;
}
} else {
if (CROSSDEBUG) {
cout << "Threw out extra cross - smaller " << endl;
}
}
} else {
vision->cross->updateCross(&b);
}
}
void Cross::checkForX(Blob b) {
const float greenThreshold = 0.8f;
int x = b.getLeftTopX();
int y = b.getLeftTopY();
int w = b.width();
int h = b.height();
int count = 0, counter = 0;
// First we scan the outside of the blob. It should basically be all
// green. What we don't want are line fragments or robot fragments
// so finding white is very bad.
// first scan the sides
for (int i = max(0, y - 2); i < min(IMAGE_HEIGHT - 1, y + h + 2); i++) {
if (x > 1) {
if (thresh->thresholded[i][x - 2] == GREEN)
count++;
else if (thresh->thresholded[i][x - 2] == WHITE)
count-=3;
counter++;
} else return;
if (x + w + 2 < IMAGE_WIDTH) {
if (thresh->thresholded[i][x + w+ 2] == GREEN)
count++;
else if (thresh->thresholded[i][x + w+ 2] == WHITE)
count-=3;
counter++;
} else return;
}
// now scan above and below
for (int i = max(0, x - 2); i < min(IMAGE_WIDTH - 1, x + w + 2); i++) {
if (y > 1) {
if (thresh->thresholded[y - 2][i] == GREEN)
count++;
else if (thresh->thresholded[y - 2][i] == WHITE)
count-=3;
counter++;
} else return;
if (y + h + 2 < IMAGE_HEIGHT) {
if (thresh->thresholded[y+h+2][i] == GREEN)
count++;
else if (thresh->thresholded[y+h+2][i] == WHITE)
count-=3;
counter++;
} else return;
}
if (CROSSDEBUG) {
cout << "Have a candidate white blob " << x << " " << y << endl;
}
const int HORIZONCHECK = 15;
// make sure we aren't too close to the horizon
if (y - HORIZONCHECK < field->horizonAt(x) && field->horizonAt(x) > 0) return;
if (y - HORIZONCHECK < field->horizonAt(x+w) && field->horizonAt(x+w) > 0) return;
// if we pass the basic threshold then make sure it isn't a line
if (count > (float)counter * greenThreshold) {
// first make sure this isn't really a line
point <int> plumbLineTop, plumbLineBottom, line1start, line1end;
plumbLineTop.x = x + w / 2; plumbLineTop.y = y;
plumbLineBottom.x = x; plumbLineBottom.y = y + h;
const vector <boost::shared_ptr<VisualLine> >* lines = vision->fieldLines->getLines();
for (vector <boost::shared_ptr<VisualLine> >::const_iterator k = lines->begin();
k != lines->end(); k++) {
pair<int, int> foo = Utility::
plumbIntersection(plumbLineTop, plumbLineBottom,
(*k)->start, (*k)->end);
if (foo.first != NO_INTERSECTION && foo.second != NO_INTERSECTION) {
if (CROSSDEBUG)
cout << "Throwing out blob that is part of a line" << endl;
return;
}
}
// Is the cross white enough? At least half the pixels must be white.
if (!rightBlobColor(b, 0.5f)) {
if (CROSSDEBUG) {
cout << "Tossing a blob for not being white enough " << endl;
}
return;
}
// passed all of our current sanity checks
if (CROSSDEBUG) {
cout << "Found a cross " << endl;
b.printBlob();
}
// Make sure we don't have more than one candidate cross. Note: we
// actually can see two crosses at some places on the field, but for
// now we just will ID one of them.
// TODO: allow seeing two crosses - but must test whether they are
// correctly aligned with each other, etc.
if (vision->cross->getWidth() > 0) {
if (w * h > vision->cross->getWidth() * vision->cross->getHeight()) {
vision->cross->updateCross(&b);
if (CROSSDEBUG) {
cout << "Larger than previous cross." << endl;
}
} else {
if (CROSSDEBUG) {
cout << "Threw out extra cross - smaller " << endl;
}
}
} else {
vision->cross->updateCross(&b);
}
}
}
int Ball::roundness(Blob b)
{
const int IMAGE_EDGE = 3;
const int BIG_ENOUGH = 4;
const int TOO_BIG_TO_CHECK = 20;
const int WIDTH_AT_SCREEN_BOTTOM = 15;
const float RATIO_TO_INT = 10.0f;
const float CORNER_CHUNK_DIV = 6.0f;
int w = b.width();
int h = b.height();
int x = b.getLeftTopX();
int y = b.getLeftTopY();
float ratio = static_cast<float>(w) / static_cast<float>(h);
int r = 10;
if ((h < SMALLBALLDIM && w < SMALLBALLDIM && ratio > BALLTOOTHIN &&
ratio < BALLTOOFAT)) {
} else if (ratio > THINBALL && ratio < FATBALL) {
} else if (y + h > IMAGE_HEIGHT - IMAGE_EDGE || x == 0 || (x + w) >
IMAGE_WIDTH - 2 || y == 0) {
if (BALLDEBUG)
cout << "Checking ratio on occluded ball: " << ratio << endl;
// we're on an edge so allow for streching
if (h > BIG_ENOUGH && w > BIG_ENOUGH && (y + h > IMAGE_HEIGHT - 2 ||
y == 0) &&
ratio < MIDFAT && ratio > 1) {
// then sides
} else if (h > BIG_ENOUGH && w > BIG_ENOUGH
&& (x == 0 || x + w > IMAGE_WIDTH - 2)
&& ratio > MIDTHIN && ratio < 1) {
} else if ((h > TOO_BIG_TO_CHECK || w > TOO_BIG_TO_CHECK)
&& (ratio > OCCLUDEDTHIN && ratio < OCCLUDEDFAT) ) {
// when we have big slivers then allow for extra
} else if (b.getLeftBottomY() > IMAGE_HEIGHT - IMAGE_EDGE &&
w > WIDTH_AT_SCREEN_BOTTOM) {
// the bottom is a really special case
} else {
if (BALLDEBUG)
//cout << "Screening for ratios" << endl;
return BAD_VALUE;
}
} else {
if (BALLDEBUG) {
drawBlob(b, BLACK);
printBlob(b);
cout << "Screening for ratios " << ratio << endl;
}
return BAD_VALUE;
}
if (ratio < 1.0) {
int offRat = ROUND2((1.0f - ratio) * RATIO_TO_INT);
r -= offRat;
} else {
int offRat = ROUND2((1.0f - 1.0f/ratio) * RATIO_TO_INT);
r -= offRat;
}
if (w * h > SMALLBALL) {
// now make some scans through the blob - horizontal, vertical, diagonal
int pix;
int goodPix = 0, badPix = 0;
if (y + h > IMAGE_HEIGHT - IMAGE_EDGE || x == 0 || (x + w) >
IMAGE_WIDTH - 2 || y == 0) {
} else {
// we're in the screen
int d = ROUND2(static_cast<float>(std::max(w, h)) /
CORNER_CHUNK_DIV);
int d3 = min(w, h);
for (int i = 0; i < d3; i++) {
pix = thresh->thresholded[y+i][x+i];
if (i < d || (i > d3 - d)) {
if (pix == ORANGE || pix == ORANGERED) {
//drawPoint(x+i, y+i, BLACK);
badPix++;
}
else
goodPix++;
} else {
if (pix == ORANGE || pix == ORANGERED || pix == ORANGEYELLOW)
goodPix++;
else if (pix != GREY) {
badPix++;
//drawPoint(x+i, y+i, PINK);
}
}
pix = thresh->thresholded[y+i][x+w-i];
if (i < d || (i > d3 - d)) {
if (pix == ORANGE || pix == ORANGERED) {
//drawPoint(x+w-i, y+i, BLACK);
badPix++;
}
else
goodPix++;
} else if (pix == ORANGE || pix == ORANGERED ||
pix == ORANGEYELLOW)
goodPix++;
else if (pix != GREY) {
badPix++;
//drawPoint(x+w-i, y+i, BLACK);
}
}
//cout << "here" << endl;
for (int i = 0; i < h; i++) {
pix = thresh->thresholded[y+i][x + w/2];
//drawPoint(x + w/2, y+i, BLACK);
if (pix == ORANGE || pix == ORANGERED || pix == ORANGEYELLOW) {
goodPix++;
} else if (pix != GREY)
badPix++;
}
}
for (int i = 0; i < w; i++) {
pix = thresh->thresholded[y+h/2][x + i];
//drawPoint(x+i, y+h/2, BLACK);
if (pix == ORANGE || pix == ORANGERED || pix == ORANGEYELLOW) {
goodPix++;
} else if (pix != GREY)
badPix++;
}
if (BALLDEBUG)
cout << "Roundness: Good " << goodPix << " " << badPix << endl;
// if more than 20% or so of our pixels tested are bad, then we toss it out
if (goodPix < badPix * 5) {
if (BALLDEBUG)
cout << "Screening for bad roundness" << endl;
return BAD_VALUE;
}
}
return 0;
}