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 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 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 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)
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)
# ======================== PLOT ===========================
if left is not None:
for i in range(0, img.shape[0] - 1):
y_fit = left[0] * (i**2) + left[1] * i + left[2]
cv2.circle(img, (int(y_fit), i), 1, (0, 0, 255), thickness=1)
if right is not None:
for i in range(0, img.shape[0] - 1):
y_fit = right[0] * (i**2) + right[1] * i + right[2]
cv2.circle(img, (int(y_fit), i), 1, (0, 0, 255), thickness=1)
cv2.imshow("Frame d", img)
cv2.moveWindow("Frame d", 100, 100)