def local_motion_task(motion_queue): birdview = BirdsEye(negate=True) BP = brickpi3.BrickPi3() while True: decimg_left = motion_queue.get() print("received") frame = cv2.medianBlur(decimg_left, 3) bird = birdview.apply(frame) left, right = ld.detect(bird, negate=True, robot=True, thin=False) car_position = int(bird.shape[1] / 2) left_speed, right_speed = st.calculate_steering(left, right, car_position) BP.set_motor_power(BP.PORT_D, left_speed) BP.set_motor_power(BP.PORT_A, right_speed)
def line_detection_task(): birdview = BirdsEye(negate=True) while True: frame = global_line_detection_queue.get() print("Received image") frame = cv2.medianBlur(frame, 3) bird = birdview.apply(frame) left, right = ld.detect(bird, negate=True, robot=True, thin=True) delta, car_position, mid = st.calculate_steering_delta( bird, left, right) #left_speed, right_speed = st.calculate_steering(bird, left, right) driving_state.set_lines(left, right) driving_state.set_steering(delta, car_position, mid)
def image_task(queue): key = '' width = cp.FRAME_WIDTH height = cp.FRAME_HEIGHT birdview = BirdsEye( perspective_file_path="../../config/camera_perspective.npy", negate=True) while key != ord('q'): frame = queue.get() frame = cv2.medianBlur(frame, 3) bird = birdview.apply(frame) # Line Detection left, right = ld.detect(bird, negate=True, robot=True, thin=True) # ======================== PLOT =========================== if left is not None: for i in range(0, bird.shape[0] - 1): y_fit = left[0] * (i**2) + left[1] * i + left[2] cv2.circle(bird, (int(y_fit), i), 1, (0, 0, 255), thickness=1) if right is not None: for i in range(0, bird.shape[0] - 1): y_fit = right[0] * (i**2) + right[1] * i + right[2] cv2.circle(bird, (int(y_fit), i), 1, (0, 0, 255), thickness=1) # ======================== CALCULATE STEERING =================== left_speed, right_speed = st.calculate_steering(bird, left, right) print(left_speed, right_speed) cv2.imshow("Frame", frame) cv2.moveWindow("Frame", 10, 10) cv2.imshow("Bird", bird) cv2.moveWindow("Bird", 10 + cp.FRAME_WIDTH, 10) key = cv2.waitKey(1) & 0xFF cv2.destroyAllWindows()
def image_task(): birdview = BirdsEye(negate=True) while True: frame = global_image_queue.get() print("Received image") frame = cv2.medianBlur(frame, 3) bird = birdview.apply(frame) left, right = ld.detect(bird, negate=True, robot=True, thin=True) left_speed, right_speed = st.calculate_steering(bird, left, right) global_motion_queue.put((left_speed, right_speed)) # ======================== PLOT =========================== if left is not None: for i in range(0, bird.shape[0] - 1): y_fit = left[0] * (i**2) + left[1] * i + left[2] cv2.circle(bird, (int(y_fit), i), 1, (0, 0, 255), thickness=1) if right is not None: for i in range(0, bird.shape[0] - 1): y_fit = right[0] * (i**2) + right[1] * i + right[2] cv2.circle(bird, (int(y_fit), i), 1, (0, 0, 255), thickness=1) cv2.imshow('CLIENT_LEFT', frame) cv2.moveWindow('CLIENT_LEFT', 10, 10) cv2.imshow('BIRD', bird) cv2.moveWindow('BIRD', 660, 10) key = cv2.waitKey(1) & 0xFF cv2.destroyAllWindows()
def image_task(image_queue): birdview = BirdsEye(negate=True) key = '' while key != ord('q'): stringData_left = image_queue.get() print("Received image") data_left = np.fromstring(stringData_left, dtype='uint8') decimg_left = cv2.imdecode(data_left, 1) # Performs operations here bird = birdview.apply(decimg_left) # Line Detection left, right = ld.detect(bird, negate=True, robot=True, thin=True) # ======================== PLOT =========================== if left is not None: for i in range(0, bird.shape[0] - 1): y_fit = left[0] * (i**2) + left[1] * i + left[2] cv2.circle(bird, (int(y_fit), i), 1, (0, 0, 255), thickness=1) if right is not None: for i in range(0, bird.shape[0] - 1): y_fit = right[0] * (i**2) + right[1] * i + right[2] cv2.circle(bird, (int(y_fit), i), 1, (0, 0, 255), thickness=1) cv2.imshow('CLIENT_LEFT', decimg_left) cv2.moveWindow('CLIENT_LEFT', 10, 10) cv2.imshow('BIRD', bird) cv2.moveWindow('BIRD', 660, 10) key = cv2.waitKey(1) & 0xFF cv2.destroyAllWindows()
def evaluate(): acc_left_list = [] acc_right_list = [] detection_left_list = [] detection_right_list = [] deviation_left_list = [] deviation_right_list = [] width = cp.FRAME_WIDTH height = cp.FRAME_HEIGHT points = np.float32([ [ 261, 326 ], [ 356, 326 ], [ 173, 478 ], [ 411, 478 ] ]) destination_points = np.float32([ [ width / cp.CHESSBOARD_ROW_CORNERS, height / cp.CHESSBOARD_COL_CORNERS ], [ width - (width / cp.CHESSBOARD_ROW_CORNERS), height / cp.CHESSBOARD_COL_CORNERS ], [ width / cp.CHESSBOARD_ROW_CORNERS, height + 200 ], [ width - (width / cp.CHESSBOARD_ROW_CORNERS), height + 200 ] ]) M = cv2.getPerspectiveTransform(points, destination_points) birdview = BirdsEye(M=M, width=width, height=height) images = [f for f in listdir(IMAGES) if isfile(join(IMAGES, f))] images = [f for f in images if ".png" in f] # images = [ # "2011_09_26_0027_0000000022.png", # # "2011_09_26_0029_0000000361.png", # "2011_09_26_0028_0000000037.png", # "2011_09_26_0027_0000000058.png", # "2011_09_26_0029_0000000035.png", # ] for path in images: print("=========================================") print(path) print() img = cv2.imread(str(IMAGES) + str(path)) img = cv2.resize(img, (width, height)) left_img = cv2.imread(str(GROUND_TRUTH) +"left_"+str(path)) left_img = cv2.resize(left_img, (width, height)) right_img = cv2.imread(str(GROUND_TRUTH) +"right_"+str(path)) right_img = cv2.resize(right_img, (width, height)) img = cv2.medianBlur(img, 3) bird = birdview.apply(img) bird_left_col = birdview.apply(left_img) bird_left = np.zeros((bird_left_col.shape[0], bird_left_col.shape[1], 1), dtype=np.uint8) cv2.cvtColor(bird_left_col, cv2.COLOR_RGB2GRAY, bird_left, 1) bird_right_col = birdview.apply(right_img) bird_right = np.zeros((bird_right_col.shape[0], bird_right_col.shape[1], 1), dtype=np.uint8) cv2.cvtColor(bird_right_col, cv2.COLOR_RGB2GRAY, bird_right, 1) # Line Detection left, right, left_features, right_features = ld.detect_verbose(bird, negate=False, robot=False, thin=False) print(left, right) print(left_features, right_features) # ========== Plot ============ l_x, l_y = left_features for i in range(0, len(l_x)): cv2.circle(bird_left_col, (int(l_y[i]), int(l_x[i])), 1, (0, 255, 0), thickness=1) r_x, r_y = right_features for i in range(0, len(r_x) - 1): cv2.circle(bird_right_col, (int(r_y[i]), int(r_x[i])), 1, (0, 255, 0), thickness=1) # ========== Accuracy =========== if left is not None: acc_left = calculate_lane_feature_accuracy(left_features, bird_left) print(acc_left) acc_left_list.append(acc_left) if right is not None: acc_right = calculate_lane_feature_accuracy(right_features, bird_right) print(acc_right) acc_right_list.append(acc_right) # ========== Detections =========== detection_left_list.append(left is not None) detection_right_list.append(right is not None) # ========== Lane position deviation ======== if left is not None: dev_left = calculate_lane_position_deviation(left, bird_left) print(dev_left) deviation_left_list.append(dev_left) # ========== Plot =========== for i in range(0, bird_left_col.shape[0]): y_fit = left[0] * (i ** 2) + left[1] * i + left[2] cv2.circle(bird_left_col, (int(y_fit), i), 1, (0, 0, 255), thickness=1) if right is not None: dev_right = calculate_lane_position_deviation(right, bird_right) print(dev_right) deviation_right_list.append(dev_right) # ========== Plot =========== for i in range(0, bird_right_col.shape[0]): y_fit = right[0] * (i ** 2) + right[1] * i + right[2] cv2.circle(bird_right_col, (int(y_fit), i), 1, (0, 0, 255), thickness=1) cv2.imshow("Original", img) cv2.imshow("GT Left", left_img) cv2.imshow("GT Right", right_img) cv2.moveWindow("Original", 100, 100) cv2.moveWindow("GT Left", 800, 100) cv2.moveWindow("GT Right", 1500, 100) cv2.imshow("Original Bird", bird) cv2.imshow("GT Left Bird", bird_left_col) cv2.imshow("GT Right Bird", bird_right_col) cv2.moveWindow("Original Bird", 100, 800) cv2.moveWindow("GT Left Bird", 800, 800) cv2.moveWindow("GT Right Bird", 1500, 800) cv2.waitKey(0) print("=============== SUMMARY ============") print("Smaples: "+str(len(images))) print() print("mean_acc_left: "+str(np.mean(acc_left_list))) print("mean_acc_right: "+ str(np.mean(acc_right_list))) print("mean_acc: "+str(np.mean(acc_left_list + acc_right_list))) print() print("mean_deviation_left: " + str(np.mean(deviation_left_list))) print("mean_deviation_right: " + str(np.mean(deviation_right_list))) print("mean_deviation: " + str(np.mean(deviation_left_list + deviation_right_list))) print() print("mis_det_left: "+str(detection_left_list.count(False)/len(detection_left_list))) print("mis_det_right: "+str(detection_right_list.count(False)/len(detection_right_list))) mis_detection = detection_left_list + detection_right_list print("mis_det: "+str(mis_detection.count(False)/len(mis_detection)))
def main(): images = [f for f in listdir(IMAGES) if isfile(join(IMAGES, f))] for path in images: width = cp.FRAME_WIDTH height = cp.FRAME_HEIGHT points = np.float32([ [ 261, 326 ], [ 356, 326 ], [ 173, 478 ], [ 411, 478 ] ]) destination_points = np.float32([ [ width / cp.CHESSBOARD_ROW_CORNERS, height / cp.CHESSBOARD_COL_CORNERS ], [ width - (width / cp.CHESSBOARD_ROW_CORNERS), height / cp.CHESSBOARD_COL_CORNERS ], [ width / cp.CHESSBOARD_ROW_CORNERS, height + 200 ], [ width - (width / cp.CHESSBOARD_ROW_CORNERS), height + 200 ] ]) M = cv2.getPerspectiveTransform(points, destination_points) birdview = BirdsEye(M=M, width=width, height=height) img = cv2.resize(img, (width, height)) img = cv2.medianBlur(img, 3) # remove noise from HS channels TODO choose # TODO adapt result to correct size bird = birdview.apply(img) # Line Detection left, right = ld.detect(bird, negate=False, robot=False, thin=False) print(left, right) cv2.imshow("Original", img) cv2.moveWindow("Original", 800, 100) cv2.waitKey(0)
def rendering_task(): birdview = BirdsEye(negate=True) while True: frame = global_rendering_queue.get() bird = birdview.apply(frame) left, right = driving_state.get_lines() # ======================== PLOT LINES =========================== if left is not None: for i in range(0, bird.shape[0] - 1): y_fit = left[0] * (i**2) + left[1] * i + left[2] cv2.circle(bird, (int(y_fit), i), 1, (0, 0, 255), thickness=1) if right is not None: for i in range(0, bird.shape[0] - 1): y_fit = right[0] * (i**2) + right[1] * i + right[2] cv2.circle(bird, (int(y_fit), i), 1, (0, 0, 255), thickness=1) # ======================== PLOT BOUNDING BOX ===================== # M = np.linalg.inv(driving_state.get_perspective_transform()) M = driving_state.get_perspective_transform() # TODO use np.linalg.inv( for conversion from bird to original # == car car_results = driving_state.get_car_detections() for res in car_results: dr.draw_detection(frame, res) detection_origin = np.array([[res['bottomright']['x']], [res['bottomright']['y']], [1]]) detection_origin_bird = np.matmul(M, detection_origin) detection_origin_bird = detection_origin_bird / detection_origin_bird[ 2] print("Detection origin bird:" + str(detection_origin_bird)) # cv2.circle(bird, (detection_origin_bird[0], detection_origin_bird[1]), 2, (123, 0, 255), thickness=4) # == sign sign_results = driving_state.get_sign_detections() for res in sign_results: dr.draw_detection(frame, res) # ================= COLLISION LINE PLOT collision_y = bird.shape[ 0] - driving_state.get_object_collision_distance() cv2.line(bird, (0, collision_y), (bird.shape[1], collision_y), (255, 0, 0), thickness=2, lineType=cv2.LINE_8) cv2.imshow('CLIENT_LEFT', frame) cv2.moveWindow('CLIENT_LEFT', 10, 10) cv2.imshow('BIRD', bird) cv2.moveWindow('BIRD', 660, 10) key = cv2.waitKey(1) & 0xFF cv2.destroyAllWindows()
points = np.float32([[261, 326], [356, 326], [173, 478], [411, 478]]) destination_points = np.float32( [[ width / cp.CHESSBOARD_ROW_CORNERS, height / cp.CHESSBOARD_COL_CORNERS ], [ width - (width / cp.CHESSBOARD_ROW_CORNERS), height / cp.CHESSBOARD_COL_CORNERS ], [width / cp.CHESSBOARD_ROW_CORNERS, height + 200], [width - (width / cp.CHESSBOARD_ROW_CORNERS), height + 200]]) M = cv2.getPerspectiveTransform(points, destination_points) birdeye = BirdsEye(M=M, width=width, height=height) img = cv2.imread(BASE_PATH + path) img = cv2.resize(img, (cp.FRAME_WIDTH, cp.FRAME_HEIGHT)) #img = cv2.GaussianBlur(img, (5, 5), 0) img = cv2.medianBlur(img, 3) # remove noise from HS channels TODO choose # Work only on bird view if DEBUG: cv2.imshow("Frame", img) img = birdeye.apply(img) # ======================== DETECTION =========================== left, right = ld.detect(img, thin=False)
[ width - (width / cp.CHESSBOARD_ROW_CORNERS), height / cp.CHESSBOARD_COL_CORNERS ], [ width / cp.CHESSBOARD_ROW_CORNERS, height - (height / cp.CHESSBOARD_COL_CORNERS) ], [ width - (width / cp.CHESSBOARD_ROW_CORNERS), height - (height / cp.CHESSBOARD_COL_CORNERS) ]]) M = cv2.getPerspectiveTransform(points, destination_points) birdeye = BirdsEye(M, width, height) img = cv2.imread(BASE_PATH + path) img = cv2.resize(img, (WIDTH, HEIGHT)) # img = birdeye.apply(img) # TODO cut image on half to have strongest line detection and avoid noise # TODO fix linee tratteggiate left, right, car_position = detect(img) # motor_controller = MotorController() # motor_controller.start() # motor_controller.get_queue().put((left, right, car_position))