class LaneFinder:
def __init__(self, parallel_thresh=(0.0003, 0.55), lane_dist_thresh=(300, 460), n_frames=1):
self.n_frames = n_frames
self.left_line = None
self.right_line = None
self.curvature_radius_left = 0
self.curvature_radius_right = 0
self.center_poly = None
self.offset = 0
self.diag_frame_count = 0
self.parallel_thresh = parallel_thresh
self.lane_dist_thresh = lane_dist_thresh
# Define the source points
self.src = np.float32([[ 300, Y_BOTTOM], # bottom left
[ 580, Y_HORIZON], # top left
[ 730, Y_HORIZON], # top right
[1100, Y_BOTTOM]]) # bottom right
# Define the destination points
self.dst = np.float32([
(self.src[0][0] + OFFSET, Y_BOTTOM),
(self.src[0][0] + OFFSET, 0),
(self.src[-1][0] - OFFSET, 0),
(self.src[-1][0] - OFFSET, Y_BOTTOM)])
# Compute the perspective transform
self.M = cv2.getPerspectiveTransform(self.src, self.dst)
self.Minv = cv2.getPerspectiveTransform(self.dst, self.src)
# Detect lane lines using the sliding window approach for the first frame
def process_image(self, orig_image, diag=False):
rawImageProcessor = RawImageProcessor()
# 1. Correct image distortion
image_undist = rawImageProcessor.undistort(orig_image)
# 2. Create thresholded binary image
image_thres = rawImageProcessor.binary_pipeline(image_undist)
# 3. Apply perspective transform to create a bird's-eye view
image_warp = self.__warp(image_thres)
# 4. Detect and plot the lane
image_final, out_img = self.__detect_lanes(image_warp, image_undist)
if (diag==False):
return image_final
# 3. Optional: return the diagnostic image instead
diag_image = self.__create_diag_output(image_undist, image_thres, out_img, image_final)
return diag_image
# Detect lane lines using the sliding window approach for the first frame
# After the first frame the algorith will search in a margin around the previous line position.
# This appraoch speeds up video processing
def process_video_frame(self, frame):
rawImageProcessor = RawImageProcessor()
# 1. Correct image distortion
image_undist = rawImageProcessor.undistort(frame)
# 2. Create thresholded binary image
image_thres = rawImageProcessor.binary_pipeline(image_undist)
# 3. Apply perspective transform to create a bird's-eye view
image_warp = self.__warp(image_thres)
# 4. Detect and plot the lane
if (self.left_line is not None and self.right_line is not None and self.left_line.best_fit_exits()
and self.right_line.best_fit_exits()):
# Use existing polynomes as starting point
lanes_found, image_final, out_img = self.__search_around_poly(image_warp, image_undist)
if not lanes_found:
# Fallback, start from scratch at current frame
image_final, out_img = self.__detect_lanes(image_warp, image_undist)
else:
# Start from scratch
image_final, out_img = self.__detect_lanes(image_warp, image_undist)
out_img = self.__create_diag_output(image_undist, image_thres, out_img, image_final)
if self.diag_frame_count == 12:
mpimg.imsave('Term1/CarND-Advanced-Lane-Lines/output_images/video_frame_output.jpg', out_img)
self.diag_frame_count += 1
return out_img
# Uses a sliding window algorithm to identify pixels within the image
# which are part of a lane line
def __sliding_window(self, binary_warped):
# Take a histogram of the bottom half of the image
histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0)
# Create an output image to draw on and visualize the result
out_img = np.dstack((binary_warped, binary_warped, binary_warped))
# Find the peak of the left and right halves of the histogram
# These will be the starting point for the left and right lines
midpoint = np.int(histogram.shape[0]//2)
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
# HYPERPARAMETERS
# Choose the number of sliding windows
nwindows = 9
# Set the width of the windows +/- margin
margin = 90
# Set minimum number of pixels found to recenter window
minpix = 60
# Set height of windows - based on nwindows above and image shape
window_height = np.int(binary_warped.shape[0]//nwindows)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Current positions to be updated later for each window in nwindows
leftx_current = leftx_base
rightx_current = rightx_base
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# Step through the windows one by one
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = binary_warped.shape[0] - (window+1)*window_height
win_y_high = binary_warped.shape[0] - window*window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
# Draw the windows on the visualization image
cv2.rectangle(out_img,(win_xleft_low,win_y_low),
(win_xleft_high,win_y_high),(0,255,0), 2)
cv2.rectangle(out_img,(win_xright_low,win_y_low),
(win_xright_high,win_y_high),(0,255,0), 2)
# Identify the nonzero pixels in x and y within the window #
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
(nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
(nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# If you found > minpix pixels, recenter next window on their mean position
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices (previously was a list of lists of pixels)
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
# Extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
return leftx, lefty, rightx, righty, out_img
# Determines if detected lane lines are plausible lines based on curvature and distance
def __check_lane_quality(self, left, right):
if len(left[0]) < MIN_POINTS_REQUIRED or len(right[0]) < MIN_POINTS_REQUIRED:
return False
else:
new_left = Line(detected_y=left[0], detected_x=left[1])
new_right = Line(detected_y=right[0], detected_x=right[1])
parallel_check = new_left.check_lines_parallel(new_right, threshold=self.parallel_thresh)
dist = new_left.distance_between_lines(new_right)
dist_check = self.lane_dist_thresh[0] < dist < self.lane_dist_thresh[1]
return parallel_check & dist_check
# Check detected lines against each other and against previous frames' lines to ensure they are valid lines
def __validate_lines(self, left_points, right_points):
left_detected = False
right_detected = False
if self.__check_lane_quality(left_points, right_points):
left_detected = True
right_detected = True
elif self.left_line is not None and self.right_line is not None:
if self.__check_lane_quality(left_points, (self.left_line.ally, self.left_line.allx)):
left_detected = True
if self.__check_lane_quality(right_points, (self.right_line.ally, self.right_line.allx)):
right_detected = True
return left_detected, right_detected
# Calculate the line curvature (in meters)
def __calc_curvature_radius(self, fit_cr):
y = np.array(np.linspace(0, IMAGE_MAX_Y, num=10))
x = np.array([fit_cr(x) for x in y])
y_eval = np.max(y)
fit_cr = np.polyfit(y * YM_PER_PIXEL, x * XM_PER_PIXEL, 2)
curverad = ((1 + (2 * fit_cr[0] * y_eval /
2. + fit_cr[1]) ** 2) ** 1.5) / np.absolute(2 * fit_cr[0])
return curverad
# Perspective transformation to bird's eye view
def __warp(self, image):
return cv2.warpPerspective(image, self.M, (image.shape[1], image.shape[0]), flags=cv2.INTER_LINEAR)
# Reverse perspective transformation
def __warp_inv(self, image):
return cv2.warpPerspective(image, self.Minv, (image.shape[1], image.shape[0]), flags=cv2.INTER_LINEAR)
# Detect lane lines using the sliding window approach
def __detect_lanes(self, image_warp, image_undist):
left_detected = right_detected = False
# Get lane pixels for left and right lane using sliding window approach
# out_img contains visualization of the process
leftx, lefty, rightx, righty, out_img = self.__sliding_window(image_warp)
## Visualization ##
# Colors in the left and right lane regions
out_img[lefty, leftx] = [255, 0, 0]
out_img[righty, rightx] = [0, 0, 255]
if not left_detected or not right_detected:
left_detected, right_detected = self.__validate_lines((leftx, lefty ),
(rightx, righty ))
# Update line information
if left_detected:
if self.left_line is not None:
self.left_line.update(y = leftx, x = lefty)
else:
self.left_line = Line(self.n_frames,
detected_y = leftx,
detected_x = lefty)
if right_detected:
if self.right_line is not None:
self.right_line.update(y = rightx, x = righty)
else:
self.right_line = Line(self.n_frames,
detected_y = rightx,
detected_x = righty)
# Draw the lane and add additional information
if self.left_line is not None and self.right_line is not None:
self.curvature_radius_left = self.__calc_curvature_radius(self.left_line.best_fit_poly)
self.curvature_radius_right = self.__calc_curvature_radius(self.right_line.best_fit_poly)
self.center_poly = (self.left_line.best_fit_poly + self.right_line.best_fit_poly) / 2
self.offset = (image_undist.shape[1] / 2 - self.center_poly(IMAGE_MAX_Y)) * XM_PER_PIXEL
image_undist = self.__plot_lane(image_undist, image_warp)
return image_undist, out_img
# Another approach to detec lane lines. This one uses the information
# about line location from the previous video frame to narrow down
# the area of intest.
def __search_around_poly(self, image_warp, image_undist):
# HYPERPARAMETER
# Choose the width of the margin around the previous polynomial to search
# The quiz grader expects 100 here, but feel free to tune on your own!
margin = 70
out_img = None
# Grab activated pixels
nonzero = image_warp.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
left_fit = self.left_line.best_fit
right_fit = self.right_line.best_fit
left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy +
left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) +
left_fit[1]*nonzeroy + left_fit[2] + margin)))
right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy +
right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) +
right_fit[1]*nonzeroy + right_fit[2] + margin)))
# Again, extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
left_detected, right_detected =self.__validate_lines((leftx, lefty), (rightx, righty))
if left_detected and right_detected:
self.left_line.update(y = leftx, x = lefty)
self.right_line.update(y = rightx, x = righty)
## Visualization ##
# Create an image to draw on and an image to show the selection window
ploty = np.linspace(0, image_warp.shape[0]-1, image_warp.shape[0])
left_fitx = (self.left_line.current_fit[0]*ploty**2 + self.left_line.current_fit[1]*ploty +
self.left_line.current_fit[2])
right_fitx = (self.right_line.current_fit[0]*ploty**2 + self.right_line.current_fit[1]*ploty +
self.right_line.current_fit[2])
out_img = np.dstack((image_warp, image_warp, image_warp))*255
window_img = np.zeros_like(out_img)
# Color in left and right line pixels
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
# Generate a polygon to illustrate the search window area
# And recast the x and y points into usable format for cv2.fillPoly()
left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin,
ploty])))])
left_line_pts = np.hstack((left_line_window1, left_line_window2))
right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin,
ploty])))])
right_line_pts = np.hstack((right_line_window1, right_line_window2))
# Draw the lane onto the warped blank image
cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
# Plot the polynomial lines onto the image
out_img = cv2.addWeighted(out_img, 0.9, window_img, 0.1, 0)
cv2.polylines(out_img, np.int_([np.array([np.transpose(np.vstack([right_fitx, ploty]))])]), False, (255, 240, 0), thickness=3)
cv2.polylines(out_img, np.int_([np.array([np.transpose(np.vstack([left_fitx, ploty]))])]), False, (255, 240, 0), thickness=3)
## End visualization steps ##
# Draw the lane and add additional information
if self.left_line is not None and self.right_line is not None:
self.curvature_radius_left = self.__calc_curvature_radius(self.left_line.best_fit_poly)
self.curvature_radius_right = self.__calc_curvature_radius(self.right_line.best_fit_poly)
self.center_poly = (self.left_line.best_fit_poly + self.right_line.best_fit_poly) / 2
self.offset = (image_undist.shape[1] / 2 - self.center_poly(IMAGE_MAX_Y)) * XM_PER_PIXEL
image_undist = self.__plot_lane(image_undist, image_warp)
lanes_found = True
else:
lanes_found = False
return lanes_found, image_undist, out_img
# Plot the detected lane and information about curvature radius and car off-center
def __plot_lane(self, imgage_org, imgage_warp):
warp_zero = np.zeros_like(imgage_warp).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
# Recast the x and y points into usable format for cv2.fillPoly()
yrange = np.linspace(0, 720)
fitted_left_x = self.left_line.best_fit_poly(yrange)
fitted_right_x = self.right_line.best_fit_poly(yrange)
pts_left = np.array([np.transpose(np.vstack([fitted_left_x, yrange]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([fitted_right_x, yrange])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))
# Warp the blank image back to original image space using inverse perspective matrix (Minv)
newwarp = self.__warp_inv(color_warp)
# Combine the result with the original image
result = cv2.addWeighted(imgage_org, 1, newwarp, 0.3, 0)
# Add information overlay rectangle
font = cv2.FONT_HERSHEY_SIMPLEX
result_overlay = result.copy()
cv2.rectangle(result_overlay, (10, 10), (410, 150), (255, 255, 255), -1)
cv2.addWeighted(result_overlay, 0.7, result, 0.3, 0, result)
lane_center_x = int(self.center_poly(IMAGE_MAX_Y))
image_center_x = int(result.shape[1] / 2)
offset_from_centre = (image_center_x - lane_center_x) * XM_PER_PIXEL # in meters
# Add curvature and offset information
font_color = (60, 60, 60) # dark grey
left_right = 'left' if offset_from_centre < 0 else 'right'
cv2.putText(result, "{:>14}: {:.2f}m ({})".format("off-center", abs(offset_from_centre), left_right),
(24, 50), font, 0.8, font_color, 2)
text = "{:.1f}m".format(self.curvature_radius_left)
cv2.putText(result, "{:>15}: {}".format("Left curvature", text), (19, 90), font, 0.8, font_color, 2)
text = "{:.1f}m".format(self.curvature_radius_right)
cv2.putText(result, "{:>15}: {}".format("Right curvature", text), (15, 130), font, 0.8, font_color, 2)
return result
def __create_diag_output(self, image, preprocessed_image, warp_image, image_final):
# Create a combined image with final result and intermidiate processing steps
diag_output = np.zeros((1080, 1280, 3), dtype=np.uint8)
# Main screen
diag_output[0:720, 0:1280] = image_final
# Three screens along the bottom
diag_output[720:1080, 0:426] = cv2.resize(image, (426,360), interpolation=cv2.INTER_AREA) # original frame
color_thresh = np.dstack((preprocessed_image, preprocessed_image, preprocessed_image)) * 255
diag_output[720:1080, 426:852] = cv2.resize(color_thresh, (426,360), interpolation=cv2.INTER_AREA) # undistorted binary filtered
if len(warp_image.shape) == 2:
color_warp = np.dstack((warp_image, warp_image, warp_image)) * 255
else:
color_warp = warp_image
diag_output[720:1080, 852:1278] = cv2.resize(color_warp, (426,360), interpolation=cv2.INTER_AREA) # warped image
return diag_output
