New drift detection

Detect/detect.py

@@ -41,7 +41,7 @@ def ROI(pic):
     roi = cv2.bitwise_and(pic, mask)
     return roi
 
-# Define the region of in which the lanes will be in the cameras view
+# Define the region of in which the lanes will be in the cameras view for the robot
 # params
 # pic: original image to apply the pre-set region of interest too
 def ROI_real(pic):
@@ -86,14 +86,15 @@ def find_middle(leftPoints, rightPoints):
 
     middle_lines = [[],[]]
 
-    if(leftPoints[1] is None or rightPoints[1] is None
-    ):
-        print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Caught the empty list~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
+    if rightPoints[1] is None:
+        print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Caught the empty right list~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
+        return 0
+    elif leftPoints[1] is None :
+        print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Caught the empty left list~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
         return 0
-
     else:
-        print(leftPoints[1])
-        print(rightPoints[1])
+        #print(leftPoints[1])
+        #print(rightPoints[1])
         for x in range(150):
 
             #print("Right x point: " + str(rightPoints[0][x]))
@@ -181,70 +182,6 @@ def find_poly_lane(pic, lines):
 
         return [right_x_new, right_y_new], [left_x_new, left_y_new]
 
-# Find the two average lines given the set of lines
-# params
-# pic: the original image
-# lines: An array of lines in the form (x1, x2, y1, y2)
-# Deprecated???
-def find_average_lane(pic, lines):
-
-        # Collections for the negative and positive sloped lines
-        left_lines  = [] # Negative slope
-        left_lines_points = [[],[]]  # Negative slope
-        right_lines = [] # Positive slope
-        right_lines_points = [[],[]] # Positive slope
-
-        for line in lines:
-            x1, y1, x2, y2 = line[0]
-            parameters = np.polyfit((x1, x2), (y1, y2), 1)
-            #print("Slope, intercept")
-            #print(parameters)
-            slope = parameters[0]
-            intercept = parameters[1]
-
-            if(slope < 0):
-                #print("Left insert")
-                left_lines.append((slope, intercept))
-                left_lines_points[0].append(x1)
-                left_lines_points[0].append(x2)
-                left_lines_points[1].append(y1)
-                left_lines_points[1].append(y2)
-
-            else:
-                #print("Right insert")
-                right_lines.append((slope, intercept))
-                right_lines_points[0].append(x1)
-                right_lines_points[0].append(x2)
-                right_lines_points[1].append(y1)
-                right_lines_points[1].append(y2)
-
-        if not left_lines:
-            #print("Left is empty")
-            left_line = [0, 0, 0, 0]
-
-        else:
-            left_average = np.average(left_lines, axis=0)
-            left_line = make_coordinates(pic, left_average)
-            #print("Left Line: ")
-            #print(left_line)
-
-        if not right_lines:
-            #print("Right is emtpy")
-            right_line = [0, 0, 0, 0]
-
-        else:
-            right_average = np.average(right_lines, axis=0)
-            right_line = make_coordinates(pic, right_average)
-            #print("Right line : ")
-            #print(right_line)
-
-        #print("Left fit")
-        #print(left_line)
-
-        #print("\nRight fit")
-        #print(right_line)
-        return np.array([left_line, right_line])
-
 def make_coordinates(image, line_parameters):
     #print(line_parameters)
     slope, intercept = line_parameters
@@ -283,14 +220,26 @@ def detectDeparture(left, car, right):
         print("On course")
         print("On course")
 
+def detectDepartureNew(midPoints):
+    midx = 300
+    midy = 400
+    for x in range(150):
+        if(myround(midPoints[1][x]) == 400):
+            print(midPoints[0][x] - midx)
+            return midPoints[0][x] - midx
+
+
+def myround(x):
+    return int(5 * round(float(x)/5))
+
 
 
 
-video = cv2.VideoCapture("test2.mp4")
+video = cv2.VideoCapture("midTest.avi")
 #video = cv2.VideoCapture("highway.mp4")
 
 
-#video = cv2.VideoCapture(1)
+#video = cv2.VideoCapture(0)
 plt.ion()
 
 while True:
@@ -307,8 +256,8 @@ while True:
 
     wEdges = detect_edge(new_img)
 
-    #cropped = ROI_real(wEdges)
-    cropped = ROI(wEdges)
+    cropped = ROI_real(wEdges)
+    #cropped = ROI(wEdges)
 
     lines = getLines(cropped)
 
@@ -329,11 +278,12 @@ while True:
         plt.scatter(Lpoints[0], Lpoints[1])
         plt.scatter(Mpoints[0], Mpoints[1])
 
-        #plt.scatter(310, 300)
+        plt.scatter(300, 400)
 
         plt.imshow(frame, zorder=0)
 
-        detectDeparture(Lpoints, 310, Rpoints)
+        #detectDeparture(Lpoints, 310, Rpoints)
+        detectDepartureNew(Mpoints)
 
         plt.pause(.001)