def visualize_lanes_using_matplotlib(self, img, left_line=None, right_line=None): # Undistort, threshold, warp img = self.img_mgr.undistort(img) binary_warped = self.get_birdseye_binary_warped(img, undistort=False) # Set the width of the windows +/- margin margin = 100 if ((left_line == None) or (right_line == None)): left_line, right_line, out_img = self.find_lane_lines( binary_warped, margin=margin, method='sliding_windows') else: left_line, right_line, out_img = self.find_lane_lines( binary_warped, margin=margin, method='previous_fit', prev_left_line=left_line, prev_right_line=right_line) # Generate x and y values for plotting ploty = np.linspace(0, binary_warped.shape[0] - 1, binary_warped.shape[0]) result = self.draw_lane_on_image(img, binary_warped, ploty, left_line.best_fitx, right_line.best_fitx) # Visualize the lines f, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 6)) f.tight_layout() ax1.imshow(out_img) ax1.set_title('Birds Eye', fontsize=15) ax1.set_xlim(0, 1296) ax1.set_ylim(972, 0) ax2.imshow(result) ax2.set_title('Lane detected', fontsize=15) plt.subplots_adjust(left=0.05, right=0.95, top=0.9, bottom=0.) plt.show() print("left curve {:.3f} px, right curve {:.3f} px".format( left_line.radius_of_curvature_px, right_line.radius_of_curvature_px)) print("left curve {:.3f} m, right curve {:.3f} m".format( left_line.radius_of_curvature_m, right_line.radius_of_curvature_m)) lane_center_px = LaneMath.get_lane_center_in_pixels( ploty, left_line.best_fit, right_line.best_fit) lane_offset_m = LaneMath.get_lane_offset_in_meters( binary_warped.shape[1], lane_center_px) print("lane center {:.3f} px, offset from lane center {:.3f} m".format( lane_center_px, lane_offset_m)) return left_line, right_line
def visualize_lane_using_basicScreen(self, img, ploty, left_line=None, right_line=None): img = self.img_mgr.undistort(img) binary_warped = self.get_birdseye_binary_warped(img, undistort=False) # Set the width of the windows +/- margin margin = 100 if ((left_line == None) or (right_line == None)): left_line, right_line, out_img = self.find_lane_lines( binary_warped, margin=margin, method='sliding_windows', produce_out_img=False) else: left_line, right_line, out_img = self.find_lane_lines( binary_warped, margin=margin, method='previous_fit', prev_left_line=left_line, prev_right_line=right_line, produce_out_img=False) undistorted_overlayed = self.draw_lane_on_image( img, binary_warped, ploty, left_line.best_fitx, right_line.best_fitx) curverad = (left_line.radius_of_curvature_m + right_line.radius_of_curvature_m) / 2 lane_center_px = LaneMath.get_lane_center_in_pixels( ploty, left_line.best_fit, right_line.best_fit) lane_offset_m = LaneMath.get_lane_offset_in_meters( binary_warped.shape[1], lane_center_px) screen = DiagnosticScreen.compose_basicScreen(undistorted_overlayed, curverad=curverad, offset=lane_offset_m) return screen, left_line, right_line
def visualize_lanes_using_windows(self, img): # Undistort, threshold, warp binary_warped = self.get_birdseye_binary_warped(img) left_fit, right_fit, out_img = self.find_lane_lines_using_windows( binary_warped) # Generate x and y values for plotting ploty = np.linspace(0, binary_warped.shape[0] - 1, binary_warped.shape[0]) left_fitx = LaneMath.eval_poly_at(ploty, left_fit) right_fitx = LaneMath.eval_poly_at(ploty, right_fit) # Visualize the lines plt.figure() plt.imshow(out_img) plt.plot(left_fitx, ploty, color='yellow') plt.plot(right_fitx, ploty, color='yellow') plt.xlim(0, 1296) plt.ylim(972, 0) plt.show() left_curverad_px, right_curverad_px = LaneMath.get_curve_radii_in_pixels( ploty, left_fit, right_fit) print("left curve {:.3f} px, right curve {:.3f} px".format( left_curverad_px, right_curverad_px)) left_curverad_m, right_curverad_m = LaneMath.get_curve_radii_in_meters( ploty, left_fitx, right_fitx) print("left curve {:.3f} m, right curve {:.3f} m".format( left_curverad_m, right_curverad_m)) lane_center_px = LaneMath.get_lane_center_in_pixels( ploty, left_fit, right_fit) lane_offset_m = LaneMath.get_lane_offset_in_meters( binary_warped.shape[1], lane_center_px) print("lane center {:.3f} px, offset from lane center {:.3f} m".format( lane_center_px, lane_offset_m)) return left_fit, right_fit
def visualize_lanes_using_diagnostic_screen(self, img, left_line=None, right_line=None): # Undistort, threshold, warp undistorted = self.img_mgr.undistort(img) # Warp to birds-eye view masked = self.birdseye.apply_cropping_mask(undistorted) warped = self.birdseye.warp(masked) # Convert to color spaces gry = cv2.cvtColor(warped, cv2.COLOR_RGB2GRAY) hls = cv2.cvtColor(warped, cv2.COLOR_RGB2HLS) # Take the average of adding the L and S channels from the HLS encoding and then apply # the appropriate thresholds lumsat_thresh = (100, 255) lumsat = (np.float32(hls[:, :, 1]) + np.float32(hls[:, :, 2])) // 2 lumsat_binary = np.zeros_like(gry) lumsat_binary[(lumsat >= lumsat_thresh[0]) & (lumsat <= lumsat_thresh[1])] = 1 binary_warped = np.zeros_like(lumsat_binary).astype(np.uint8) binary_warped[lumsat_binary == 1] = 1 # Set the width of the windows +/- margin margin = 100 if ((left_line == None) or (right_line == None)): left_line, right_line, out_img = self.find_lane_lines( binary_warped, margin=margin, method='sliding_windows') else: left_line, right_line, out_img = self.find_lane_lines( binary_warped, margin=margin, method='previous_fit', prev_left_line=left_line, prev_right_line=right_line) # Generate x and y values for plotting ploty = np.linspace(0, binary_warped.shape[0] - 1, binary_warped.shape[0]) undistorted_overlayed = self.draw_lane_on_image( undistorted, binary_warped, ploty, left_line.best_fitx, right_line.best_fitx) curverad = (left_line.radius_of_curvature_m + right_line.radius_of_curvature_m) / 2 lane_center_px = LaneMath.get_lane_center_in_pixels( ploty, left_line.best_fit, right_line.best_fit) lane_offset_m = LaneMath.get_lane_offset_in_meters( binary_warped.shape[1], lane_center_px) screen = DiagnosticScreen.compose_diagScreen( curverad=curverad, offset=lane_offset_m, mainDiagScreen=undistorted_overlayed, diag1=warped, diag2=lumsat_binary, diag3=None, diag4=None, diag5=None, diag6=None, diag7=out_img, diag8=None, diag9=None) return screen, left_line, right_line
def find_lane_lines(self, binary_warped, margin=100, method='sliding_windows', prev_left_line=None, prev_right_line=None, produce_out_img=True): # This code was adapted from the Udacity 'Finding the Lines' section of the # Advanced Lane Finding lesson # https://classroom.udacity.com/nanodegrees/nd013/parts/fbf77062-5703-404e-b60c-95b78b2f3f9e/modules/2b62a1c3-e151-4a0e-b6b6-e424fa46ceab/lessons/40ec78ee-fb7c-4b53-94a8-028c5c60b858/concepts/c41a4b6b-9e57-44e6-9df9-7e4e74a1a49a #### # 1. Try to identify the lane lines in the image # Identify the x and y positions of all non-zero valued pixels in the image nonzero = binary_warped.nonzero() nonzeroy = np.array(nonzero[0]) nonzerox = np.array(nonzero[1]) # Try to select only the points related to the the lines # If we have a previous line, use the 'previous fit' method to get the indexes # of the nonzero values associated with the lines if ((method == 'previous_fit') and (prev_left_line is not None) and (prev_left_line.detected != False) and (prev_right_line is not None) and (prev_right_line.detected != False)): # Grab the fitx points along the line for all of the non-zero pixel y-values left_nonzerofitx = LaneMath.eval_poly_at(nonzeroy, prev_left_line.best_fit) right_nonzerofitx = LaneMath.eval_poly_at(nonzeroy, prev_right_line.best_fit) # Grab the indices of any non-zero pixels that are within the specified margin of the fitx points left_lane_inds = ((nonzerox > (left_nonzerofitx - margin)) & (nonzerox < (left_nonzerofitx + margin))) right_lane_inds = ((nonzerox > (right_nonzerofitx - margin)) & (nonzerox < (right_nonzerofitx + margin))) # Initialize empty arrays for the windows, which are not used for this method but will # be consulted later for drawing to the output image. left_lane_windows, right_lane_windows = [], [] else: # Otherwise fall back to the sliding windows method left_lane_inds, right_lane_inds, left_lane_windows, right_lane_windows = self.find_line_indices_using_sliding_windows( binary_warped, nonzerox, nonzeroy, margin=margin) # 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] # Generate y values for plotting and fitting ploty = np.linspace(0, binary_warped.shape[0] - 1, binary_warped.shape[0]) # Fit a second order polynomial using any historical information if possible # Otherwise, just use the non-zero pixels we detected #left_fit, left_history_heatmap = fit_from_history_heatmap(leftx, lefty, ploty, prev_line=prev_left_line, img=binary_warped) #right_fit, right_history_heatmap = fit_from_history_heatmap(rightx, righty, ploty, prev_line=prev_right_line, img=binary_warped) # Fit a second order polynomial to each group of pixels left_fit = np.polyfit(lefty, leftx, 2) right_fit = np.polyfit(righty, rightx, 2) if (produce_out_img): # Create an output image to draw on and visualize the result and # Color the non-zero values that are part of the lanes #out_img = np.dstack((left_history_heatmap, np.zeros_like(left_history_heatmap), right_history_heatmap)) # Create an output image to draw on and visualize the result and # Color the non-zero values that are part of the lanes out_img = np.dstack( (binary_warped, binary_warped, binary_warped)) * 255 out_img[lefty, leftx] = [255, 0, 0] out_img[righty, rightx] = [0, 0, 255] # Draw the windows on the visualization image (if there are any to draw) for rect_points in left_lane_windows: cv2.rectangle(out_img, rect_points[0], rect_points[1], (0, 255, 0), 2) for rect_points in right_lane_windows: cv2.rectangle(out_img, rect_points[0], rect_points[1], (0, 255, 0), 2) #### # 2. Now check the sanity of the curves we tried to detect sane_left = True sane_right = True # Generate x-values for plotting and conversion to meters left_fitx = LaneMath.eval_poly_at(ploty, left_fit) right_fitx = LaneMath.eval_poly_at(ploty, right_fit) # Calculate the curvature radius in pixels at the bottom of the image y_eval = np.max(ploty) left_radius_of_curvature_px = LaneMath.get_curve_radius( y_eval, left_fit) right_radius_of_curvature_px = LaneMath.get_curve_radius( y_eval, right_fit) # Calculate the curvature radius in meters # The image size and fitx values need to be specified since the x- and y- # values need to be scaled before fitting a line and evaluating at a point. left_radius_of_curvature_m = LaneMath.get_curve_radius_in_meters( ploty, left_fitx) right_radius_of_curvature_m = LaneMath.get_curve_radius_in_meters( ploty, right_fitx) # - Check that Left and Right curvature is not too small (<100) if (left_radius_of_curvature_m < 100): print("WARNING: left_radius_of_curvature_m < 100:", left_radius_of_curvature_m) sane_left = False if (right_radius_of_curvature_m < 100): print("WARNING: right_radius_of_curvature_m < 100:", right_radius_of_curvature_m) sane_right = False # - Check that Left and Right have similar curvature left_shape = self.measure_relative_line_curvature(left_fitx) #print("left_shape", left_shape) right_shape = self.measure_relative_line_curvature(right_fitx) #print("right_shape", right_shape) # - Check that Left and Right are separated by approximately the right distance horizontally line_diff = np.subtract(right_fitx, left_fitx).astype(int) line_mean = np.mean(line_diff).astype(int) if (line_mean > 825) or (line_mean < 525): print( "WARNING: mean line_diff out of range: 525 > {} > 825".format( line_mean)) sane_left = False sane_right = False # - Check that Left and Right are roughly parallel norm_line_diff = line_diff - line_mean #print("norm_line_diff", norm_line_diff) max_line_x_diff = np.max(np.abs(norm_line_diff)) max_line_x_thresh = 140 if (max_line_x_diff > max_line_x_thresh): print("WARNING: max line x diff {} > thresh {}".format( max_line_x_diff, max_line_x_thresh)) sane_left = False sane_right = False #### # 3. Depending on the sanity check results and past history, figure out what values to use going forward left_line = Line() left_line.detected_fit = left_fit left_line.detected_pixelsx = leftx left_line.detected_pixelsy = lefty right_line = Line() right_line.detected_fit = right_fit right_line.detected_pixelsx = rightx right_line.detected_pixelsy = righty if (sane_left): left_line.detected = True else: if (prev_left_line is not None): left_line.detected = self.lines_are_similar( ploty, prev_left_line.best_fit, left_fit) if (sane_right): right_line.detected = True else: if (prev_right_line is not None): right_line.detected = self.lines_are_similar( ploty, prev_right_line.best_fit, right_fit) if ((left_line.detected is False) and (prev_left_line is not None)): # Predict based on history available in recent_fitxs left_line.used_fitx = prev_left_line.predict_next_fitx() left_line.used_fit = np.polyfit(ploty, left_line.used_fitx, 2) else: # Either the line was detected successfully, so use it, or # we don't have any history to use, so we have no choice but use the fit we have. # The sliding windows method will be used next time anyway. left_line.used_fit = left_fit left_line.used_fitx = left_fitx if ((right_line.detected is False) and (prev_right_line is not None)): # Predict based on history available in recent_fitxs right_line.used_fitx = prev_right_line.predict_next_fitx() right_line.used_fit = np.polyfit(ploty, right_line.used_fitx, 2) else: # Either the line was detected successfully, so use it, or # we don't have any history to use, so we have no choice but use the fit we have. # The sliding windows method will be used next time anyway. right_line.used_fit = right_fit right_line.used_fitx = right_fitx ### # 4. Update our recent history and best evaluations # Copy previous recent_fitxs values if available if (prev_left_line is not None): if (len(prev_left_line.recent_fitxs) == prev_left_line.history_depth): left_line.recent_fitxs = prev_left_line.recent_fitxs[1:] else: left_line.recent_fitxs = prev_left_line.recent_fitxs[:] # Append the new used_fitx value to the history left_line.recent_fitxs.append(left_line.used_fitx) # Copy previous recent_fitxs values if available if (prev_right_line is not None): if (len(prev_right_line.recent_fitxs) == prev_right_line.history_depth): right_line.recent_fitxs = prev_right_line.recent_fitxs[1:] else: right_line.recent_fitxs = prev_right_line.recent_fitxs[:] # Append the new used_fitx value to the history right_line.recent_fitxs.append(right_line.used_fitx) # Update the best_fit and best_fitx values left_line.best_fitx = Prediction.find_weighted_averages( left_line.recent_fitxs, left_line.history_depth)[-1] left_line.best_fit = np.polyfit(ploty, left_line.best_fitx, 2) right_line.best_fitx = Prediction.find_weighted_averages( right_line.recent_fitxs, right_line.history_depth)[-1] right_line.best_fit = np.polyfit(ploty, right_line.best_fitx, 2) #### # 5. Calculate the radius based on the best values left_line.radius_of_curvature_px = LaneMath.get_curve_radius( y_eval, left_line.best_fit) left_line.radius_of_curvature_m = LaneMath.get_curve_radius_in_meters( ploty, left_line.best_fitx) right_line.radius_of_curvature_px = LaneMath.get_curve_radius( y_eval, right_line.best_fit) right_line.radius_of_curvature_m = LaneMath.get_curve_radius_in_meters( ploty, right_line.best_fitx) if (produce_out_img): # Draw the search window on the output image if using the previous fit method if (method == 'previous_fit'): # 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)) # Create an image to show the selection window window_img = np.zeros_like(out_img) cv2.fillPoly(window_img, np.int_([left_line_pts]), (0, 255, 0)) cv2.fillPoly(window_img, np.int_([right_line_pts]), (0, 255, 0)) # Draw the selection window onto the output image out_img = cv2.addWeighted(out_img, 1, window_img, 0.3, 0) # Draw the used lines on the output image if (left_line.detected): self.plot_line(out_img, left_line.used_fitx, ploty) else: self.plot_line(out_img, left_line.used_fitx, ploty, color=(0, 255, 255)) if (right_line.detected): self.plot_line(out_img, right_line.used_fitx, ploty) else: self.plot_line(out_img, right_line.used_fitx, ploty, color=(0, 255, 255)) # Draw the best lines on the output image self.plot_line(out_img, left_line.best_fitx, ploty, color=(255, 0, 255)) self.plot_line(out_img, right_line.best_fitx, ploty, color=(255, 0, 255)) else: out_img = None return left_line, right_line, out_img
def visualize_lines_from_fit(self, img, l_fit, r_fit): # Undistort, threshold, warp binary_warped = self.get_birdseye_binary_warped(img) left_fit, right_fit, out_img = self.find_lane_lines_from_fit( binary_warped, l_fit, r_fit) # Generate x and y values for plotting ploty = np.linspace(0, binary_warped.shape[0] - 1, binary_warped.shape[0]) left_fitx = LaneMath.eval_poly_at(ploty, left_fit) right_fitx = LaneMath.eval_poly_at(ploty, right_fit) # Set the width of the windows +/- margin margin = 100 # 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)) # Create an image to show the selection window window_img = np.zeros_like(out_img) cv2.fillPoly(window_img, np.int_([left_line_pts]), (0, 255, 0)) cv2.fillPoly(window_img, np.int_([right_line_pts]), (0, 255, 0)) # Draw the selection window onto the output image result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0) plt.figure() plt.imshow(result) plt.plot(left_fitx, ploty, color='yellow') plt.plot(right_fitx, ploty, color='yellow') plt.xlim(0, 1296) plt.ylim(972, 0) plt.show() left_curverad_px, right_curverad_px = LaneMath.get_curve_radii_in_pixels( ploty, left_fit, right_fit) print("left curve {:.3f} px, right curve {:.3f} px".format( left_curverad_px, right_curverad_px)) left_curverad_m, right_curverad_m = LaneMath.get_curve_radii_in_meters( ploty, left_fitx, right_fitx) print("left curve {:.3f} m, right curve {:.3f} m".format( left_curverad_m, right_curverad_m)) lane_center_px = LaneMath.get_lane_center_in_pixels( ploty, left_fit, right_fit) lane_offset_m = LaneMath.get_lane_offset_in_meters( binary_warped.shape[1], lane_center_px) print("lane center {:.3f} px, offset from lane center {:.3f} m".format( lane_center_px, lane_offset_m)) return left_fit, right_fit