class LaneTracker(object):
"""
Tracks the lane in a series of consecutive frames.
"""
def __init__(self, first_frame, n_windows=9):
"""
Initialises a tracker object.
Parameters
----------
first_frame : First frame of the frame series. We use it to get dimensions and initialise values.
n_windows : Number of windows we use to track each lane edge.
"""
(self.h, self.w, _) = first_frame.shape
self.win_n = n_windows
self.left = None
self.right = None
self.l_windows = []
self.r_windows = []
self.initialize_lines(first_frame)
def initialize_lines(self, frame):
"""
Finds starting points for left and right lines (e.g. lane edges) and initialises Window and Line objects.
Parameters
----------
frame : Frame to scan for lane edges.
"""
# Take a histogram of the bottom half of the image
edges = get_edges(frame)
(flat_edges, _) = flatten_perspective(edges)
histogram = np.sum(flat_edges[int(self.h / 2):, :], axis=0)
nonzero = flat_edges.nonzero()
# Create empty lists to receive left and right lane pixel indices
l_indices = np.empty([0], dtype=np.int)
r_indices = np.empty([0], dtype=np.int)
window_height = int(self.h / self.win_n)
for i in range(self.win_n):
l_window = Window(
y1=self.h - (i + 1) * window_height,
y2=self.h - i * window_height,
x=self.l_windows[-1].x if len(self.l_windows) > 0 else np.argmax(histogram[:self.w // 2])
)
r_window = Window(
y1=self.h - (i + 1) * window_height,
y2=self.h - i * window_height,
x=self.r_windows[-1].x if len(self.r_windows) > 0 else np.argmax(histogram[self.w // 2:]) + self.w // 2
)
# Append nonzero indices in the window boundary to the lists
l_indices = np.append(l_indices, l_window.pixels_in(nonzero), axis=0)
r_indices = np.append(r_indices, r_window.pixels_in(nonzero), axis=0)
self.l_windows.append(l_window)
self.r_windows.append(r_window)
self.left = Line(x=nonzero[1][l_indices], y=nonzero[0][l_indices], h=self.h, w = self.w)
self.right = Line(x=nonzero[1][r_indices], y=nonzero[0][r_indices], h=self.h, w = self.w)
def scan_frame_with_windows(self, frame, windows):
"""
Scans a frame using initialised windows in an attempt to track the lane edges.
Parameters
----------
frame : New frame
windows : Array of windows to use for scanning the frame.
Returns
-------
A tuple of arrays containing coordinates of points found in the specified windows.
"""
indices = np.empty([0], dtype=np.int)
nonzero = frame.nonzero()
window_x = None
for window in windows:
indices = np.append(indices, window.pixels_in(nonzero, window_x), axis=0)
window_x = window.mean_x
return (nonzero[1][indices], nonzero[0][indices])
def process(self, frame, draw_lane=True, draw_statistics=True):
"""
Performs a full lane tracking pipeline on a frame.
Parameters
----------
frame : New frame to process.
draw_lane : Flag indicating if we need to draw the lane on top of the frame.
draw_statistics : Flag indicating if we need to render the debug information on top of the frame.
Returns
-------
Resulting frame.
"""
edges = get_edges(frame)
(flat_edges, unwarp_matrix) = flatten_perspective(edges)
(l_x, l_y) = self.scan_frame_with_windows(flat_edges, self.l_windows)
self.left.process_points(l_x, l_y)
(r_x, r_y) = self.scan_frame_with_windows(flat_edges, self.r_windows)
self.right.process_points(r_x, r_y)
if draw_statistics:
edges = get_edges(frame, separate_channels=True)
debug_overlay = self.draw_debug_overlay(flatten_perspective(edges)[0])
top_overlay = self.draw_lane_overlay(flatten_perspective(frame)[0])
debug_overlay = cv2.resize(debug_overlay, (0, 0), fx=0.3, fy=0.3)
top_overlay = cv2.resize(top_overlay, (0, 0), fx=0.3, fy=0.3)
frame[:250, :, :] = frame[:250, :, :] * .4
(h, w, _) = debug_overlay.shape
frame[20:20 + h, 20:20 + w, :] = debug_overlay
frame[20:20 + h, 20 + 20 + w:20 + 20 + w + w, :] = top_overlay
text_x = 20 + 20 + w + w + 20
self.draw_text(frame, 'Radius of curvature: {} m'.format(self.radius_of_curvature()), text_x, 80)
self.draw_text(frame, 'Distance (left): {:.1f} m'.format(self.left.camera_distance()), text_x, 140)
self.draw_text(frame, 'Distance (right): {:.1f} m'.format(self.right.camera_distance()), text_x, 200)
if draw_lane:
frame = self.draw_lane_overlay(frame, unwarp_matrix)
return frame
def draw_text(self, frame, text, x, y):
cv2.putText(frame, text, (x, y), cv2.FONT_HERSHEY_SIMPLEX, .8, (255, 255, 255), 2)
def draw_debug_overlay(self, binary, lines=True, windows=True):
"""
Draws an overlay with debugging information on a bird's-eye view of the road (e.g. after applying perspective
transform).
Parameters
----------
binary : Frame to overlay.
lines : Flag indicating if we need to draw lines.
windows : Flag indicating if we need to draw windows.
Returns
-------
Frame with an debug information overlay.
"""
if len(binary.shape) == 2:
image = np.dstack((binary, binary, binary))
else:
image = binary
if windows:
for window in self.l_windows:
coordinates = window.coordinates()
cv2.rectangle(image, coordinates[0], coordinates[1], (1., 1., 0), 2)
for window in self.r_windows:
coordinates = window.coordinates()
cv2.rectangle(image, coordinates[0], coordinates[1], (1., 1., 0), 2)
if lines:
cv2.polylines(image, [self.left.get_points()], False, (1., 0, 0), 2)
cv2.polylines(image, [self.right.get_points()], False, (1., 0, 0), 2)
return image * 255
def draw_lane_overlay(self, image, unwarp_matrix=None):
"""
Draws an overlay with tracked lane applying perspective unwarp to project it on the original frame.
Parameters
----------
image : Original frame.
unwarp_matrix : Transformation matrix to unwarp the bird's eye view to initial frame. Defaults to `None` (in
which case no unwarping is applied).
Returns
-------
Frame with a lane overlay.
"""
# Create an image to draw the lines on
overlay = np.zeros_like(image).astype(np.uint8)
points = np.vstack((self.left.get_points(), np.flipud(self.right.get_points())))
# Draw the lane onto the warped blank image
cv2.fillPoly(overlay, [points], (0, 255, 0))
if unwarp_matrix is not None:
# Warp the blank back to original image space using inverse perspective matrix (Minv)
overlay = cv2.warpPerspective(overlay, unwarp_matrix, (image.shape[1], image.shape[0]))
# Combine the result with the original image
return cv2.addWeighted(image, 1, overlay, 0.3, 0)
def radius_of_curvature(self):
"""
Calculates radius of the lane curvature by averaging curvature of the edge lines.
Returns
-------
Radius of the lane curvature in meters.
"""
return int(np.average([self.left.radius_of_curvature(), self.right.radius_of_curvature()]))
class LaneDetection(object):
"""Class to dectect and draw the lanes on a serise of images/frames"""
def __init__(self,
h,
w,
CALIBRATION_IMAGES,
NX,
NY,
nwindows=9,
margin=100,
tol=50):
#Variables for the image size
self.h = h
self.w = w
#Initlaise objects to perform the calibration and perspective transforms
self.calibrator = CameraCalibration(CALIBRATION_IMAGES, NX, NY)
self.perspective_transform = PerspectiveTransform()
#Initalise line objects to keep track of the properties of the fitted lines
#overtime
self.left_line = Line(self.h, self.w)
self.right_line = Line(self.h, self.w)
#Initalise window objects to keep track of the
self.nwindows = nwindows
self.window_height = np.int(self.h / nwindows)
self.left_window = []
self.right_window = []
self.margin = margin
self.tol = tol
self.initalise_windows()
def process_frame(self, image, debug=True):
# Step 1: Correct for the camera image distortion
corrected_image = self.calibrator.undistort(image)
# Step 2: Binary threshold image to detect the lane lines
threshold_image = detect_edges(corrected_image)
# Step 3: Apply a perspective transform to the image for birds eye view
perspective_image = self.perspective_transform.warp(threshold_image)
# Step 4: Finding and fitting a polynomial line to the lane lines
if not (self.left_line.detected) or not (self.right_line.detected):
self.fit_windows(perspective_image)
if debug:
edges_debug = detect_edges(corrected_image, True)
perspective_debug = self.perspective_transform.warp(
edges_debug)
debug_image = self.debug_visualisation_windows(
perspective_debug)
else:
self.fit_region(perspective_image)
if debug:
edges_debug = detect_edges(corrected_image, True)
perspective_debug = self.perspective_transform.warp(
edges_debug)
debug_image = self.debug_visualisation_region(
perspective_debug)
if debug:
overhead_lane = self.draw_lanes(
self.perspective_transform.warp(corrected_image), True)
debug_viewer = cv2.resize(debug_image, (0, 0), fx=0.3, fy=0.3)
overhead_viewer = cv2.resize(overhead_lane, (0, 0), fx=0.3, fy=0.3)
#Make top region of image darker
corrected_image[:250, :, :] = corrected_image[:250, :, :] * 0.4
debug_h = debug_viewer.shape[0]
debug_w = debug_viewer.shape[1]
#Overlay the debug view to the top of the image
corrected_image[20:20 + debug_h, 20:20 + debug_w, :] = debug_viewer
#Overlay the overhead view to the top of the image
corrected_image[20:20 + debug_h, 40 + 3 * 20 + 2 * debug_w:3 * 20 +
3 * debug_w + 40, :] = overhead_viewer
#Print the lane distance and radius of curv to the screen
x_location = 3 * 20 + debug_w
self.text_to_image(
corrected_image, 'Radius of curvature: {:.1f} m'.format(
self.avg_radius_of_curvature()), x_location, 40)
self.text_to_image(
corrected_image, 'Left distance: {:.3f} m'.format(
self.left_line.camera_distance()), x_location, 100)
self.text_to_image(
corrected_image, 'Right distance: {:.3f} m'.format(
self.right_line.camera_distance()), x_location, 160)
self.text_to_image(
corrected_image, 'Center distance: {:.3f} m'.format(
self.left_line.camera_distance() -
self.right_line.camera_distance()), x_location, 220)
identified_lane = self.draw_lanes(corrected_image)
return identified_lane
def text_to_image(self, image, text, coordinate_x, coordinate_y):
cv2.putText(image, text, (coordinate_x, coordinate_y),
cv2.FONT_HERSHEY_SIMPLEX, .8, (255, 255, 255))
def fit_region(self, warped_image):
left_fit = self.left_line.average_fits()
right_fit = self.right_line.average_fits()
if not (self.left_line.detected) or not (self.right_line.detected):
print("No current coefficents")
return
nonzero = warped_image.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
margin = self.margin
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]
self.left_line.compute_line(lefty, leftx)
self.right_line.compute_line(righty, rightx)
def initalise_windows(self):
leftx = np.int(self.w / 4)
rightx = np.int(3 * (self.w / 4))
for i in range(self.nwindows):
left_window = Window(self.h - (i + 1) * self.window_height,
self.h - i * self.window_height, leftx,
self.margin, self.tol)
right_window = Window(self.h - (i + 1) * self.window_height,
self.h - i * self.window_height, rightx,
self.margin, self.tol)
self.left_window.append(left_window)
self.right_window.append(right_window)
def fit_windows(self, image_edges):
#Create a histrogram of the bottom half of the image and find two peaks
histogram = np.sum(image_edges[int(image_edges.shape[0] / 2):, :],
axis=0)
midpoint = np.int(histogram.shape[0] / 2)
leftx_current = np.argmax(histogram[:midpoint])
rightx_current = np.argmax(histogram[midpoint:]) + midpoint
# Find the locations of the non-zero pixels
nonzero = image_edges.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Create empty lists for the left and right lane pixel indices (might not be required)
left_inds = []
right_inds = []
for i in range(self.nwindows):
if i == 0:
self.left_window[i].x_center = leftx_current
self.right_window[i].x_center = rightx_current
else:
self.left_window[i].x_center = self.left_window[
i - 1].x_next_window
self.right_window[i].x_center = self.right_window[
i - 1].x_next_window
left_inds.append(self.left_window[i].find_pixels(nonzero))
right_inds.append(self.right_window[i].find_pixels(nonzero))
# Concatenate the arrays of indices
left_inds = np.concatenate(left_inds)
right_inds = np.concatenate(right_inds)
# Extract left and right line pixel positions
leftx = nonzerox[left_inds]
lefty = nonzeroy[left_inds]
rightx = nonzerox[right_inds]
righty = nonzeroy[right_inds]
# Find lines to the extracted pixels
self.left_line.compute_line(lefty, leftx)
self.right_line.compute_line(righty, rightx)
def debug_visualisation_windows(self, image, lines=True, windows=True):
out_img = image
if windows:
for window in self.left_window:
coordinate = window.get_coordinate()
cv2.rectangle(out_img, coordinate[0], coordinate[1],
(0, 255, 0), 2)
for window in self.right_window:
coordinate = window.get_coordinate()
cv2.rectangle(out_img, coordinate[0], coordinate[1],
(0, 255, 0), 2)
if lines:
#Print the average fitted lines
y, left_fitx = self.left_line.generate_line()
if left_fitx is not None:
cv2.polylines(out_img,
[np.stack((left_fitx, y)).astype(np.int).T],
False, (255, 0, 255), 2)
y, right_fitx = self.right_line.generate_line()
if right_fitx is not None:
cv2.polylines(out_img,
[np.stack((right_fitx, y)).astype(np.int).T],
False, (255, 0, 255), 2)
#Print the current fitted lines
y, left_fit_currentx = self.left_line.generate_line_current()
if left_fit_currentx is not None:
cv2.polylines(
out_img,
[np.stack((left_fit_currentx, y)).astype(np.int).T], False,
(255, 128, 0), 2)
y, right_fit_currentx = self.right_line.generate_line_current()
if right_fit_currentx is not None:
cv2.polylines(
out_img,
[np.stack((right_fit_currentx, y)).astype(np.int).T],
False, (255, 128, 0), 2)
return out_img
def debug_visualisation_region(self, image, lines=True, region=True):
out_img = image #*255
window_img = np.zeros_like(out_img)
if region:
y, left_fitx = self.left_line.generate_line_current()
y, right_fitx = self.right_line.generate_line_current()
margin = self.margin
# 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, y]))])
left_line_window2 = np.array(
[np.flipud(np.transpose(np.vstack([left_fitx + margin, y])))])
left_line_pts = np.hstack((left_line_window1, left_line_window2))
right_line_window1 = np.array(
[np.transpose(np.vstack([right_fitx - margin, y]))])
right_line_window2 = np.array(
[np.flipud(np.transpose(np.vstack([right_fitx + margin, y])))])
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))
out_img = cv2.addWeighted(out_img, 1.0, window_img, 0.3, 0)
if lines:
#Print the average fitted lines
y, left_fitx = self.left_line.generate_line()
if left_fitx is not None:
cv2.polylines(out_img,
[np.stack((left_fitx, y)).astype(np.int).T],
False, (255, 0, 255), 2)
y, right_fitx = self.right_line.generate_line()
if right_fitx is not None:
cv2.polylines(out_img,
[np.stack((right_fitx, y)).astype(np.int).T],
False, (255, 0, 255), 2)
#Print the current fitted lines
y, left_fit_currentx = self.left_line.generate_line_current()
if left_fit_currentx is not None:
cv2.polylines(
out_img,
[np.stack((left_fit_currentx, y)).astype(np.int).T], False,
(255, 128, 0), 2)
y, right_fit_currentx = self.right_line.generate_line_current()
if right_fit_currentx is not None:
cv2.polylines(
out_img,
[np.stack((right_fit_currentx, y)).astype(np.int).T],
False, (255, 128, 0), 2)
return out_img
def draw_lanes(self, undist, birdseye=False):
y, left_fitx = self.left_line.generate_line()
y, right_fitx = self.right_line.generate_line()
# Create an image to draw the lines on
warp_zero = np.zeros_like(undist[:, :, 0]).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
if left_fitx is None or right_fitx is None:
return undist
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, y]))])
pts_right = np.array(
[np.flipud(np.transpose(np.vstack([right_fitx, y])))])
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))
if not (birdseye):
lane = self.perspective_transform.inverse_warp(color_warp)
else:
lane = color_warp
# Warp the blank back to original image space using inverse perspective matrix (Minv)
#newwarp = cv2.warpPerspective(color_warp, Minv, (image.shape[1], image.shape[0]))
# Combine the result with the original image
result = cv2.addWeighted(undist, 1, lane, 0.3, 0)
return result
def avg_radius_of_curvature(self):
return np.average([
self.left_line.radius_of_curvature(),
self.right_line.radius_of_curvature()
])
