DetectionModule.cpp

// Gets the best corners from assumed corners
std::vector<int> getBestcorners(std::map<int, double> Corners)
{
    // Initialize the best corners
    std::vector<int> bestCorners;
    bestCorners.clear();

    // Initialize helper variables
    std::vector<int> allIndexes;
    allIndexes.clear();
    std::vector<double> allAngles;
    allAngles.clear();
    std::vector<int> currentCorners;
    currentCorners.clear();

    // For each assumed corner
    for (std::pair<int, double> corner : Corners)
    {
        // Store its index and corresponding angle
        allIndexes.emplace_back(corner.first);
        allAngles.emplace_back(corner.second);
    }

    // For each assumed corner
    for (int i = 0; i < Corners.size(); i++)
    {
        // Ithe current subset of corners is empty
        if (currentCorners.size() == 0)
        {
            // Add assumed corner to current subset
            currentCorners.emplace_back(allIndexes[i]);
        }
        // If the assumed corner is not the last and is in the neighbourhood of the last corner of the subset
        else if (i != (Corners.size() - 1) && (allIndexes[i] - currentCorners[currentCorners.size() - 1]) < EDGE_NEIGHBOURHOOD)
        {
            // Add assumed corner to current subset
            currentCorners.emplace_back(allIndexes[i]);
        }
        // If the subset ends
        else
        {
            // Initialize helper variables
            int bestIndex;
            double bestAngle;
            double bestError = 1000;

            // For each corner in the subset
            for (int j = 0; j < currentCorners.size(); j++)
            {
                // Get the angle and error to 90 or 270 degrees
                double angle = Corners.at(currentCorners[j]);
                double error = fabs(fabs(angle) - 90.0);

                // If the corner is better than the current best one
                if (error < bestError)
                {
                    // Store corner as the best fit
                    bestIndex = currentCorners[j];
                    bestAngle = angle;
                    bestError = error;
                }
            }
            
            // Add the best fitting corner to the best corners
            bestCorners.emplace_back(bestIndex);
            
            // Reset the current subset
            currentCorners.clear();
        }
    }

    // Return the best corners
    return bestCorners;
}