print(
'ERROR: Could not load file "{0}" as an image!'.format(image_path))
sys.exit()
lanes_found, left_lane_line, right_lane_line, vanishing_point = \
detect_lanes(
cv_image,
left_search_strips,
right_search_strips,
)
if lanes_found == False:
print('ERROR: Could not find lanes in the image!')
sys.exit()
vanishing_point_pixels = tuple2inttuple(colvec2tuple(vanishing_point))
cv2.circle(cv_image, vanishing_point_pixels, 10, (255, 255, 255))
bottom_image_line = Line(np.matrix([[0], [480]]), np.matrix([[1], [0]]))
bottom_left = Line.intersection(left_lane_line, bottom_image_line)
bottom_right = Line.intersection(right_lane_line, bottom_image_line)
bottom_left_pixels = tuple2inttuple(colvec2tuple(bottom_left))
bottom_right_pixels = tuple2inttuple(colvec2tuple(bottom_right))
cv2.circle(cv_image, bottom_left_pixels, 10, (0, 0, 255))
cv2.circle(cv_image, bottom_right_pixels, 10, (255, 0, 0))
cv2.line(cv_image, vanishing_point_pixels, bottom_left_pixels,
(255, 0, 255), 1, cv2.CV_AA)
cv2.line(cv_image, vanishing_point_pixels, bottom_right_pixels,
(255, 0, 255), 1, cv2.CV_AA)
def hw4_lane_pose_estimation():
'''
TODO DOCUMENTATION
'''
image_paths = ['LDWS_test/LDWS_test_data {0:03}.bmp'.format(x) for x in range(1, 609)]
intrinsic_matrix, distortion_coefficients = intrinsic_calibration.hw4_calibration(False)
left_search_strips, right_search_strips = lane_detection.define_hw4_search_strips()
cv2.namedWindow('display')
for image_path in image_paths:
cv_image = cv2.imread(image_path)
lanes_found, left_lane_line, right_lane_line, vanishing_point = \
lane_detection.detect_lanes(
cv_image,
left_search_strips,
right_search_strips,
)
display_image = cv_image
if lanes_found:
vanishing_point_pixels = tuple2inttuple(colvec2tuple(vanishing_point))
cv2.circle(display_image, vanishing_point_pixels, 10, (255, 255, 255))
bottom_image_line = Line(np.matrix([[0],[480]]), np.matrix([[1],[0]]))
bottom_left = Line.intersection(left_lane_line, bottom_image_line)
bottom_right = Line.intersection(right_lane_line, bottom_image_line)
bottom_left_pixels = tuple2inttuple(colvec2tuple(bottom_left))
bottom_right_pixels = tuple2inttuple(colvec2tuple(bottom_right))
cv2.circle(display_image, bottom_left_pixels, 10, (0, 0, 255))
cv2.circle(display_image, bottom_right_pixels, 10, (255, 0, 0))
almost_bottom_image_line = Line(np.matrix([[0],[480-150]]), np.matrix([[1],[0]]))
almost_bottom_left = Line.intersection(left_lane_line, almost_bottom_image_line)
almost_bottom_right = Line.intersection(right_lane_line, almost_bottom_image_line)
almost_bottom_left_pixels = tuple2inttuple(colvec2tuple(almost_bottom_left))
almost_bottom_right_pixels = tuple2inttuple(colvec2tuple(almost_bottom_right))
cv2.circle(display_image, almost_bottom_left_pixels, 10, (0, 0, 255))
cv2.circle(display_image, almost_bottom_right_pixels, 10, (255, 0, 0))
cv2.line(display_image, vanishing_point_pixels, bottom_left_pixels, (255, 0, 255), 1, cv2.CV_AA)
cv2.line(display_image, vanishing_point_pixels, bottom_right_pixels, (255, 0, 255), 1, cv2.CV_AA)
object_points = np.array([
[-1.6, 0, 0],
[1.6, 0, 0],
[-1.6, 4.0, 0],
[1.6, 4.0, 0],
])
image_points = np.array([
bottom_left.T.A[0],
bottom_right.T.A[0],
almost_bottom_left.T.A[0],
almost_bottom_right.T.A[0],
])
solve_pnp_results = cv2.solvePnP(
object_points,
image_points,
intrinsic_matrix,
distortion_coefficients,
)
pnp_success, rotation_omega, translate = solve_pnp_results
horizontal_drift = -translate[0][0]
print(horizontal_drift)
cv2.line(display_image, (320-80, 10), (320+80, 10), (0, 0, 0), 1, cv2.CV_AA)
cv2.line(display_image, (320-80, 10), (320-80, 50), (0, 0, 0), 1, cv2.CV_AA)
cv2.line(display_image, (320+80, 10), (320+80, 50), (0, 0, 0), 1, cv2.CV_AA)
cv2.line(display_image, (320-80, 50), (320+80, 50), (0, 0, 0), 1, cv2.CV_AA)
cv2.line(display_image, (320+int(40*horizontal_drift/0.4), 30), (320+int(40*horizontal_drift/0.4), 50), (0, 0, 0), 1, cv2.CV_AA)
cv2.imshow('display', display_image)
cv2.waitKey(1)
def hw4_lane_pose_estimation():
'''
TODO DOCUMENTATION
'''
image_paths = [
'LDWS_test/LDWS_test_data {0:03}.bmp'.format(x) for x in range(1, 609)
]
intrinsic_matrix, distortion_coefficients = intrinsic_calibration.hw4_calibration(
False)
left_search_strips, right_search_strips = lane_detection.define_hw4_search_strips(
)
cv2.namedWindow('display')
for image_path in image_paths:
cv_image = cv2.imread(image_path)
lanes_found, left_lane_line, right_lane_line, vanishing_point = \
lane_detection.detect_lanes(
cv_image,
left_search_strips,
right_search_strips,
)
display_image = cv_image
if lanes_found:
vanishing_point_pixels = tuple2inttuple(
colvec2tuple(vanishing_point))
cv2.circle(display_image, vanishing_point_pixels, 10,
(255, 255, 255))
bottom_image_line = Line(np.matrix([[0], [480]]),
np.matrix([[1], [0]]))
bottom_left = Line.intersection(left_lane_line, bottom_image_line)
bottom_right = Line.intersection(right_lane_line,
bottom_image_line)
bottom_left_pixels = tuple2inttuple(colvec2tuple(bottom_left))
bottom_right_pixels = tuple2inttuple(colvec2tuple(bottom_right))
cv2.circle(display_image, bottom_left_pixels, 10, (0, 0, 255))
cv2.circle(display_image, bottom_right_pixels, 10, (255, 0, 0))
almost_bottom_image_line = Line(np.matrix([[0], [480 - 150]]),
np.matrix([[1], [0]]))
almost_bottom_left = Line.intersection(left_lane_line,
almost_bottom_image_line)
almost_bottom_right = Line.intersection(right_lane_line,
almost_bottom_image_line)
almost_bottom_left_pixels = tuple2inttuple(
colvec2tuple(almost_bottom_left))
almost_bottom_right_pixels = tuple2inttuple(
colvec2tuple(almost_bottom_right))
cv2.circle(display_image, almost_bottom_left_pixels, 10,
(0, 0, 255))
cv2.circle(display_image, almost_bottom_right_pixels, 10,
(255, 0, 0))
cv2.line(display_image, vanishing_point_pixels, bottom_left_pixels,
(255, 0, 255), 1, cv2.CV_AA)
cv2.line(display_image, vanishing_point_pixels,
bottom_right_pixels, (255, 0, 255), 1, cv2.CV_AA)
object_points = np.array([
[-1.6, 0, 0],
[1.6, 0, 0],
[-1.6, 4.0, 0],
[1.6, 4.0, 0],
])
image_points = np.array([
bottom_left.T.A[0],
bottom_right.T.A[0],
almost_bottom_left.T.A[0],
almost_bottom_right.T.A[0],
])
solve_pnp_results = cv2.solvePnP(
object_points,
image_points,
intrinsic_matrix,
distortion_coefficients,
)
pnp_success, rotation_omega, translate = solve_pnp_results
horizontal_drift = -translate[0][0]
print(horizontal_drift)
cv2.line(display_image, (320 - 80, 10), (320 + 80, 10), (0, 0, 0),
1, cv2.CV_AA)
cv2.line(display_image, (320 - 80, 10), (320 - 80, 50), (0, 0, 0),
1, cv2.CV_AA)
cv2.line(display_image, (320 + 80, 10), (320 + 80, 50), (0, 0, 0),
1, cv2.CV_AA)
cv2.line(display_image, (320 - 80, 50), (320 + 80, 50), (0, 0, 0),
1, cv2.CV_AA)
cv2.line(display_image,
(320 + int(40 * horizontal_drift / 0.4), 30),
(320 + int(40 * horizontal_drift / 0.4), 50), (0, 0, 0),
1, cv2.CV_AA)
cv2.imshow('display', display_image)
cv2.waitKey(1)
if cv_image == None:
print('ERROR: Could not load file "{0}" as an image!'.format(image_path))
sys.exit()
lanes_found, left_lane_line, right_lane_line, vanishing_point = \
detect_lanes(
cv_image,
left_search_strips,
right_search_strips,
)
if lanes_found == False:
print('ERROR: Could not find lanes in the image!')
sys.exit()
vanishing_point_pixels = tuple2inttuple(colvec2tuple(vanishing_point))
cv2.circle(cv_image, vanishing_point_pixels, 10, (255, 255, 255))
bottom_image_line = Line(np.matrix([[0],[480]]), np.matrix([[1],[0]]))
bottom_left = Line.intersection(left_lane_line, bottom_image_line)
bottom_right = Line.intersection(right_lane_line, bottom_image_line)
bottom_left_pixels = tuple2inttuple(colvec2tuple(bottom_left))
bottom_right_pixels = tuple2inttuple(colvec2tuple(bottom_right))
cv2.circle(cv_image, bottom_left_pixels, 10, (0, 0, 255))
cv2.circle(cv_image, bottom_right_pixels, 10, (255, 0, 0))
cv2.line(cv_image, vanishing_point_pixels, bottom_left_pixels, (255, 0, 255), 1, cv2.CV_AA)
cv2.line(cv_image, vanishing_point_pixels, bottom_right_pixels, (255, 0, 255), 1, cv2.CV_AA)
cv2.namedWindow('display')
cv2.imshow('display', cv_image)