Winkelerkennung

AutonomousMode/ControlModule/control_module.cpp

@@ -1,9 +1,29 @@
 #include "control_module.h"
 
-ControlModule::ControlModule(/* args */)
+ControlModule::ControlModule(){    
+}
+
+ControlModule::ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed)
 {
+    this->forwardSpeed = forwardSpeed;
+    this->rotateSpeed = rotateSpeed;
+    this->moveSideSpeed = moveSideSpeed;
 }
 
 ControlModule::~ControlModule()
 {
+}
+
+void ControlModule::moveForward(){
+    for(int i = 0; motors; i++){
+
+    }
+};
+
+void ControlModule::moveSide(){
+
+}
+
+void ControlModule::rotate(){
+
 }

AutonomousMode/ControlModule/control_module.h

@@ -3,9 +3,15 @@
 class ControlModule
 {
 private:
-    /* data */
+    float motors[4]; //LeftFront; RightFront; LeftBack; RightBack
+    float forwardSpeed;
+    float rotateSpeed;
+    float moveSideSpeed;
 public:
     ControlModule(/* args */);
-
+    ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed);
     ~ControlModule();
+    void moveForward();
+    void rotate();
+    void moveSide();
 };

AutonomousMode/Input/input.cpp

@@ -1,8 +1,8 @@
 #include "input.h"
 
 // TODO: Wenn ihr in die Zeile den Pfad zum Testvideo statt der 0 packt, benmutzt er das Testvideo.
-//Input::Input(int videoHeight, int videoWidth) : cap("C:\\Users\\User\\Desktop\\Studium\\02_Master_MSY\\2. Semester  Winter 22 23\\Projekt\\Line-Following-Robot\\AutonomousMode\\Test_data\\video1.h264"), videoHeight(videoHeight), videoWidth(videoWidth)
-Input::Input(int videoHeight, int videoWidth) : cap(0), videoHeight(videoHeight), videoWidth(videoWidth)
+Input::Input(int videoHeight, int videoWidth) : cap("C:\\Line-Following-Robot\\AutonomousMode\\Test_data\\video1.h264"), videoHeight(videoHeight), videoWidth(videoWidth)
+//Input::Input(int videoHeight, int videoWidth) : cap(0), videoHeight(videoHeight), videoWidth(videoWidth)
 {
     this->cap.set(CAP_PROP_FRAME_HEIGHT, videoHeight);
     this->cap.set(CAP_PROP_FRAME_WIDTH, videoWidth);

AutonomousMode/Processing/processing.cpp

@@ -1,5 +1,8 @@
 #include "processing.h"
 
+
+
+
 Processing::Processing(/* args */)
 {
 }
@@ -47,6 +50,7 @@ void Processing::filterReflections(FrameData& frameData)
             indicesToDelete.push_back(i);
             continue;
         }
+        
     }
 
     //reverse the vector with the indices so the order isn't messed up when deleting:
@@ -82,6 +86,66 @@ bool Processing::checkIntersection(const std::vector<std::vector<Point>>& lines,
     return true;
 }
 
