Line-Following-Robot/AutonomousMode/lfr.cpp

#include "lfr.h"

#define right false
#define left true

LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize)
    : iAmLooping(false), input(videoHeight, videoWidth), processing(), controlModule(), interpreter(), intersectionHandler()
{
    this->iAmLooping = false;
    this->thresholdBinary = thresholdBinary;
    this->gaussKernelSize = gaussKernelSize;

    this->videoFlag = false;
    this->saveOutputFlag = false;
    this->outputFileName = "";
}

LFR::~LFR()
{
    if(iAmLooping)
    {
        this->endLoop();
    }
}

void LFR::loop()
{
    if(this->videoFlag) {namedWindow("Display window");}
    while(iAmLooping)
    {
        Mat originalImage = input.readWebcam();

        Point roiOrigin(0, int(originalImage.rows*(3.25/6.0)));
        Rect roi(roiOrigin.x, roiOrigin.y, originalImage.cols, originalImage.rows/6);
        //Mat processedImage = originalImage(roi);
        Mat processedImage = originalImage;

        processing.processImage(processedImage, this->thresholdBinary, this->gaussKernelSize);
        //processedImage = processedImage(roi);
        FrameData data = processing.calculateLineSegments(processedImage, roi);
        processing.filterReflections(data);
        processing.calcAngles(data, originalImage.cols, originalImage.rows, left);
        this->provideOutput(originalImage, processedImage, data, roi);
    }
    if(this->videoFlag) {destroyWindow("Display window");}
    input.freeWebcam();
}

void LFR::startLoop()
{
    iAmLooping = true;
    this->loopThread=thread(&LFR::loop, this);
}

void LFR::endLoop()
{
    iAmLooping = false;
    this->loopThread.join();
    return;
}

void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi)
{
    for(int i = 0; i < frameData.contours.size(); i++)
    {
        drawContours(originalImage, frameData.contours, i, Scalar(0,255,255), 1, 8, noArray(), 0, Point(roi.x, roi.y));

        rectangle(originalImage, frameData.boundingBoxes[i], Scalar(0,255,0));
        Rect center(Point(frameData.middlePoints[i].x-2, frameData.middlePoints[i].y-2), Point(frameData.middlePoints[i].x+2, frameData.middlePoints[i].y+2));
        rectangle(originalImage, center, Scalar(0,0,255));
        Rect leftRect(Point(frameData.leftEdges[i].x-2, frameData.leftEdges[i].y-2), Point(frameData.leftEdges[i].x+2, frameData.leftEdges[i].y+2));
        rectangle(originalImage, leftRect, Scalar(0,0,255));
    }

    if(frameData.contours.size() > 0)
    {
        //Draw the Arrow for the check of the angle
        int length = 100;
        Point P1 = frameData.middlePoints[frameData.index];
        Point P2;

        P2.x =  (int)round(P1.x + length * cos(frameData.angle * CV_PI / 180.0));
        P2.y =  (int)round(P1.y + length * sin(frameData.angle * CV_PI / 180.0));
        cv::arrowedLine(originalImage, P1, P2, Scalar(0,0,255), 2, 8);
    }
    if(this->videoFlag)
    {
        imshow("Display window", originalImage);
        imshow("processed:", processedImage);
        char c = (char)waitKey(25);
    }
    if (this->saveOutputFlag && !(this->outputFileName.empty()))
    {
        imwrite(this->outputFileName, originalImage);        
    }
}