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() ])