def test_above_and_to_the_left(self):
location = (40, 70)
center_of_image_in_global = (50, 50)
center_of_image_in_pixels = (300, 300)
pixels = utils.convert_to_pixel_coords(location, center_of_image_in_global,
center_of_image_in_pixels)
answer = (100, 200)
self.assertTupleEqual(pixels, answer)
pixels = utils.convert_to_pixel_coords(location, center_of_image_in_global,
center_of_image_in_pixels, resolution=0.2)
answer = (200, 250)
self.assertTupleEqual(answer, pixels)
def get_track_box(annotation: Dict[str, Any],
center_coordinates: Tuple[float, float],
center_pixels: Tuple[float, float],
resolution: float = 0.1) -> np.ndarray:
"""
Get four corners of bounding box for agent in pixels.
:param annotation: The annotation record of the agent.
:param center_coordinates: (x, y) coordinates in global frame
of the center of the image.
:param center_pixels: (row_index, column_index) location of the center
of the image in pixel coordinates.
:param resolution: Resolution pixels/meter of the image.
"""
assert resolution > 0
location = annotation['translation'][:2]
yaw_in_radians = quaternion_yaw(Quaternion(annotation['rotation']))
row_pixel, column_pixel = convert_to_pixel_coords(location,
center_coordinates,
center_pixels,
resolution)
width = annotation['size'][0] / resolution
length = annotation['size'][1] / resolution
# Width and length are switched here so that we can draw them along the x-axis as
# opposed to the y. This makes rotation easier.
return pixels_to_box_corners(row_pixel, column_pixel, length, width,
yaw_in_radians)
def test_same_location(self):
location = (50, 50)
center_of_image_in_global = (50, 50)
center_of_image_in_pixels = (400, 250)
pixels = utils.convert_to_pixel_coords(location, center_of_image_in_global, center_of_image_in_pixels)
self.assertTupleEqual(pixels, (400, 250))
def test_below_and_to_the_left(self):
location = (30, 40)
center_of_image_in_global = (50, 50)
center_of_image_in_pixels = (400, 250)
pixels = utils.convert_to_pixel_coords(location, center_of_image_in_global, center_of_image_in_pixels)
answer = (500, 50)
self.assertTupleEqual(pixels, answer)
def draw_lanes_on_image(
image: np.ndarray,
lanes: Dict[str, List[Tuple[float, float, float]]],
agent_global_coords: Tuple[float, float],
agent_yaw_in_radians: float,
agent_pixels: Tuple[int, int],
resolution: float,
color_function: Callable[[float, float],
Color] = color_by_yaw) -> np.ndarray:
"""
Draws lanes on image.
:param image: Image to draw lanes on. Preferably all-black or all-white image.
:param lanes: Mapping from lane id to list of coordinate tuples in global coordinate system.
:param agent_global_coords: Location of the agent in the global coordinate frame.
:param agent_yaw_in_radians: Yaw of agent in radians.
:param agent_pixels: Location of the agent in the image as (row_pixel, column_pixel).
:param resolution: Resolution in meters/pixel.
:param color_function: By default, lanes are colored by the yaw difference between the pose
on the lane and the agent yaw. However, you can supply your own function to color the lanes.
:return: Image (represented as np.ndarray) with lanes drawn.
"""
for poses_along_lane in lanes.values():
for start_pose, end_pose in zip(poses_along_lane[:-1],
poses_along_lane[1:]):
start_pixels = convert_to_pixel_coords(start_pose[:2],
agent_global_coords,
agent_pixels, resolution)
end_pixels = convert_to_pixel_coords(end_pose[:2],
agent_global_coords,
agent_pixels, resolution)
start_pixels = (start_pixels[1], start_pixels[0])
end_pixels = (end_pixels[1], end_pixels[0])
color = color_function(agent_yaw_in_radians, start_pose[2])
# Need to flip the row coordinate and the column coordinate
# because of cv2 convention
cv2.line(image, start_pixels, end_pixels, color, thickness=5)
return image