DetectionModule.cpp

// 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]);
    }
}