# is not the position of the rover but a bit in front of it bottom_offset = 6 source = np.float32([[14, 140], [301, 140], [200, 96], [118, 96]]) destination = np.float32([ [image.shape[1] / 2 - dst_size, image.shape[0] - bottom_offset], [image.shape[1] / 2 + dst_size, image.shape[0] - bottom_offset], [ image.shape[1] / 2 + dst_size, image.shape[0] - 2 * dst_size - bottom_offset ], [ image.shape[1] / 2 - dst_size, image.shape[0] - 2 * dst_size - bottom_offset ], ]) warped = perspect_transform(image, source, destination) colorsel = color_thresh(warped, rgb_thresh=(160, 160, 160)) # Extract nabigable terrain pixels xpix, ypix = rover_coords(colorsel) # Generate 200x200 pixel worldmap worldmap = np.zeros((200, 200)) scale = 10 # Get navigable pixel positions in world coords x_world, y_world = pix_to_world(xpix, ypix, rover_xpos, rover_ypos, rover_yaw, worldmap.shape[0], scale) # Add pixel positions to worldmap worldmap[y_world, x_world] += 1 print('Xpos = ', rover_xpos, 'Ypos = ', rover_ypos, 'Yaw = ', rover_yaw) # Plot the map in rover-centric coords f, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 7))
source = np.float32([[14, 140], [301, 140], [200, 96], [118, 96]]) destination = np.float32([ [image.shape[1] / 2 - dst_size, image.shape[0] - bottom_offset], [image.shape[1] / 2 + dst_size, image.shape[0] - bottom_offset], [ image.shape[1] / 2 + dst_size, image.shape[0] - 2 * dst_size - bottom_offset ], [ image.shape[1] / 2 - dst_size, image.shape[0] - 2 * dst_size - bottom_offset ], ]) warped = perspect_transform(image, source, destination) # Perform perspective transform colorsel = color_thresh(warped, rgb_thresh=(160, 160, 160)) # threshold the warped image xpix, ypix = rover_coords(colorsel) # convert to rover-centric coordinates distances, angles = to_polar_coords(xpix, ypix) # Convert to polar coordinates avg_angle = np.mean(angles) # Compite the average angle # Do some plotting fig = plt.figure(figsize=(12, 9)) plt.subplot(221) plt.imshow(image) plt.subplot(222) plt.imshow(warped) plt.subplot(223) plt.imshow(colorsel, cmap='gray') plt.subplot(224)