+int Processing::calcAngles(FrameData &data, int imageColums, int imageRows, bool turnLeft)
+{
+    int angle = 0;
+    if(data.contours.size() > 0)
+    {
+            int index;
+            if(turnLeft){
+                // get the most left/right contour
+                int leftmostEdge = imageColums;
+                
+                for (int i = 0; i < data.leftEdges.size(); i++)
+                {
+                    int edge = data.leftEdges[i].x;
+                                            
+                    if (edge <= leftmostEdge)
+                    {
+                        leftmostEdge = edge;
+                        index = i;
+                    }
+                }
+            }else
+            {
+                int rightmostEdge = 0;
+                
+                for (int i = 0; i < data.leftEdges.size(); i++)
+                {
+                    int edge = data.leftEdges[i].x;
+                    
+                    if (edge >= rightmostEdge)
+                    {
+                        rightmostEdge = edge;
+                        index = i;
+                    }
+                }
+            }    
+            // Find the Top-Left and Buttom-Left Point of the Contour
+            
+            std::vector<cv::Point> leftMostContour = data.contours[index];
+            int xleftButtom = imageColums; 
+            int xleftTop = imageColums;
+            int yButtom = data.boundingBoxes[index].y;
+            int yTop = data.boundingBoxes[index].height;
+            
+            for (int i = 0; i < leftMostContour.size(); i++){
+                if(leftMostContour[i].y == 0 && leftMostContour[i].x < xleftButtom)
+                {
+                    xleftButtom = leftMostContour[i].x;
+                }else if(leftMostContour[i].y > yTop -4 && leftMostContour[i].x < xleftTop){
+                    xleftTop = leftMostContour[i].x;
+                }
+            }
+
+        // calculate angle
+        int deltaX = abs(xleftButtom - xleftTop);
+        int deltaY = yTop;                
+        angle = VectorOfLines::calcAngle(deltaX, deltaY);
+    }
+    return angle;
+}
+
 
 void Processing::processImage(Mat& inputPicture, int thresholdBinary, int gaussKernelSize)
 {
@@ -126,7 +190,7 @@ FrameData Processing::calculateLineSegments(Mat& inputPicture, const cv::Rect& r
     auto iterator = data.contours.begin();
     while(iterator != data.contours.end())
     {
-        if (contourArea(*iterator) < 3500)
+        if (contourArea(*iterator) < 7000)
         {
             iterator = data.contours.erase(iterator);
         }
@@ -137,8 +201,6 @@ FrameData Processing::calculateLineSegments(Mat& inputPicture, const cv::Rect& r
         else
         {
             //Check for intersection with lines:
-
-
             Rect boundingBox = boundingRect(*iterator);
             boundingBox.x += roi.x;
             boundingBox.y += roi.y;

AutonomousMode/Processing/processing.h

@@ -3,6 +3,7 @@
 #include <utils.h>
 #include <vector>
 
+
 using namespace cv;
 using namespace std;
 
@@ -23,4 +24,5 @@ public:
     void processImage(Mat& inputPicture, int thresholdBinary, int gaussKernelSize);
     void filterReflections(FrameData& frameData);
     FrameData calculateLineSegments(Mat& inputPicture, const cv::Rect& roi);
+    int calcAngles(FrameData &data, int imageColums, int imageRows, bool turnLeft);
 };

AutonomousMode/Utils/utils.cpp

@@ -1,5 +1,7 @@
 #include "utils.h"
 
+#define PI 3.14159265359
+
 LFRPoint::LFRPoint(/* args */) : x(0.0), y(0.0)
 {
 }
@@ -99,4 +101,25 @@ vector<Vec4i> VectorOfLines::findMiddleLine(vector<Vec4i> &lines){
     
     }
     return middleLines;
+}
+
+int VectorOfLines::calcAngle(int deltaX, int deltaY){
+
+        int refAngle = 0;
+
+        if(deltaX > 10E-12){
+        refAngle = atan(deltaY/deltaX) * 180/PI;
+
+        // convert from img coordinates to regbot coordinates
+        refAngle = -(refAngle);
+
+        if (refAngle > 90)
+            refAngle = refAngle - 180;
+
+        cout << refAngle << "\n";
+        }else{
+            refAngle = -90;
+        }
+        return refAngle;
+   
 }

AutonomousMode/Utils/utils.h

@@ -59,6 +59,7 @@ class VectorOfLines{
     float calcZeroPoint(cv::Point x, float m);
     static double calcDistance(Point p0, Point p1);
     vector<Vec4i> findMiddleLine(vector<Vec4i> &lines);
+    static int calcAngle(int deltaX, int deltaY);
 
 };
 
@@ -69,6 +70,7 @@ public:
     std::vector<cv::Rect> boundingBoxes;
     std::vector<cv::Point> leftEdges;
     std::vector<cv::Point> middlePoints;
+    std::vector<double> angles;
 
     FrameData(): contours(), boundingBoxes(), leftEdges(), middlePoints() {}
 };

AutonomousMode/lfr.cpp

@@ -28,8 +28,8 @@ void LFR::loop()
     {
         Mat originalImage = input.readWebcam();
 
-        Point roiOrigin(0, int(originalImage.rows*(7.5/12.0)));
-        Rect roi(roiOrigin.x, roiOrigin.y, originalImage.cols, originalImage.rows/12);
+        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;
 
@@ -37,7 +37,8 @@ void LFR::loop()
         //processedImage = processedImage(roi);
         FrameData data = processing.calculateLineSegments(processedImage, roi);
         processing.filterReflections(data);
-        this->provideOutput(originalImage, processedImage, data, roi);
+        int angle = processing.calcAngles(data, originalImage.cols, originalImage.rows, false);
+        this->provideOutput(originalImage, processedImage, data, roi, angle);
     }
     if(this->videoFlag) {destroyWindow("Display window");}
     input.freeWebcam();
@@ -56,7 +57,7 @@ void LFR::endLoop()
     return;
 }
 
-void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi)
+void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi, const int angle)
 {
     for(int i = 0; i < frameData.contours.size(); i++)
     {
@@ -67,6 +68,15 @@ void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData&
         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));
+        
+        int length = 100;
+        Point P1 = frameData.middlePoints[i];
+        Point P2;
+
+        P2.x =  (int)round(P1.x + length * cos(angle * CV_PI / 180.0));
+        P2.y =  (int)round(P1.y + length * sin(angle * CV_PI / 180.0));
+        cv::arrowedLine(originalImage, P1, P2, Scalar(0,0,255), 2, 8);
+
     }
     if(this->videoFlag)
     {

AutonomousMode/lfr.h

@@ -28,7 +28,7 @@ class LFR
     int thresholdBinary;
     int gaussKernelSize;
 
-    void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);
+    void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi, const int angle);
 
 public: