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