Skip to content

Instantly share code, notes, and snippets.

Created Jun 16, 2016
Embed
What would you like to do?
// Detect all edges
void detectEdges()
{
// Set first and last laser point as undefined
world->currentLaserPoints[0].type = UndefinedEdge;
world->currentLaserPoints[world->currentLaserPoints.size()-1].type = UndefinedEdge;
// Reset detected edges to only the first and last laser point
detectedEdges.clear();
detectedEdges.emplace_back(world->currentLaserPoints[0]);
detectedEdges.emplace_back(world->currentLaserPoints[world->currentLaserPoints.size()-1]);
// Initialize the collection of assumed sharp corners
std::map<int, double> assumedSharpCorners;
assumedSharpCorners.clear();
// Initialize the collection of assumed hollow corners
std::map<int, double> assumedHollowCorners;
assumedHollowCorners.clear();
// For each laser point
for (int i = 1; i < world->currentLaserPoints.size()-1; i++)
{
// If the point is not feasible
if (isSingularPoint(i) || !isValidPoint(i))
{
// Continue to next point
continue;
}
// If the point is feasible
else
{
// Check for range jumps before or after the point.
// Update the type and store the laser point accordingly
if (world->currentLaserPoints[i].range - world->currentLaserPoints[i+1].range > RANGE_JUMP_THRESHOLD)
{
if (isValidPoint(i+1))
{
world->currentLaserPoints[i].type = UndefinedEdge;
detectedEdges.emplace_back(world->currentLaserPoints[i]);
}
else {
world->currentLaserPoints[i].type = SharpCorner;
detectedEdges.emplace_back(world->currentLaserPoints[i]);
}
continue;
}
else if (world->currentLaserPoints[i+1].range - world->currentLaserPoints[i].range > RANGE_JUMP_THRESHOLD)
{
world->currentLaserPoints[i].type = SharpCorner;
detectedEdges.emplace_back(world->currentLaserPoints[i]);
continue;
}
else if (world->currentLaserPoints[i-1].range - world->currentLaserPoints[i].range > RANGE_JUMP_THRESHOLD)
{
world->currentLaserPoints[i].type = SharpCorner;
detectedEdges.emplace_back(world->currentLaserPoints[i]);
continue;
}
else if (world->currentLaserPoints[i].range - world->currentLaserPoints[i-1].range > RANGE_JUMP_THRESHOLD)
{
if (isValidPoint(i-1))
{
world->currentLaserPoints[i].type = UndefinedEdge;
detectedEdges.emplace_back(world->currentLaserPoints[i]);
}
else {
world->currentLaserPoints[i].type = SharpCorner;
detectedEdges.emplace_back(world->currentLaserPoints[i]);
}
continue;
}
// If there are no range jumps before or after the point
else if ( i >= EDGE_NEIGHBOURHOOD && i <= world->currentLaserPoints.size() - EDGE_NEIGHBOURHOOD)
{
// Initialize variables which are going to define a neighbourhood of points around the current point
LaserPoint currentPoint = world->currentLaserPoints[i];
LaserPoint endPoint1;
LaserPoint endPoint2;
// Assume the laser point is a corner
bool isValidCorner = true;
// Loop over the neighbourhood of hte points
for (int j = 1; j <= EDGE_NEIGHBOURHOOD; j++)
{
// Store distances to the current laser point
double distance1 = getDistance(world->currentLaserPoints[i-j], world->currentLaserPoints[i-j+1]);
double distance2 = getDistance(world->currentLaserPoints[i+j], world->currentLaserPoints[i+j-1]);
// If there is a range jump in the neighbourhood before the current point
if (distance1 > RANGE_JUMP_THRESHOLD)
{
// Assume the current laser point is an invalid corner
isValidCorner = false;
}
// If the neighbourhood is sufficiently connected
else
{
// Set end point of the neighbourhood before the current point
endPoint1 = world->currentLaserPoints[i-j];
}
// If there is a range jump in the neighbourhood after the current point
if (distance2 > RANGE_JUMP_THRESHOLD)
{
// Assume the current laser point is an invalid corner
isValidCorner = false;
}
// If the neighbourhood is sufficiently connected
else
{
// Set end point of the neighbourhood after the current point
endPoint2 = world->currentLaserPoints[i+j];
}
}
// If the laser point could be a valid corner
if (isValidCorner)
{
// Define variables to help determine the angle of the point using the cross product
Point vector1 = Point(endPoint1.x - currentPoint.x, endPoint1.y - currentPoint.y);
Point vector2 = Point(endPoint2.x - currentPoint.x, endPoint2.y - currentPoint.y);
double crossLength = vector1.x * vector2.y - vector1.y * vector2.x;
double length1 = getDistance(world->currentLaserPoints[i], world->currentLaserPoints[i-EDGE_NEIGHBOURHOOD]);
double length2 = getDistance(world->currentLaserPoints[i], world->currentLaserPoints[i+EDGE_NEIGHBOURHOOD]);
double value = crossLength / (length1 * length2);
// If the sine of the angle surpass a threshold for being a sharp corner
// Ergo, if the angle is sufficienlty 90 degrees
if (value > EDGE_VALUE_THRESHOLD)
{
// Store the laser point in the assumed sharp corners
assumedSharpCorners.insert(std::pair<int,double>(i, asin(value)));
continue;
}
// If the sine of the angle surpass a threshold for being a hollow corner
// Ergo, if the angle is sufficienlty 270 degrees
if (value < - EDGE_VALUE_THRESHOLD)
{
// Store the laser point in the assumed hollow corners
assumedHollowCorners.insert(std::pair<int,double>(i, asin(value)));
continue;
}
}
}
}
}
// Get the best fitting corners from the set of all assumed corners
std::vector<int> bestSharpCorners = getBestcorners(assumedSharpCorners);
std::vector<int> bestHollowCorners = getBestcorners(assumedHollowCorners);
// Store all found sharp corners derived using the cross product
for (int index : bestSharpCorners)
{
world->currentLaserPoints[index].type = SharpCorner;
detectedEdges.emplace_back(world->currentLaserPoints[index]);
}
// Store all found hollow corners derived using the cross product
for (int index : bestHollowCorners)
{
world->currentLaserPoints[index].type = HollowCorner;
detectedEdges.emplace_back(world->currentLaserPoints[index]);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment