/**
* Prints out machine learning information, such as the gradient, %white,
* %red, %blue and the sonar difference percentage.
*/
void BayesianRobotValidator::printMachineLearningData(
VisionFrame &frame, const Fovea &saliency,
const std::vector<PossibleRobot> &possibleRobots) {
for (unsigned int i = 0; i < possibleRobots.size(); ++i) {
const PossibleRobot &robot = possibleRobots[i];
unsigned int white = 0;
unsigned int robotRed = 0;
unsigned int robotBlue = 0;
unsigned int total = 0;
for (int col = robot.region.a.x(); col < robot.region.b.x(); ++col) {
for (int row = robot.region.a.y(); row < robot.region.b.y(); ++row) {
Colour c = saliency.colour(col, row);
if (c == cWHITE) {
++white;
} else if (c == cTEAM_HOME) {
++robotRed;
} else if (c == cTEAM_AWAY) {
++robotBlue;
}
++total;
}
}
double gradient = (double)robot.region.height() / robot.region.width();
double whitePercentage = (double) white / (double) total;
double redPercentage = (double) robotRed / (double) total;
double bluePercentage = (double) robotBlue / (double) total;
double sonarDifferencePercentage = (double) robot.sonarDifference / (double) robot.feet.distance();
if (robot.sonarDifference == -1) {
sonarDifferencePercentage = -1;
}
printf("%0.4f,%0.4f,%0.4f,%0.4f,%0.4f,\n", gradient, whitePercentage, redPercentage, bluePercentage, sonarDifferencePercentage);
}
}
double PossibleRobot::getPercentageOfColours(const Fovea &saliency, const std::set<Colour> &colours) const {
unsigned int colourTotal = 0;
unsigned int total = 0;
//TODO: figure out why we should ever have this outside the bounds.
//Bandaid fix for competition. Gross Gross Gross, need to find cause of this.
int rows = (saliency.top) ? TOP_SALIENCY_ROWS : BOT_SALIENCY_ROWS;
int cols = (saliency.top) ? TOP_SALIENCY_COLS : BOT_SALIENCY_COLS;
if (region.a.x() < 0 || region.a.x() > cols || region.b.x() < 0 || region.b.x() > cols) {
return 0;
}
if (region.a.y() < 0 || region.a.y() > rows || region.b.y() < 0 || region.b.y() > rows) {
return 0;
}
for (int col = region.a.x(); col < region.b.x(); ++col) {
for (int row = region.a.y(); row < region.b.y(); ++row) {
Colour pixelColour = saliency.colour(col, row);
if (colours.count(pixelColour)) {
++colourTotal;
}
++total;
}
}
return (double)colourTotal / (double)total;
}
/**
* Look in the region around about where the jersey would be (25% - 75% height)
* and if there is more then half of a jersey colour on the left or right of the
* region expand the box outwards.
*/
std::vector<PossibleRobot> RobotWidener::expandJerseys(VisionFrame &frame,
const Fovea &saliency, std::vector<PossibleRobot> possibleRobots) const {
std::vector<PossibleRobot> robots;
for (unsigned int i = 0; i < possibleRobots.size(); ++i) {
PossibleRobot robot = possibleRobots[i];
int leftCol = robot.region.a.x();
int rightCol = robot.region.b.x();
int startRow = robot.region.a.y() + robot.region.height() / 4;
int endRow = robot.region.b.y() - robot.region.height() / 4;
int regionHeight = endRow - startRow;
int previousCol = 0;
if (i > 0) {
previousCol = possibleRobots[i - 1].region.b.x();
}
//do left
int col = leftCol;
int type = 0; //0:none 1: blue 2: red;
while (col > previousCol) {
int numberRed = 0;
int numberBlue = 0;
for (int row = startRow; row < endRow; ++row) {
Colour c = saliency.colour(col, row);
if (c == cROBOT_BLUE) {
++numberBlue;
} else if (c == cROBOT_RED) {
++numberRed;
}
}
if (numberBlue * 2 > regionHeight && (type == 0 || type == 1)) {
type = 1;
--col;
} else if (numberRed * 2 > regionHeight && (type == 0 || type == 2)) {
type = 2;
--col;
} else {
break;
}
}
if (col < leftCol) {
robot.region.a.x() = col;
}
int nextCol = saliency.bb.width();
if (i + 1 < possibleRobots.size()) {
nextCol = possibleRobots[i + 1].region.a.x();
}
col = rightCol;
while (col < saliency.bb.width()) {
int numberRed = 0;
int numberBlue = 0;
for (int row = startRow; row < endRow; ++row) {
Colour c = saliency.colour(col, row);
if (c == cROBOT_BLUE) {
++numberBlue;
} else if (c == cROBOT_RED) {
++numberRed;
}
}
if (numberBlue * 2 > regionHeight && (type == 0 || type == 1)) {
type = 1;
++col;
} else if (numberRed * 2 > regionHeight && (type == 0 || type == 2)) {
type = 2;
++col;
} else {
break;
}
}
if (col > rightCol) {
robot.region.b.x() = col;
}
robots.push_back(robot);
}
return robots;
}
// Finds the distance to post
// Tunes the BBox using higher resolution foveas
RRCoord GoalDetection::findDistanceToPost(VisionFrame &frame,
const Fovea& fovea,
BBox& goal,
int numPosts,
PostInfo& p) {
bool trustDistance = true;
float differenceThreshold = 1.7;
// **** Try finding the distance using kinematics of the base ****
findBaseOfPost(frame, goal);
const CameraToRR *convRR = &frame.cameraToRR;
Point base = Point((goal.a.x()+goal.b.x())/2, goal.b.y());
RRCoord rr = convRR->convertToRR(base, false);
float kdistance = rr.distance();
// **** Try using the width the find the distance ****
// Calculate width distance at 3 points and take median
std::map<float, float> distances;
for (float h = 0.89; h < 1; h += 0.05) {
float d = widthDistanceToPost(frame, goal, h);
distances.insert(std::make_pair(d,h));
}
std::map<float, float>::iterator i = distances.begin();
if (distances.size() > 1) ++i;
float wdistance = widthDistanceToPost(frame, goal, i->second, true);
// **** Decide which distance to use ****
// Kinematics is usually more accurate, so use it unless we know it's wrong
// If post ends at bottom of image, probably not the bottom, so use width
bool width = false;
if (fovea.top && goal.b.y() > (TOP_IMAGE_ROWS-10)) {
width = true;
}
// If still yellow below the base, probably missed the bottom, so use width
// Only for 1 post though
if (numPosts == 1) {
Point fTop = fovea.mapImageToFovea(goal.a);
Point fBot = fovea.mapImageToFovea(goal.b);
const YHistogram &yhistogram = fovea.yhistogram;
int height = (fBot.y() - fTop.y()) / 4; // set max scan size
int endPoint = std::min(fovea.bb.height(), fBot.y() + height);
int noYellow = fBot.y();
for (int i = fBot.y(); i < endPoint; ++i) {
int current = yhistogram._counts[i][cGOAL_YELLOW];
if (current < 10) noYellow = i;
}
if (noYellow == fBot.y()) trustDistance = false;
}
// Decided to use width distance
if (width) {
if (wdistance < 1500) rr.distance() = wdistance;
else trustDistance = false;
// differenceThreshold = 1.7;
}
// Check that kinematics and width distances are similar
else if (kdistance < 2500) {
}
else if (((kdistance / wdistance) > differenceThreshold) ||
((wdistance / kdistance) > differenceThreshold)) {
trustDistance = false;
}
// Check distance is reasonable
if (rr.distance() > 12000) {
trustDistance = false;
rr.distance() = 12000;
}
// Set variables in PostInfo
p.rr = rr;
p.kDistance = kdistance;
p.wDistance = wdistance;
p.trustDistance = trustDistance;
p.imageCoords = goal;
return rr;
}
void GoalDetection::performSanityChecks(const Fovea &fovea,
const VisionFrame &frame,
std::vector<BBox> ®ions) {
std::vector<BBox> candidates (regions);
regions.clear();
std::vector<BBox>::iterator it;
for (it = candidates.begin(); it != candidates.end(); ++it) {
// Check that the middle and the bottom 75% for edges
// Only need 1 strong edge in each to be good
int middle = (it->a.y() + it->b.y()) / 2;
int bottom = it->a.y() + (it->b.y() - it->a.y()) * 0.75;
bool keepM = false;
bool keepB = false;
for (int col = it->a.x(); col <= it->b.x(); ++col) {
// Check middle
Point edge = fovea.edge(col, middle);
float magnitude = (edge.x() * edge.x()) + (edge.y() * edge.y());
if (magnitude > MIN_EDGE_THRESHOLD) keepM = true;
// Check bottom
edge = fovea.edge(col, bottom);
magnitude = (edge.x() * edge.x()) + (edge.y() * edge.y());
if (magnitude > MIN_EDGE_THRESHOLD) keepB = true;
}
if (!keepM) {
//std::cout << "throwing away since no keepM" << std::endl;
continue ;
}
if (!keepB) {
//std::cout << "throwing away since no keepB" << std::endl;
continue;
}
// Check % of colour in goal post - ie compare length and colour
float length = it->b.y() - it->a.y();
int centre = (it->a.x() + it->b.x()) / 2;
float numColourPixels = 0;
for (int row = it->a.y(); row < it->b.y(); ++row) {
if (fovea.colour(centre, row) == cGOAL_YELLOW) {
++numColourPixels;
}
}
if ((numColourPixels / length) < COLOUR_RATIO_THRESHOLD) {
//std::cout << "throwing away since not enough colour" << std::endl;
continue;
}
// Check bottom of goal post is below field edge
Point fieldEdge = fovea.mapFoveaToImage(Point(centre, 0));
int fieldEdgeY = 0;
if (fovea.top) {
fieldEdgeY = frame.topStartScanCoords[fieldEdge.x()];
} else {
fieldEdgeY = frame.botStartScanCoords[fieldEdge.x()];
}
fieldEdge.y() = std::max(fieldEdge.y(), fieldEdgeY);
fieldEdge = fovea.mapImageToFovea(fieldEdge);
if (fieldEdge.y() > it->b.y()) {
//std::cout << "throwing away since above field edge" << std::endl;
continue;
}
regions.push_back(*it);
}
// If more than 2 goals, things have gone wrong, so panic
if (regions.size() > 2) regions.clear();
}
void GoalDetection::findGoals(
VisionFrame &frame,
const Fovea &fovea,
unsigned int *seed)
{
goalFoveas.clear();
std::vector<PostInfo> posts;
// Find candidate regions to look for goals
std::vector<BBox> regions;
findCandidateRegions(fovea, regions);
// Sanity check goals to eliminate crappy ones
performSanityChecks(fovea, frame, regions);
// Convert into goal posts
std::vector<BBox>::const_iterator it;
for (it = regions.begin(); it != regions.end(); ++it) {
PostInfo p;
BBox bb (fovea.mapFoveaToImage(it->a), fovea.mapFoveaToImage(it->b));
// If we are in the top camera, ensure we don't round the BBox into the bottom camera
if (fovea.top && bb.b.y() == TOP_IMAGE_ROWS) {
bb.b.y() = (TOP_IMAGE_ROWS-1);
}
// Calculate best distance to goal - also fine tunes the BBox
// Fine tunes and sets the BBox
// Sets rr, kDistance, wDistance, trustDistance on variable p
RRCoord rr = findDistanceToPost(frame, fovea, bb, regions.size(), p);
//if (rr.distance() > 2000) p.trustDistance = false;
// Work out left and right goals
// Sets type and dir
determineLeftRightPost(fovea, regions, *it, p);
// Save goal post
posts.push_back(p);
}
// Sanity check the LHS / RHS of goals
if (posts.size() == 2) {
PostInfo pLeft, pRight;
if (posts[0].type == PostInfo::pLeft) {
pLeft = posts[0];
pRight = posts[1];
} else {
pLeft = posts[1];
pRight = posts[0];
}
if (pLeft.dir == PostInfo::pToLeftOf &&
pLeft.rr.distance() >= pRight.rr.distance()) {
posts[0].dir = PostInfo::pUnknown;
posts[1].dir = PostInfo::pUnknown;
} else if (pLeft.dir == PostInfo::pToRightOf &&
pRight.rr.distance() >= pLeft.rr.distance()) {
posts[0].dir = PostInfo::pUnknown;
posts[1].dir = PostInfo::pUnknown;
}
}
// Merge top and bottom cameras
const float headingThreshold = DEG2RAD(7);
if (frame.posts.empty()) {
frame.posts = posts;
} else if (fovea.top && posts.size() > 0 && frame.posts.size() == 1) {
for (unsigned int i = 0; i < posts.size(); i++) {
if (abs(posts[i].rr.heading() - frame.posts[0].rr.heading())
> headingThreshold) {
frame.posts.push_back(posts[i]);
}
}
// Ensure if we have 2 posts, they are left and right
if (frame.posts.size() == 2) {
if (frame.posts[0].rr.heading() < frame.posts[1].rr.heading()) {
frame.posts[0].type = PostInfo::pRight;
frame.posts[1].type = PostInfo::pLeft;
} else {
frame.posts[0].type = PostInfo::pLeft;
frame.posts[1].type = PostInfo::pRight;
}
}
}
}
// Uses colour to make rough bounding boxes
void GoalDetection::findCandidateRegions(const Fovea &fovea,
std::vector<BBox> ®ions) {
const YHistogram &yhistogram = fovea.yhistogram;
const XHistogram &xhistogram = fovea.xhistogram;
std::vector<BBox> candidates(regions);
// Find rough vertical bounding boxes for goal posts
int start = 0;
for (int i = 0; i < xhistogram.size; ++i) {
if (xhistogram._counts[i][cGOAL_YELLOW] > MIN_COLOUR_THRESHOLD) {
if (start == 0) {
start = i;
}
} else if (start != 0) {
// Save region
candidates.push_back(BBox(Point(start, 0), Point(i, fovea.bb.height())));
start = 0;
}
}
// Find roughly where the crossbar is
start = 0;
int max = 0;
int maxI = 0;
int prev = 0;
for (int i = 0; i < yhistogram.size; ++i) {
int current = yhistogram._counts[i][cGOAL_YELLOW];
if ((current > max) &&
(current > MIN_H_COLOUR_THRESHOLD) &&
(current > (5*prev))) {
max = current;
maxI = i;
}
prev = current;
}
// Now that we have regions, find the bottom
std::vector<BBox> candidates2 (candidates);
candidates.clear();
std::vector<BBox>::const_iterator it;
for (it = candidates2.begin(); it != candidates2.end(); ++it) {
int start = it->a.x();
int end = it->b.x();
// Identified a region horizontally (start-end), now clip it vertically
int middle = (start + end) /2;
// Find longest run of colour
int s = 0;
int length = 0;
int bestS = 0;
int bestLength = 0;
int lastYellow = 0;
bool skip = false;
for (int j = 0; j < fovea.bb.height(); ++j) {
Colour c = fovea.colour(middle, j);
// Keep track of last yellow
if (c == cGOAL_YELLOW && s != 0) lastYellow = j;
// If yellow, start counting at first
if (c == cGOAL_YELLOW && s == 0) {
s = j;
length = 0;
}
// If not green or background keep tracking run
//if (c != cBACKGROUND && c != cFIELD_GREEN) {
if (c != cFIELD_GREEN) {
++length;
if (skip) ++length;
}
// If not, maybe end a run
else if (s != 0) {
// If gap is just 1 pixel (not green), don't end
if (j <= (s+length) && c != cFIELD_GREEN) {
skip = true;
continue;
}
// Otherwise end region
if (length > bestLength) {
bestS = s;
bestLength = length;
}
s = 0;
length = 0;
}
skip = false;
}
// Case for bottom of image
if (length > bestLength) {
bestS = s;
bestLength = length;
}
// Don't over extend the bottom way past the last yellow pixel we saw
int base = std::min(lastYellow, bestS+bestLength);
// Save region
if (maxI != 0) {
candidates.push_back(BBox(Point(start, maxI), Point(end, base)));
} else {
candidates.push_back(BBox(Point(start, bestS), Point(end, base)));
}
start = 0;
}
regions = candidates;
}
