Winkelerkennung

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() {}
 };