/* Are two blobs close enough that they might actually be the same?
Note: if the area between is bigger than one of the blobs then can it?
*/
bool Robots::closeEnough(int i, int j) {
Blob a = blobs->get(i);
Blob b = blobs->get(j);
int width1 = a.width();
int width2 = b.width();
int height1 = a.height();
int height2 = b.height();
int bigWidth = max(width1, width2);
int bigHeight = max(height1, height2);
int dist1 = distance(a.getLeft(), a.getRight(), b.getLeft(), b.getRight());
int dist2 = distance(a.getTop(), a.getBottom(), b.getTop(), b.getBottom());
if (a.getRight() <= 0 || b.getRight() <= 0) {
return false;
}
if (dist1 > 5 && dist2 > 5) {
return false;
}
if (dist1 < bigHeight && dist2 < bigWidth) {
return true;
}
return false;
}
/* Given a viable uniform blob, add all vertically
alligned white blobs to create an "area of interest" for where to more
rigorously determine where a robot is
@param robotBlob potential uniform blob
@return blob indicating an area of interest for robot detection
*/
Blob Robots::createAreaOfInterest(Blob robotBlob) {
int left = robotBlob.getLeft();
int right = robotBlob.getRight();
int top = robotBlob.getTop();
int bottom = robotBlob.getBottom();
int height = robotBlob.height();
int width = right-left;
for (int i = 0; i < whiteBlobs->number(); i++){
if (whiteBlobs->get(i).isAligned(robotBlob)) {
robotBlob.merge(whiteBlobs->get(i));
}
}
//places constraints the on the area of interest with knowledge of the relative
//ratio of the dimensions of the robot to the dimensions of its uniform
//NOTE: this ratio could be more accurately defined
if (robotBlob.getLeft() < left-width) robotBlob.setLeft(left-width);
if (robotBlob.getRight() > right+width) robotBlob.setRight(right+width);
if (robotBlob.getTop() < top-5*height) robotBlob.setLeft(top-5*height);
if (robotBlob.getBottom() > bottom+6*height) robotBlob.setBottom(bottom+6*height);
return robotBlob;
}
/* When we are looking down, the shadowed carpet often has lots of Navy.
Make sure we aren't just looking at carpet
*/
bool Robots::notGreen(Blob candidate) {
int bottom = candidate.getBottom();
int top = candidate.getTop();
int left = candidate.getLeft();
int right = candidate.getRight();
int area = candidate.width() * candidate.height() / 5;
int greens = 0;
for (int i = left; i < right; i++) {
for (int j = top; j < bottom; j++) {
if (Utility::isGreen(thresh->getThresholded(j, i))) {
greens++;
if (greens > area) {
return true;
}
}
}
}
return false;
}
/* We have a swatch of color. Let's make sure that there is some white
around somewhere as befits our robots.
*/
bool Robots::noWhite(Blob b) {
const int MINWHITE = 5;
int left = b.getLeft(), right = b.getRight();
int top = b.getTop(), bottom = b.getBottom();
int width = b.width();
int tops, bottoms;
for (int i = 1; i < b.height(); i++) {
tops = 0; bottoms = 0;
for (int x = left; x <= right; x++) {
if (top - i >= 0 && Utility::isWhite(thresh->getThresholded(top - i,x)))
tops++;
if (bottom + i < IMAGE_HEIGHT &&
Utility::isWhite(thresh->getThresholded(bottom+i,x)))
bottoms++;
if (tops > width / 2 || tops == width) return false;
if (bottoms > width / 2 || tops == width) return false;
}
}
return true;
}
/* Since the white blob check catches a lot of extra stuff like lines,
we need to check a bit more carefully.
*/
bool Robots::whiteAbove(Blob candidate) {
int top = candidate.getTop();
int height = candidate.height();
int scanline = top - height;
for (int y = scanline; y > 0 && y > scanline - height; y -= 3) {
int white = 0;
int green = 0;
for (int x = candidate.getLeft(); x < candidate.getRight(); x++) {
if (Utility::isWhite(thresh->getThresholded(y, x))) {
white++;
} else if (Utility::isGreen(thresh->getThresholded(y, x))) {
green++;
}
}
if (green > candidate.width() / 2 && white == 0) {
return false;
}
if (white > candidate.width() / 4) {
return true;
}
}
return false;
}
/* Since the white blob check catches a lot of extra stuff like lines,
we need to check a bit more carefully.
*/
bool Robots::whiteBelow(Blob candidate) {
int bottom = candidate.getBottom();
int height = candidate.height();
int scanline = bottom + height;
for (int y = scanline; y < IMAGE_HEIGHT && y < scanline + height; y += 3) {
int white = 0;
int green = 0;
for (int x = candidate.getLeft(); x < candidate.getRight(); x++) {
if (Utility::isWhite(thresh->getThresholded(y, x))) {
white++;
} else if (Utility::isGreen(thresh->getThresholded(y, x))) {
green++;
}
}
if (green > candidate.width() / 2 && white == 0) {
return false;
}
if (white > candidate.width() / 4) {
return true;
}
}
return false;
}
/* We have two blobs that are reasonably close. See if they should be
merged.
*/
void Robots::checkMerge(int i, int j) {
Blob a = blobs->get(i);
Blob b = blobs->get(j);
if (debugRobots) {
cout << endl << endl << "Checking merge" << endl;
printBlob(a);
printBlob(b);
}
int dist1 = distance(a.getLeft(), a.getRight(), b.getLeft(), b.getRight());
int dist2 = distance(a.getTop(), a.getBottom(), b.getTop(), b.getBottom());
int green = 0;
int col = 0;
int miss = 0;
// top to bottom defines the y region between the two
int top = max(a.getTop(), b.getTop());
int bottom = min(a.getBottom(), b.getBottom());
int midx = (a.getLeft() + a.getRight()) / 2;
// left to right defines the x region between the two
int left = a.getRight();
int right = b.getLeft();
int midy = (a.getTop() + a.getBottom()) / 2;
int ii;
// check if they overlap in either dimension
if (left > right) {
// they overlap in x so we'll swap left and right
int temp = left;
left = right;
right = temp;
}
if (top > bottom) {
int temp = top;
top = bottom;
bottom = temp;
}
int width = right - left + 1;
int height = bottom - top + 1;
int area = max(10, width * height / 27);
int stripe = max(width, height);
for (int x = left; x < right; x+=3) {
for (int y = top; y < bottom; y+=3) {
if (debugRobots) {
vision->drawPoint(x, y, MAROON);
}
if (Utility::colorsEqual(thresh->getThresholded(y, x), color)) {
col++;
if (col > area || col > stripe) {
blobs->mergeBlobs(i, j);
if (debugRobots) {
cout << "Merge" << endl;
}
return;
}
} else if (Utility::isGreen(thresh->getThresholded(y, x))) {
green++;
if (green > 5) {
return;
}
} else if (Utility::isWhite(thresh->getThresholded(y, x))) {
miss++;
if (miss > area / 9 || miss > stripe) {
return;
}
}
}
}
if (col > min(width, height) && green < 3) {
blobs->mergeBlobs(i, j);
if (debugRobots) {
cout << "Merge" << endl;
}
}
}
bool Robots::sanityChecks(int index) {
Blob candidate = blobs->get(index);
//thresholds for smallest allowable blob
const int blobHeightMin = 3;
const int blobAreaMin = 15;
int height = candidate.height();
int bottom = candidate.getBottom();
if (candidate.getRight() > 0) {
//blobs must be large enough
if (candidate.height()*candidate.width() < blobAreaMin) {
if (debugRobots){
cout << "Blob area was too small" << endl;
}
return false;
}
// uniforms should be wider than they are tall
if (candidate.height() > candidate.width()) {
if (debugRobots){
cout << "Blob was taller than it was wide" << endl;
}
return false;
}
//a robot cannot be inside another robot
for (int i = 0; i < blobs->number(); i++) {
if (i == index) continue;
if (blobs->blobsOverlap(index, i)) {
blobs->mergeBlobs(index, i);
if (debugRobots){
cout << "Blob was inside other robot" << endl;
}
return false;
}
}
// there ought to be some white below the uniform
if (bottom < IMAGE_HEIGHT - 10 &&
!checkWhiteAllignment(candidate)) {
if (debugRobots) {
cout << "Bad robot from white alignment check" << endl;
}
return false;
}
// the last check was pretty general, let's improve
if (candidate.getBottom() < IMAGE_HEIGHT - candidate.height() * 2
&& !whiteBelow(candidate)) {
if (debugRobots) {
cout << "Got rid for lack of white below" << endl;
}
return false;
}
if (candidate.getTop() > candidate.height() * 2
&& !whiteAbove(candidate)) {
if (debugRobots) {
cout << "Got rid for lack of white above" << endl;
}
return false;
}
// for some blobs we check even harder for white
if (height < 2 * blobHeightMin && noWhite(candidate)) {
if (debugRobots) {
cout << "Got rid of small one for white" << endl;
}
return false;
}
if (color == NAVY_BIT && vision->pose->getHorizonY(0) < 0 &&
notGreen(candidate)) {
if (debugRobots){
cout << "Got rid for pose check" << endl;
}
return false;
}
return true;
}
return false;
}
//correct the area of interest to a reasonable estimate of the robot
Blob Robots::correctBlob(Blob area){
int left = max(0, area.getLeft());
int right = min(area.getRight(), IMAGE_WIDTH);
int top = max(0, area.getTop());
int bottom = min(area.getBottom(), IMAGE_HEIGHT);
int width = right - left;
int height = bottom - top;
int nonRobot = 0;
//uncomment for debugging of corrections. Drawn rect is blob before correction
//vision->drawRect(left,top, width, height, WHITE);
//correct left side
for (int x = left; x < left + (width/2); x++) {
for (int y = top; y < bottom; y++) {
if (Utility::isGreen(thresh->getThresholded(y,x)) ||
Utility::isOrange(thresh->getThresholded(y,x))) {
nonRobot++;
}
if (nonRobot > .4*height) {
area.setLeft(x+1);
nonRobot = -1;
break;
}
}
if (nonRobot != -1) break;
nonRobot = 0;
}
//correct right side
for (int x = right; x > left + (width/2); x--) {
for (int y = top; y < bottom; y++) {
if (Utility::isGreen(thresh->getThresholded(y,x)) ||
Utility::isOrange(thresh->getThresholded(y,x))) {
nonRobot++;
}
if (nonRobot > .4*height) {
area.setRight(x-1);
nonRobot = -1;
break;
}
}
if (nonRobot != -1) break;
nonRobot = 0;
}
//correct top
for (int y = top; y < top + (height/2); y++) {
for (int x = left; x < right; x++) {
if (Utility::isGreen(thresh->getThresholded(y,x)) ||
Utility::isOrange(thresh->getThresholded(y,x))) {
nonRobot++;
};
if (nonRobot > .4*width) {
area.setTop(y+1);
nonRobot = -1;
break;
}
}
if (nonRobot != -1) break;
nonRobot = 0;
}
//correct bottom
for (int y = bottom; y > top + (height/2); y--) {
for (int x = left; x < right; x++) {
if (Utility::isGreen(thresh->getThresholded(y,x)) ||
Utility::isOrange(thresh->getThresholded(y,x))) {
nonRobot++;
}
if (nonRobot > .4*width) {
area.setBottom(y-1);
nonRobot = -1;
break;
}
}
if (nonRobot != -1) break;
nonRobot = 0;
}
return area;
}
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;
}
/* Returns true is the blob abuts any image edge
@param b the blob to check
@return true when the blob is near an edge
*/
bool Ball::nearEdge(Blob b) {
return nearImageEdgeX(b.getLeft(), 1) || nearImageEdgeX(b.getRight(), 1) ||
nearImageEdgeY(b.getTop(), 1) || nearImageEdgeY(b.getBottom(), 2);
}