def capture(data0, data1, vc0, vc1, target_dir, index):
vc0.grab()
vc1.grab()
_, img0 = vc0.retrieve()
_, img1 = vc1.retrieve()
img0 = calib.undistort(data0, img0)
img1 = calib.undistort(data1, img1)
cv2.imwrite(os.path.join(target_dir, '{}-img0.jpg'.format(index)), img0)
cv2.imwrite(os.path.join(target_dir, '{}-img1.jpg'.format(index)), img1)
def pipeline(img, is_running_on_video=True):
global left_curve, right_curve, frame_idx
img_shape = img.shape
# Undistort
undistorted = undistort(img, mtx, dist)
# Binarize
binary_result, color_binary_result = gen_binary_image(undistorted)
# Bird's Eye View
M, M_inv, warped = warp_top_view(binary_result)
# Find Lanes
if frame_idx == 0:
left_curve, right_curve, warped_lane_curves = find_lane_curves(
warped, left_curve, right_curve)
else:
left_curve, right_curve, warped_lane_curves = find_lane_curves_seed(
warped, left_curve, right_curve)
# Draw Lanes on Road
img_lane_curves = draw_lane_curves_on_road(undistorted, M_inv, left_curve,
right_curve)
# Find curvatures and offset
l_curvature = left_curve.curvature_metres()
r_curvature = right_curve.curvature_metres()
offset_from_center = get_offset_from_center(img_shape, left_curve,
right_curve)
# Combine all results
result = stitch_all_results(img_lane_curves, binary_result, warped,
warped_lane_curves, l_curvature, r_curvature,
offset_from_center)
if is_running_on_video:
frame_idx += 1
return result
def show_camera(vc, calibration_data):
while True:
ret, img = vc.read()
cv2.imshow("original", img)
res = calib.undistort(calibration_data, img)
cv2.imshow("undistored", res)
if (cv2.waitKey(20) & 0xFF) == 0x1B:
break
cv2.destroyAllWindows()
sxbinary = np.zeros_like(scaled_sobel)
sxbinary[(scaled_sobel >= sx_thresh[0])
& (scaled_sobel <= sx_thresh[1])] = 1
# Combine results
binary_result = np.logical_or(color_binary, sxbinary)
binary_result = binary_result.astype(np.uint8)
color_binary_result = np.dstack(
(np.zeros_like(sxbinary), sxbinary, color_binary)) * 255
return binary_result, color_binary_result
if __name__ == '__main__':
calib_dir = 'camera_cal'
test_img_dir = 'test_images'
output_dir = 'output_images'
test_images = os.listdir(test_img_dir)
file_idx = 5
file_name = os.path.splitext(test_images[file_idx])[0]
img = mpimg.imread(os.path.join(test_img_dir, test_images[file_idx]))
mtx, dist = calibrate(calib_dir)
undistorted = undistort(img, mtx, dist)
binary_result, color_binary_result = gen_binary_image(undistorted)
write_img_pair(undistorted, color_binary_result, "Original Image",
"Binary Image",
os.path.join(output_dir, file_name + "_binary_result.jpg"))
def process(self, frame):
"""
Main pipeline function to detect the lane lines given an image
:param frame: The current frame
:return: Overlayed frame with the lane lines
"""
# Save the shape of the original image
self.img_sz = frame.shape
# Compute the transformation co-ordinates
transform_dst = np.array([[0, frame.shape[0]], [0, 0],
[frame.shape[1], 0],
[frame.shape[1], frame.shape[0]]])
# Find a probable region of interest
self.__find_roi(frame)
# Initialize the frame memory with the frame shape with ROI is unstable
if not self.roi_stable or self.frame_memory is None:
self.frame_memory = np.zeros(shape=(self.frame_mem_max,
frame.shape[0],
frame.shape[1]),
dtype=np.uint8)
# Apply the distortion correction to the raw image.
if self.cam_calibration is not None:
dist_correct = calib.undistort(frame, self.cam_calibration)
else:
dist_correct = np.copy(frame)
# Transform the perspective
perspective_transformer = preprocess.PerspectiveTransformer(
np.float32(self.ROI), np.float32(transform_dst))
birdseye = perspective_transformer.transform(dist_correct)
# Use color transforms, gradients, etc., to create a thresholded binary image.
white, yellow, color, sobel, combined, binary = self.__threshold_image(
birdseye, self.debug_level > 1)
# Add the image to the frame memory
self.frame_memory[self.frame_idx] = binary
self.frame_avail[self.frame_idx] = True
# Allow more images to be added only after the ROI is stable.
if self.roi_stable:
self.frame_idx = (self.frame_idx + 1) % self.frame_mem_max
# Compute a motion blur across the frame memory
motion = np.zeros(shape=(self.img_sz[0], self.img_sz[1]),
dtype=np.uint8)
for idx, img in enumerate(self.frame_memory):
if self.frame_avail[idx]:
motion[(img > .5)] = 1
if self.fit_err > self.constants[
'FIT_ERR_THRESHOLD'] or not self.roi_stable:
# Search for the lanes using sliding windows
lane_markings = self.hist_finder.find(motion, self.lane_width,
self.roi_stable)
self.find_mode = 'hist'
else:
lane_markings = self.__get_markings_from_fit()
self.find_mode = 'prev'
# Get a fit from the lane markings
confidences, fits, masks = self.__get_fit(motion, lane_markings)
# Smoothen the fit
for i in np.arange(2):
# If we don't have a value from previous frame, just use the current one and hope for the best
# If the ROI is not stable, then also just use the current one and don't smoothen
if self.fit_values[i] is None or not self.roi_stable:
self.fit_err = 0
self.fit_values[i] = fits[i]
else:
# Check if we've got a valid value now
if fits[i] is not None:
# Calculate the error from the previous frame
self.fit_err = np.mean((self.fit_values[i] - fits[i])**2)
# If error is within expected values, then average the fit
if self.fit_err < self.constants[
'FIT_ERR_THRESHOLD'] and confidences[i] > 0.05:
# Average out based on the confidence
prev_confidence = self.constants['FIT_AVG_WEIGHT'] + \
((1 - self.constants['FIT_AVG_WEIGHT']) * self.fit_confidence /
(self.fit_confidence + confidences[i]))
curr_confidence = (1 - self.constants['FIT_AVG_WEIGHT']) * \
(confidences[i] / (self.fit_confidence + confidences[i]))
self.fit_values[i] = (self.fit_values[i] * prev_confidence) + \
(fits[i] * curr_confidence)
elif self.find_mode =='hist' and \
self.fit_err < (2 * self.constants['FIT_ERR_THRESHOLD']) and \
confidences[i] > self.constants['CONFIDENCE_THRESHOLD']:
self.fit_err = 0
self.fit_values[i] = fits[i]
# Update the confidences
if fits[0] is not None and fits[1] is not None:
prev_confidence = self.constants['FIT_AVG_WEIGHT'] + \
((1 - self.constants['FIT_AVG_WEIGHT']) * self.fit_confidence /
(self.fit_confidence + np.mean(confidences)))
curr_confidence = (1 - self.constants['FIT_AVG_WEIGHT']) * np.mean(confidences) / \
(self.fit_confidence + np.mean(confidences))
self.fit_confidence = (self.fit_confidence * prev_confidence) + \
(np.mean(confidences) * curr_confidence)
# Draw the lanes
if self.fit_values[0] is not None and self.fit_values[1] is not None:
overlay = self.__draw_overlay(binary)
else:
overlay = np.zeros_like(frame)
# Transform back to the original perspective and add the overlay onto the original image
overlay_perspective = perspective_transformer.inverse_transform(
overlay)
result = cv2.addWeighted(overlay_perspective, .7, frame, 1., 0.)
# Compute the curvature
curvature = self.__compute_curvature(np.mean(self.fit_values, 0),
self.img_sz[0] / 2)
str_curv = 'Curvature = ' + str(np.round(curvature, 2))
cv2.putText(result, str_curv, (500, 400), self.font, 1, (255, 0, 0), 1)
# Compute the offset from the middle
dist_offset = (((self.lane_bottom[1] - self.lane_bottom[0]) -
(self.img_sz[1] / 2)) /
(self.lane_bottom[1] - self.lane_bottom[0]))
dist_offset = np.round(dist_offset * 100, 2)
str_offset = 'Lane deviation: ' + str(dist_offset) + '%.'
if dist_offset > 20.:
cv2.putText(result, str_offset, (500, 700), self.font, 1,
(255, 0, 0), 1)
else:
cv2.putText(result, str_offset, (500, 700), self.font, 1,
(0, 255, 255), 1)
"""
DEBUG CODE
"""
if self.debug_level > 0:
# Write up the confidence
str_conf = 'Confidence = ' + str(np.round(self.fit_confidence, 2))
cv2.putText(result, str_conf, (30, 120), self.font, 1, (255, 0, 0),
2)
# Raw confidence
str_conf = 'Raw confidence: Left = ' + str(
np.round(confidences[0], 2)) + '; Right = ' + str(
np.round(confidences[1], 2))
cv2.putText(result, str_conf, (30, 150), self.font, 1, (255, 0, 0),
2)
# Error in fit values
str_err = 'Error in fit values = ' + str(np.round(self.fit_err, 2))
cv2.putText(result, str_err, (30, 180), self.font, 1, (255, 0, 0),
2)
# Draw boxes for each found marking
box_marked = np.copy(birdseye)
size = frame.shape[0] / self.constants['HIST_VERT_SLICES']
for idx, marking in enumerate(lane_markings):
start = int(size *
(self.constants['HIST_VERT_SLICES'] - idx - 1))
end = int(size * (self.constants['HIST_VERT_SLICES'] - idx))
if marking[0] is not None:
cv2.rectangle(box_marked, (int(marking[0]) - 100, end),
(int(marking[0]) + 100, start), (255), 10)
if marking[1] is not None:
cv2.rectangle(box_marked, (int(marking[1]) - 100, end),
(int(marking[1]) + 100, start), (255), 10)
# Mark the ROI in the image
marked = np.copy(frame)
cv2.line(marked, (self.ROI[0][0], self.ROI[0][1]),
(self.ROI[1][0], self.ROI[1][1]), (255, 0, 0), 10)
cv2.line(marked, (self.ROI[2][0], self.ROI[2][1]),
(self.ROI[3][0], self.ROI[3][1]), (255, 0, 0), 10)
cv2.line(marked, (self.ROI[0][0], self.ROI[0][1]),
(self.ROI[3][0], self.ROI[3][1]), (255, 0, 0), 10)
cv2.line(marked, (self.ROI[1][0], self.ROI[1][1]),
(self.ROI[2][0], self.ROI[2][1]), (255, 0, 0), 10)
str_conf = 'ROI Stable = ' + str(self.roi_stable)
cv2.putText(marked, str_conf, (30, 60), self.font, 1, (255, 0, 0),
2)
str_conf = 'Lane Width = ' + str(self.lane_width)
cv2.putText(marked, str_conf, (30, 90), self.font, 1, (255, 0, 0),
2)
diagScreen = np.zeros((1080, 1920, 3), dtype=np.uint8)
diagScreen[0:720,
0:1280] = cv2.resize(result, (1280, 720),
interpolation=cv2.INTER_AREA)
marked = cv2.copyMakeBorder(marked,
10,
10,
10,
10,
cv2.BORDER_CONSTANT,
value=(255, 255, 0))
marked = cv2.resize(marked, (320, 240),
interpolation=cv2.INTER_AREA)
diagScreen[0:240, 1280:1600] = marked
birdseye = cv2.copyMakeBorder(birdseye,
10,
10,
10,
10,
cv2.BORDER_CONSTANT,
value=(255, 255, 0))
birdseye = cv2.resize(birdseye, (320, 240),
interpolation=cv2.INTER_AREA)
diagScreen[240:480, 1280:1600] = birdseye
box_marked = cv2.copyMakeBorder(box_marked,
10,
10,
10,
10,
cv2.BORDER_CONSTANT,
value=(255, 255, 0))
box_marked = cv2.resize(box_marked, (320, 240),
interpolation=cv2.INTER_AREA)
diagScreen[480:720, 1280:1600] = box_marked
motion = cv2.cvtColor(motion, cv2.COLOR_GRAY2RGB) * 255.
motion = cv2.copyMakeBorder(motion,
10,
10,
10,
10,
cv2.BORDER_CONSTANT,
value=(255, 255, 0))
motion = cv2.resize(motion, (320, 240),
interpolation=cv2.INTER_AREA)
diagScreen[720:960, 0:320] = motion
mask = cv2.cvtColor(masks[0] + masks[1], cv2.COLOR_GRAY2RGB) * 255.
mask = cv2.copyMakeBorder(mask,
10,
10,
10,
10,
cv2.BORDER_CONSTANT,
value=(255, 255, 0))
mask = cv2.resize(mask, (320, 240), interpolation=cv2.INTER_AREA)
diagScreen[720:960, 320:640] = mask
overlay = cv2.copyMakeBorder(overlay,
10,
10,
10,
10,
cv2.BORDER_CONSTANT,
value=(255, 255, 0))
overlay = cv2.resize(overlay, (320, 240),
interpolation=cv2.INTER_AREA)
diagScreen[720:960, 960:1280] = overlay
overlay_perspective = cv2.copyMakeBorder(overlay_perspective,
10,
10,
10,
10,
cv2.BORDER_CONSTANT,
value=(255, 255, 0))
overlay_perspective = cv2.resize(overlay_perspective, (320, 240),
interpolation=cv2.INTER_AREA)
diagScreen[720:960, 1280:1600] = overlay_perspective
# Show off the plots if required
if self.debug_level > 1:
fig = plt.figure()
fig.add_subplot(3, 4,
1), plt.imshow(frame), plt.title('Original')
fig.add_subplot(3, 4, 2), plt.imshow(dist_correct), plt.title(
'Distortion corrected')
fig.add_subplot(3, 4,
3), plt.imshow(marked), plt.title('ROI marked')
fig.add_subplot(
3, 4, 4), plt.imshow(birdseye), plt.title('Bird\'s view')
fig.add_subplot(3, 4, 5), plt.imshow(
binary, cmap='gray'), plt.title('Thresholded')
fig.add_subplot(3, 4, 6), plt.imshow(
box_marked, cmap='gray'), plt.title('Histogram find')
fig.add_subplot(3, 4, 7), plt.imshow(
masks[0] + masks[1],
cmap='gray'), plt.title('Masked pixels')
fig.add_subplot(3, 4,
8), plt.imshow(overlay), plt.title('Overlay')
fig.add_subplot(3, 4, 9), plt.imshow(
overlay_perspective), plt.title('Perspective overlay')
fig.add_subplot(3, 4,
10), plt.imshow(result), plt.title('Result')
plt.show()
if self.debug_level > 2:
# Construct a diagnostic view
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-00orig.jpg', cv2.cvtColor(frame, cv2.COLOR_RGB2BGR))
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-01roi.jpg', cv2.cvtColor(marked, cv2.COLOR_RGB2BGR))
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-02distortion.jpg',
cv2.cvtColor(dist_correct, cv2.COLOR_RGB2BGR))
cv2.imwrite(
self.debug_loc +
'/{0:04d}'.format(self.cnt) + '-03birdseye.jpg',
cv2.cvtColor(birdseye, cv2.COLOR_RGB2BGR))
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-04threshold.jpg',
cv2.cvtColor(binary, cv2.COLOR_GRAY2BGR) * 255.)
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-05motion.jpg',
cv2.cvtColor(motion, cv2.COLOR_GRAY2BGR) * 255.)
cv2.imwrite(
self.debug_loc +
'/{0:04d}'.format(self.cnt) + '-06select.jpg',
cv2.cvtColor(box_marked, cv2.COLOR_RGB2BGR))
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-07pixels.jpg',
cv2.cvtColor(masks[0] + masks[1], cv2.COLOR_GRAY2BGR) *
255.)
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-08overlay.jpg', cv2.cvtColor(overlay, cv2.COLOR_RGB2BGR))
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-09overlay_p.jpg',
cv2.cvtColor(overlay_perspective, cv2.COLOR_RGB2BGR))
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-10result.jpg', cv2.cvtColor(result, cv2.COLOR_RGB2BGR))
cv2.imwrite(
self.debug_loc + '/{0:04d}'.format(self.cnt) +
'-11diag.png', cv2.cvtColor(diagScreen, cv2.COLOR_RGB2BGR))
self.cnt += 1
return result