Line-Following-Robot/AutonomousMode/Processing/processing.cpp

#include "processing.h"

Processing::Processing(/* args */)
{
}

Processing::~Processing()
{
}

static double angle( Point pt1, Point pt2, Point pt0 )
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

void Processing::filterReflections(FrameData& frameData)
{
    //Try to filter reflections from the frame data.

    std::vector<int> indicesToDelete;
    for(int i = 0; i < frameData.contours.size(); i++)
    {

        // First approach: correct contours nearly fill their bounding box.
        // So delete all contours with an area smaller than 75% of their bounding box
        double contourArea = cv::contourArea(frameData.contours[i], false);
        double boundingBoxArea = double(frameData.boundingBoxes[i].width*frameData.boundingBoxes[i].height);
        double minRatio = 0.75;
        if(boundingBoxArea/contourArea < minRatio)
        {
            indicesToDelete.push_back(i);
            continue;
        }

        // Second approach: The contour should be nearly convex
        // So delete all contours with an area smaller than 95% of their convex hull.
        std::vector<Point> hull;
        cv::convexHull(frameData.contours[i], hull);
        double hullArea = cv::contourArea(hull, false);
        double minRatioHull = 0.95;
        if(contourArea/boundingBoxArea < minRatio)
        {
            indicesToDelete.push_back(i);
            continue;
        }

        // Third approach:
        // Calculate the HoughLinesP of the contour.
        // There should be 4 lines
        // This one is not really working yet.
        Rect boundingRect = cv::boundingRect(frameData.contours[i]);
        Point offset(-boundingRect.x, -boundingRect.y);
        Mat contourMat = Mat::zeros(frameData.boundingBoxes[i].height, frameData.boundingBoxes[i].width, CV_8UC1);
        drawContours(contourMat, frameData.contours, i, Scalar(255,255,255), 1, 8, noArray(), 0, offset);
        std::vector<Vec4i> linesP;
        HoughLinesP(contourMat, linesP, 1, CV_PI/180, 20, 10, 5 );
        //imshow("Some window", contourMat);
        if(linesP.size() < 4)
        {
            //indicesToDelete.push_back(i);
            //continue;
            //std::cout << linesP.size();
        }
    }

    //reverse the vector with the indices so the order isn't messed up when deleting:
    std::reverse(indicesToDelete.begin(), indicesToDelete.end());
    for(int index : indicesToDelete)
    {
        frameData.boundingBoxes.erase(frameData.boundingBoxes.begin() + index);
        frameData.contours.erase(frameData.contours.begin() + index);
        frameData.leftEdges.erase(frameData.leftEdges.begin() + index);
        frameData.middlePoints.erase(frameData.middlePoints.begin() + index);
    }
    return;
}

void Processing::processImage(Mat& inputPicture, int thresholdBinary, int gaussKernelSize)
{
    //Idea here is: Processing module consists of two methods:
    // One (this) to do all kinds of stuff to the picture (grayscale conversion, threshold, gauss etc etc)
    // And one (the other one) to segment the lines.
    // No return value here as the input is passed by reference -> directly modified.
    cvtColor(inputPicture, inputPicture, COLOR_BGR2GRAY);
    GaussianBlur(inputPicture, inputPicture, Size(gaussKernelSize, gaussKernelSize), 0); 
    threshold(inputPicture, inputPicture, thresholdBinary, 255, THRESH_BINARY);

    //Perform an opening
    Mat kernel(5,5, CV_8UC1,1);
    morphologyEx(inputPicture, inputPicture, 2, kernel);
}

FrameData Processing::calculateLineSegments(Mat& inputPicture, const cv::Rect& roi)
{

    FrameData data;
    Mat inputPictureRoi = inputPicture(roi);
    cv::findContours(inputPictureRoi, data.contours, RETR_LIST, CHAIN_APPROX_SIMPLE);

    /* TODO:
    * Was ich mir hier vorstelle:
    * Wir haben die konturen und die hough linien
    * Konturen haben schwäche mit Reflektionen, hough linien sind unvollständig.
    * Irgendwie sollten die beiden Datensätze gemerged werden, um das "gute" aus beiden zu vereinen.
    * Evtl in richtung "Schau den Bereich der Contour (+ Toleranz?) im hough bild an. Wenn keine Linie im Bereich ist, ist die Contour eine Reflektion"
    */

    vector<Vec4i> lines;
    Canny(inputPicture, inputPicture, 50, 100, 3);
    HoughLinesP(inputPicture, lines, 1, CV_PI/180, 100, 30, 150);  
    //Draw lines  
    inputPicture = Mat::zeros(inputPicture.size().height, inputPicture.size().width, CV_8UC1);

    for( size_t i = 0; i < lines.size(); i++ )
    {
        line( inputPicture, Point(lines[i][0], lines[i][1]), Point(lines[i][2], lines[i][3]), Scalar(255,255,255), 3, 8 );
    }


    //Delete the areas that are too small
    auto iterator = data.contours.begin();
    while(iterator != data.contours.end())
    {
        if (contourArea(*iterator) < 3500)
        {
            iterator = data.contours.erase(iterator);
        }
        else
        {
            Rect boundingBox = boundingRect(*iterator);
            boundingBox.x += roi.x;
            boundingBox.y += roi.y;
            data.boundingBoxes.push_back(boundingBox);
            data.middlePoints.push_back(Point(boundingBox.x+boundingBox.width/2, boundingBox.y+boundingBox.height/2));
            data.leftEdges.push_back(Point(boundingBox.x, boundingBox.y+boundingBox.height/2));
            ++iterator;
        }
    }
    return data;
}