[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)
plt.plot(xpix, ypix, '.')
plt.ylim(-160, 160)
plt.xlim(0, 160)
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))
f.tight_layout()
ax1.plot(xpix, ypix, '.')
ax1.set_title('Rover Space', fontsize=40)
# Define a function to convert from cartesian to polar coordinates
def to_polar_coords(xpix, ypix):
# Calculate distance to each pixel
dist = np.sqrt(xpix**2 + ypix**2)
# Calculate angle using arctangent function
angles = np.arctan2(ypix, xpix)
return dist, angles
image = mpimg.imread('angle_example.jpg')
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 coords
distances, angles = to_polar_coords(xpix, ypix) # Convert to polar coords
avg_angle = np.mean(angles) # Compute 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)
plt.plot(xpix, ypix, '.')
plt.ylim(-160, 160)
plt.xlim(0, 160)