Line-Following-Robot/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::processImage(Mat& inputPicture, int thresholdValue, 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.
    
    vector<Vec4i> lines;

    cvtColor(inputPicture, inputPicture, COLOR_BGR2GRAY);
    threshold(inputPicture, inputPicture, thresholdValue, 255, THRESH_BINARY);
    GaussianBlur(inputPicture, inputPicture, Size(gaussKernelSize, gaussKernelSize), 0);
    
    Canny(inputPicture, inputPicture, 50, 200, 3);

    HoughLinesP(inputPicture, lines, 1, CV_PI/180, 150, 0, 0);  
    //Draw lines  
    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(0,0,255), 3, 8 );
    }
    vector<VectorOfLines> vectors;

    Point point11;
    Point point12;
    Point point21;
    Point point22;

    for( size_t i = 0; i < lines.size(); i++ )
    {
        point11 = Point(lines[i][0], lines[i][1]);
        point12 = Point( lines[i][2], lines[i][3]);
        float gradient1 = VectorOfLines::calcGradient(point11, point12);
        for( size_t j = 0; j < lines.size(); j++ )
        {
            if(j != i){
            point21 = Point(lines[j][0], lines[j][1]);
            point22 = Point(lines[j][2], lines[j][3]);
            float gradient2 = VectorOfLines::calcGradient(point21, point22);
            float gradient12 = VectorOfLines::calcGradient(point12, point21);
            if((norm(gradient1 - gradient2) < 0.05) & (norm(gradient1 - gradient12) <  0.05))
            {
                //To Do: add line between 2 lines
            }

            }
        }
    }


    imshow("Result", inputPicture);
    waitKey(0);
    destroyWindow("Result");

    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(0,0,255), 3, 8 );
        
    }
    
}

std::vector<LFRLine> Processing::calculateLineSegments(const Mat& inputPicture)
{
    //https://stackoverflow.com/questions/45322630/how-to-detect-lines-in-opencv
    return std::vector<LFRLine>();
}