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))