def create_crosswalks_model(map_api: MapAPI) -> CrosswalkModel:
vertices = []
vertexType = []
for element in map_api:
element_id = MapAPI.id_as_str(element.id)
if map_api.is_crosswalk(element):
crosswalk = map_api.get_crosswalk_coords(element_id)
coords = earclip(crosswalk["xyz"])
for triangle in coords:
for vertex in triangle:
vertices.append(vertex[0])
vertices.append(vertex[1])
# z-axis (0 in 2d)
vertices.append(0.001)
# White
vertexType.append(1)
vertices = np.array(vertices, dtype=np.float32)
vertexType = np.array(vertexType, dtype=np.float32)
return CrosswalkModel(vertices, len(vertices) // 3, colors=vertexType)
def _get_parent_road_network_segment(self,
lane: Lane) -> RoadNetworkSegment:
parent_road_network_seg_id: str = MapAPI.id_as_str(
lane.parent_segment_or_junction)
parent_road_network_seg: RoadNetworkSegment = self.proto_API[
parent_road_network_seg_id]
return parent_road_network_seg
def get_bounds(self) -> dict:
"""
For each elements of interest returns bounds [[min_x, min_y],[max_x, max_y]] and proto ids
Coords are computed by the MapAPI and, as such, are in the world ref system.
Returns:
dict: keys are classes of elements, values are dict with `bounds` and `ids` keys
"""
lanes_ids = []
crosswalks_ids = []
lanes_bounds = np.empty(
(0, 2, 2), dtype=np.float) # [(X_MIN, Y_MIN), (X_MAX, Y_MAX)]
crosswalks_bounds = np.empty(
(0, 2, 2), dtype=np.float) # [(X_MIN, Y_MIN), (X_MAX, Y_MAX)]
for element in self.proto_API:
element_id = MapAPI.id_as_str(element.id)
if self.proto_API.is_lane(element):
lane = self.proto_API.get_lane_coords(element_id)
x_min = min(np.min(lane["xyz_left"][:, 0]),
np.min(lane["xyz_right"][:, 0]))
y_min = min(np.min(lane["xyz_left"][:, 1]),
np.min(lane["xyz_right"][:, 1]))
x_max = max(np.max(lane["xyz_left"][:, 0]),
np.max(lane["xyz_right"][:, 0]))
y_max = max(np.max(lane["xyz_left"][:, 1]),
np.max(lane["xyz_right"][:, 1]))
lanes_bounds = np.append(lanes_bounds,
np.asarray([[[x_min, y_min],
[x_max, y_max]]]),
axis=0)
lanes_ids.append(element_id)
if self.proto_API.is_crosswalk(element):
crosswalk = self.proto_API.get_crosswalk_coords(element_id)
x_min = np.min(crosswalk["xyz"][:, 0])
y_min = np.min(crosswalk["xyz"][:, 1])
x_max = np.max(crosswalk["xyz"][:, 0])
y_max = np.max(crosswalk["xyz"][:, 1])
crosswalks_bounds = np.append(
crosswalks_bounds,
np.asarray([[[x_min, y_min], [x_max, y_max]]]),
axis=0,
)
crosswalks_ids.append(element_id)
return {
"lanes": {
"bounds": lanes_bounds,
"ids": lanes_ids
},
"crosswalks": {
"bounds": crosswalks_bounds,
"ids": crosswalks_ids
},
}
def __init__(
self,
render_context: RenderContext,
semantic_map_path: str,
world_to_ecef: np.ndarray,
):
super(TLSemanticRasterizer, self).__init__()
self.render_context = render_context
self.raster_size = render_context.raster_size_px
self.pixel_size = render_context.pixel_size_m
self.ego_center = render_context.center_in_raster_ratio
self.world_to_ecef = world_to_ecef
self.proto_API = MapAPI(semantic_map_path, world_to_ecef)
self.bounds_info = self.get_bounds()
self.raster_channels = 3
def __init__(self, raster_size: Tuple[int, int],
pixel_size: Union[np.ndarray, list,
float], ego_center: np.ndarray,
filter_agents_threshold: float, history_num_frames: int,
semantic_map_path: str, world_to_ecef: np.ndarray):
super().__init__()
self.raster_size = raster_size
self.pixel_size = pixel_size
if isinstance(pixel_size, np.ndarray) or isinstance(pixel_size, list):
self.pixel_size = pixel_size[0]
self.filter_agents_threshold = filter_agents_threshold
self.proto_API = MapAPI(semantic_map_path, world_to_ecef)
# Create and hide a window
DisplayManager.create_display(width=raster_size[0],
height=raster_size[1])
self.output_fbo = initialize_framebuffer_object(width=raster_size[0],
height=raster_size[1])
self.loader: Loader = Loader()
self.shader: StaticShader = StaticShader()
self.camera: Camera = Camera()
self.renderer: Renderer = Renderer(display_width=raster_size[0],
display_height=raster_size[1],
camera=self.camera,
pixel_size=self.pixel_size)
self.renderer.add_renderer('entity_renderer', EntityRenderer())
self.renderer.add_renderer('map_renderer', MapRenderer())
self.agent_model: Cube = Cube()
self.agent_model.load_to_vao(self.loader)
self.lane_surface_model: LaneSurfaceModel = create_lane_surface_model(
self.proto_API)
self.lane_surface_model.load_to_vao(self.loader)
self.lane_lines_model: LaneLinesModel = create_lane_line_model(
self.proto_API)
self.lane_lines_model.load_to_vao(self.loader)
self.crosswalk_model: CrosswalkModel = create_crosswalks_model(
self.proto_API)
self.crosswalk_model.load_to_vao(self.loader)
self.agents = []
self.map_layers = []
def create_lane_line_model(map_api: MapAPI) -> LaneLinesModel:
vertices = []
vertexType = []
for element in map_api:
element_id = MapAPI.id_as_str(element.id)
if map_api.is_lane(element):
lane = map_api.get_lane_coords(element_id)
lane_coords = lane["xyz_left"]
for idx in range(len(lane_coords) - 1):
vertices.append(lane_coords[idx][0])
vertices.append(lane_coords[idx][1])
vertices.append(1.)
vertexType.append(3)
vertices.append(lane_coords[idx + 1][0])
vertices.append(lane_coords[idx + 1][1])
vertices.append(1.)
vertexType.append(3)
lane_coords = lane["xyz_right"]
for idx in range(len(lane_coords) - 1):
vertices.append(lane_coords[idx][0])
vertices.append(lane_coords[idx][1])
vertices.append(1.)
vertexType.append(3)
vertices.append(lane_coords[idx + 1][0])
vertices.append(lane_coords[idx + 1][1])
vertices.append(1.)
vertexType.append(3)
vertices = np.array(vertices, dtype=np.float32)
vertexType = np.array(vertexType, dtype=np.float32)
return LaneLinesModel(vertices, len(vertices) // 3, vertexType)
def create_lane_surface_model(map_api: MapAPI) -> LaneSurfaceModel:
vertices = []
vertexType = []
if obj_exists("lane_surface_vertices", './cache'):
vertices = obj_load("lane_surface_vertices", './cache')
vertexType = obj_load("lane_surface_vertex_types", "./cache")
return LaneSurfaceModel(vertices,
len(vertices) // 3,
colors=vertexType)
print("Generating triangles from lanes' polygon dataponts...")
for element in tqdm(map_api):
element_id = MapAPI.id_as_str(element.id)
if map_api.is_lane(element):
lane = map_api.get_lane_coords(element_id)
left = lane["xyz_left"]
right = lane["xyz_right"]
coords = np.vstack((left, np.flip(right, 0)))
coords = earclip(coords)
for triangle in coords:
for vertex in triangle:
vertices.append(vertex[0])
vertices.append(vertex[1])
# z-axis (0 in 2d)
vertices.append(0.0001)
vertexType.append(1000)
vertices = np.array(vertices, dtype=np.float32)
vertexType = np.array(vertexType, dtype=np.float32)
obj_save(vertices, "lane_surface_vertices", "./cache")
obj_save(vertexType, "lane_surface_vertex_types", "./cache")
return LaneSurfaceModel(vertices, len(vertices) // 3, colors=vertexType)
def render_semantic_map(
self, center_in_world: np.ndarray, raster_from_world: np.ndarray, tl_faces: np.ndarray = None,
tl_face_color=True
) -> th.Union[torch.Tensor, dict]:
"""Renders the semantic map at given x,y coordinates.
Args:
center_in_world (np.ndarray): XY of the image center in world ref system
raster_from_world (np.ndarray):
Returns:
th.Union[torch.Tensor, dict]
"""
# filter using half a radius from the center
raster_radius = float(np.linalg.norm(self.raster_size * self.pixel_size)) / 2
# get active traffic light faces
if tl_face_color:
active_tl_ids = set(filter_tl_faces_by_status(tl_faces, "ACTIVE")["face_id"].tolist())
# setup canvas
raster_size = self.render_context.raster_size_px
res = dict(path=list(), path_type=list())
for idx in elements_within_bounds(center_in_world, self.bounds_info["lanes"]["bounds"], raster_radius):
lane = self.proto_API[self.bounds_info["lanes"]["ids"][idx]].element.lane
# get image coords
lane_coords = self.proto_API.get_lane_coords(self.bounds_info["lanes"]["ids"][idx])
xy_left = cv2_subpixel(transform_points(lane_coords["xyz_left"][:, :2], raster_from_world))
xy_right = cv2_subpixel(transform_points(lane_coords["xyz_right"][:, :2], raster_from_world))
xy_left = normalize_line(xy_left)
xy_right = normalize_line(xy_right)
lane_type = "black" # no traffic light face is controlling this lane
if tl_face_color:
lane_tl_ids = set([MapAPI.id_as_str(la_tc) for la_tc in lane.traffic_controls])
for tl_id in lane_tl_ids.intersection(active_tl_ids):
if self.proto_API.is_traffic_face_colour(tl_id, "red"):
lane_type = "red"
elif self.proto_API.is_traffic_face_colour(tl_id, "green"):
lane_type = "green"
elif self.proto_API.is_traffic_face_colour(tl_id, "yellow"):
lane_type = "yellow"
for vector in [xy_left, xy_right]:
vector = crop_tensor(vector, raster_size)
if len(vector):
res['path'].append(vector)
res['path_type'].append(path_type_to_number(lane_type))
return res
def render_semantic_map(
self, center_world: np.ndarray, raster_from_world: np.ndarray, history_tl_faces: List[np.ndarray]
) -> np.ndarray:
"""Renders the semantic map at given x,y coordinates.
Args:
center_world (np.ndarray): XY of the image center in world ref system
raster_from_world (np.ndarray):
Returns:
np.ndarray: RGB raster
"""
img = 255 * np.ones(shape=(self.raster_size[1], self.raster_size[0], 3), dtype=np.uint8)
tl_img = 255 * np.ones(shape=(self.raster_size[1], self.raster_size[0], 3), dtype=np.uint8)
# filter using half a radius from the center
raster_radius = float(np.linalg.norm(self.raster_size * self.pixel_size)) / 2
# get active traffic light faces
all_active_tls = [filter_tl_faces_by_status(tl_faces, "ACTIVE") for tl_faces in history_tl_faces]
curr_active_tl_ids = create_active_tl_dict(all_active_tls[0], all_active_tls[1:])
# plot lanes
lanes_lines = defaultdict(list)
persistence_lines = defaultdict(list)
for idx in elements_within_bounds(center_world, self.bounds_info["lanes"]["bounds"], raster_radius):
lane = self.proto_API[self.bounds_info["lanes"]["ids"][idx]].element.lane
# get image coords
lane_coords = self.proto_API.get_lane_coords(self.bounds_info["lanes"]["ids"][idx])
xy_left = cv2_subpixel(transform_points(lane_coords["xyz_left"][:, :2], raster_from_world))
xy_right = cv2_subpixel(transform_points(lane_coords["xyz_right"][:, :2], raster_from_world))
lanes_area = np.vstack((xy_left, np.flip(xy_right, 0))) # start->end left then end->start right
# Note(lberg): this called on all polygons skips some of them, don't know why
cv2.fillPoly(img, [lanes_area], (17, 17, 31), lineType=cv2.LINE_AA, shift=CV2_SHIFT)
# Create lane lines for the current index
lane_type = "default" # no traffic light face is controlling this lane
lane_tl_ids = set([MapAPI.id_as_str(la_tc) for la_tc in lane.traffic_controls])
for tl_id in lane_tl_ids.intersection(set(curr_active_tl_ids.keys())):
if self.proto_API.is_traffic_face_colour(tl_id, "red"):
lane_type = "red"
elif self.proto_API.is_traffic_face_colour(tl_id, "green"):
lane_type = "green"
elif self.proto_API.is_traffic_face_colour(tl_id, "yellow"):
lane_type = "yellow"
persistence_val = curr_active_tl_ids[tl_id]
persistence_lines[persistence_val].extend([xy_left, xy_right])
lanes_lines[lane_type].extend([xy_left, xy_right])
cv2.polylines(img, lanes_lines["default"], False, (255, 217, 82), lineType=cv2.LINE_AA, shift=CV2_SHIFT)
cv2.polylines(img, lanes_lines["green"], False, (0, 255, 0), lineType=cv2.LINE_AA, shift=CV2_SHIFT)
cv2.polylines(img, lanes_lines["yellow"], False, (255, 255, 0), lineType=cv2.LINE_AA, shift=CV2_SHIFT)
cv2.polylines(img, lanes_lines["red"], False, (255, 0, 0), lineType=cv2.LINE_AA, shift=CV2_SHIFT)
# Fill in tl persistence
for p in persistence_lines.keys():
cv2.polylines(tl_img, persistence_lines[p], False, (0, 0, int(p)), lineType=cv2.LINE_AA, shift=CV2_SHIFT)
# if True: plt.imshow(tl_img); plt.title(str(len(persistence_lines))); plt.show()
# plot crosswalks
crosswalks = []
for idx in elements_within_bounds(center_world, self.bounds_info["crosswalks"]["bounds"], raster_radius):
crosswalk = self.proto_API.get_crosswalk_coords(self.bounds_info["crosswalks"]["ids"][idx])
xy_cross = cv2_subpixel(transform_points(crosswalk["xyz"][:, :2], raster_from_world))
crosswalks.append(xy_cross)
cv2.polylines(img, crosswalks, True, (255, 117, 69), lineType=cv2.LINE_AA, shift=CV2_SHIFT)
tl_img = np.sum(tl_img, axis=-1)
return np.concatenate([img, tl_img[:, :, np.newaxis]], axis=-1)
def get_lanes(self, center_in_world: np.ndarray,
raster_from_world: np.ndarray, active_tl_ids: set,
raster_radius: float) -> Iterator[Dict[str, Any]]:
lane_indexes = elements_within_bounds(
center_in_world, self.bounds_info["lanes"]["bounds"],
raster_radius)
for idx in lane_indexes:
lane_id = self.bounds_info["lanes"]["ids"][idx]
lane = self.proto_API[lane_id].element.lane
parent_road = self._get_parent_road_network_segment(lane)
lane_coords = self.proto_API.get_lane_coords(
self.bounds_info["lanes"]["ids"][idx])
lane_tl_ids = set(
[MapAPI.id_as_str(la_tc) for la_tc in lane.traffic_controls])
lane_tl_ids = lane_tl_ids.intersection(active_tl_ids)
tl_categories = self._categorize_tl(lane_tl_ids)
centroids = [lane_coords["xyz_left"], lane_coords["xyz_right"]]
for centroid in centroids:
lane_info = {
"lane_id":
lane_id,
"access_restriction":
self._get_access_restriction(lane),
"orientation_in_parent_segment":
self._get_orientation(lane),
"turn_type_in_parent_junction":
self._get_turn_type(lane),
"centroid":
transform_points(centroid[:, :2], raster_from_world),
"has_adjacent_lane_change_left":
len(MapAPI.id_as_str(lane.adjacent_lane_change_left)) > 0,
"has_adjacent_lane_change_right":
len(MapAPI.id_as_str(lane.adjacent_lane_change_right)) > 0,
"tl_count":
len(lane.traffic_controls),
"tl_signal_red_face_count":
len(tl_categories["signal_red_face"]),
"tl_signal_yellow_face_count":
len(tl_categories["signal_yellow_face"]),
"tl_signal_green_face_count":
len(tl_categories["signal_green_face"]),
"tl_signal_left_arrow_red_face_count":
len(tl_categories["signal_left_arrow_red_face"]),
"tl_signal_left_arrow_yellow_face_count":
len(tl_categories["signal_left_arrow_yellow_face"]),
"tl_signal_left_arrow_green_face_count":
len(tl_categories["signal_left_arrow_green_face"]),
"tl_signal_right_arrow_red_face_count":
len(tl_categories["signal_right_arrow_red_face"]),
"tl_signal_right_arrow_yellow_face_count":
len(tl_categories["signal_right_arrow_yellow_face"]),
"tl_signal_right_arrow_green_face_count":
len(tl_categories["signal_right_arrow_green_face"]),
"can_have_parked_cars":
lane.can_have_parked_cars,
"road_speed_limit_meters_per_second":
self._get_speed_limit(parent_road),
"road_class":
self._get_road_class(parent_road),
"road_direction":
self._get_road_direction(parent_road),
"road_fwd_lane_count":
self._get_fwd_lane_count(parent_road),
"road_bwd_lane_count":
self._get_bwd_lane_count(parent_road)
}
yield lane_info
class TLSemanticRasterizer(Rasterizer):
"""
Rasteriser for the vectorised semantic map (generally loaded from json files).
"""
@staticmethod
def from_cfg(cfg, data_manager):
raster_cfg = cfg["raster_params"]
# map_type = raster_cfg["map_type"]
dataset_meta_key = raster_cfg["dataset_meta_key"]
render_context = RenderContext(
raster_size_px=np.array(raster_cfg["raster_size"]),
pixel_size_m=np.array(raster_cfg["pixel_size"]),
center_in_raster_ratio=np.array(raster_cfg["ego_center"]),
)
# filter_agents_threshold = raster_cfg["filter_agents_threshold"]
# history_num_frames = cfg["model_params"]["history_num_frames"]
semantic_map_filepath = data_manager.require(
raster_cfg["semantic_map_key"])
try:
dataset_meta = _load_metadata(dataset_meta_key, data_manager)
world_to_ecef = np.array(dataset_meta["world_to_ecef"],
dtype=np.float64)
except (KeyError, FileNotFoundError
): # TODO remove when new dataset version is available
world_to_ecef = get_hardcoded_world_to_ecef()
return TLSemanticRasterizer(render_context, semantic_map_filepath,
world_to_ecef)
def __init__(
self,
render_context: RenderContext,
semantic_map_path: str,
world_to_ecef: np.ndarray,
):
super(TLSemanticRasterizer, self).__init__()
self.render_context = render_context
self.raster_size = render_context.raster_size_px
self.pixel_size = render_context.pixel_size_m
self.ego_center = render_context.center_in_raster_ratio
self.world_to_ecef = world_to_ecef
self.proto_API = MapAPI(semantic_map_path, world_to_ecef)
self.bounds_info = self.get_bounds()
self.raster_channels = 3
# TODO is this the right place for this function?
def get_bounds(self) -> dict:
"""
For each elements of interest returns bounds [[min_x, min_y],[max_x, max_y]] and proto ids
Coords are computed by the MapAPI and, as such, are in the world ref system.
Returns:
dict: keys are classes of elements, values are dict with `bounds` and `ids` keys
"""
lanes_ids = []
crosswalks_ids = []
lanes_bounds = np.empty(
(0, 2, 2), dtype=np.float) # [(X_MIN, Y_MIN), (X_MAX, Y_MAX)]
crosswalks_bounds = np.empty(
(0, 2, 2), dtype=np.float) # [(X_MIN, Y_MIN), (X_MAX, Y_MAX)]
for element in self.proto_API:
element_id = MapAPI.id_as_str(element.id)
if self.proto_API.is_lane(element):
lane = self.proto_API.get_lane_coords(element_id)
x_min = min(np.min(lane["xyz_left"][:, 0]),
np.min(lane["xyz_right"][:, 0]))
y_min = min(np.min(lane["xyz_left"][:, 1]),
np.min(lane["xyz_right"][:, 1]))
x_max = max(np.max(lane["xyz_left"][:, 0]),
np.max(lane["xyz_right"][:, 0]))
y_max = max(np.max(lane["xyz_left"][:, 1]),
np.max(lane["xyz_right"][:, 1]))
lanes_bounds = np.append(lanes_bounds,
np.asarray([[[x_min, y_min],
[x_max, y_max]]]),
axis=0)
lanes_ids.append(element_id)
if self.proto_API.is_crosswalk(element):
crosswalk = self.proto_API.get_crosswalk_coords(element_id)
x_min = np.min(crosswalk["xyz"][:, 0])
y_min = np.min(crosswalk["xyz"][:, 1])
x_max = np.max(crosswalk["xyz"][:, 0])
y_max = np.max(crosswalk["xyz"][:, 1])
crosswalks_bounds = np.append(
crosswalks_bounds,
np.asarray([[[x_min, y_min], [x_max, y_max]]]),
axis=0,
)
crosswalks_ids.append(element_id)
return {
"lanes": {
"bounds": lanes_bounds,
"ids": lanes_ids
},
"crosswalks": {
"bounds": crosswalks_bounds,
"ids": crosswalks_ids
},
}
def rasterize(
self,
history_frames: np.ndarray,
history_agents: List[np.ndarray],
history_tl_faces: List[np.ndarray],
agent: Optional[np.ndarray] = None,
) -> np.ndarray:
if agent is None:
ego_translation_m = history_frames[0]["ego_translation"]
ego_yaw_rad = rotation33_as_yaw(history_frames[0]["ego_rotation"])
else:
ego_translation_m = np.append(
agent["centroid"], history_frames[0]["ego_translation"][-1])
ego_yaw_rad = agent["yaw"]
raster_from_world = self.render_context.raster_from_world(
ego_translation_m, ego_yaw_rad)
world_from_raster = np.linalg.inv(raster_from_world)
# get XY of center pixel in world coordinates
center_in_raster_px = np.asarray(self.raster_size) * (0.5, 0.5)
center_in_world_m = transform_point(center_in_raster_px,
world_from_raster)
sem_im = self.render_semantic_map(center_in_world_m, raster_from_world,
history_tl_faces[0])
return sem_im.astype(np.float32) / 255
def render_semantic_map(self, center_in_world: np.ndarray,
raster_from_world: np.ndarray,
tl_faces: np.ndarray) -> np.ndarray:
"""Renders the semantic map at given x,y coordinates.
Args:
center_in_world (np.ndarray): XY of the image center in world ref system
raster_from_world (np.ndarray):
Returns:
np.ndarray: RGB raster
"""
img = 255 * np.ones(
shape=(self.raster_size[1], self.raster_size[0], 3),
dtype=np.uint8)
# filter using half a radius from the center
raster_radius = float(
np.linalg.norm(self.raster_size * self.pixel_size)) / 2
# get active traffic light faces
active_tl_ids = set(
filter_tl_faces_by_status(tl_faces, "ACTIVE")["face_id"].tolist())
# plot lanes
lanes_lines = defaultdict(list)
for idx in elements_within_bounds(center_in_world,
self.bounds_info["lanes"]["bounds"],
raster_radius):
lane = self.proto_API[self.bounds_info["lanes"]["ids"]
[idx]].element.lane
# get image coords
lane_coords = self.proto_API.get_lane_coords(
self.bounds_info["lanes"]["ids"][idx])
xy_left = cv2_subpixel(
transform_points(lane_coords["xyz_left"][:, :2],
raster_from_world))
xy_right = cv2_subpixel(
transform_points(lane_coords["xyz_right"][:, :2],
raster_from_world))
lanes_area = np.vstack(
(xy_left, np.flip(xy_right,
0))) # start->end left then end->start right
# Note(lberg): this called on all polygons skips some of them, don't know why
cv2.fillPoly(img, [lanes_area], (17, 17, 31),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
lane_type = "default" # no traffic light face is controlling this lane
lane_tl_ids = set(
[MapAPI.id_as_str(la_tc) for la_tc in lane.traffic_controls])
for tl_id in lane_tl_ids.intersection(active_tl_ids):
if self.proto_API.is_traffic_face_colour(tl_id, "red"):
lane_type = "red"
elif self.proto_API.is_traffic_face_colour(tl_id, "green"):
lane_type = "green"
elif self.proto_API.is_traffic_face_colour(tl_id, "yellow"):
lane_type = "yellow"
lanes_lines[lane_type].extend([xy_left, xy_right])
# For debugging
# import IPython; IPython.embed()
cv2.polylines(img,
lanes_lines["default"],
False, (255, 217, 82),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
cv2.polylines(img,
lanes_lines["green"],
False, (0, 255, 0),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
cv2.polylines(img,
lanes_lines["yellow"],
False, (255, 255, 0),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
cv2.polylines(img,
lanes_lines["red"],
False, (255, 0, 0),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
# plot crosswalks
crosswalks = []
for idx in elements_within_bounds(
center_in_world, self.bounds_info["crosswalks"]["bounds"],
raster_radius):
crosswalk = self.proto_API.get_crosswalk_coords(
self.bounds_info["crosswalks"]["ids"][idx])
xy_cross = cv2_subpixel(
transform_points(crosswalk["xyz"][:, :2], raster_from_world))
crosswalks.append(xy_cross)
cv2.polylines(img,
crosswalks,
True, (255, 117, 69),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
return img
def to_rgb(self, in_im: np.ndarray, **kwargs: dict) -> np.ndarray:
return (in_im * 255).astype(np.uint8)
def render_semantic_map(self, center_in_world: np.ndarray,
raster_from_world: np.ndarray,
tl_faces: np.ndarray) -> np.ndarray:
"""Renders the semantic map at given x,y coordinates.
Args:
center_in_world (np.ndarray): XY of the image center in world ref system
raster_from_world (np.ndarray):
Returns:
np.ndarray: RGB raster
"""
img = 255 * np.ones(
shape=(self.raster_size[1], self.raster_size[0], 3),
dtype=np.uint8)
# filter using half a radius from the center
raster_radius = float(
np.linalg.norm(self.raster_size * self.pixel_size)) / 2
# get active traffic light faces
active_tl_ids = set(
filter_tl_faces_by_status(tl_faces, "ACTIVE")["face_id"].tolist())
# plot lanes
lanes_lines = defaultdict(list)
for idx in elements_within_bounds(center_in_world,
self.bounds_info["lanes"]["bounds"],
raster_radius):
lane = self.proto_API[self.bounds_info["lanes"]["ids"]
[idx]].element.lane
# get image coords
lane_coords = self.proto_API.get_lane_coords(
self.bounds_info["lanes"]["ids"][idx])
xy_left = cv2_subpixel(
transform_points(lane_coords["xyz_left"][:, :2],
raster_from_world))
xy_right = cv2_subpixel(
transform_points(lane_coords["xyz_right"][:, :2],
raster_from_world))
lanes_area = np.vstack(
(xy_left, np.flip(xy_right,
0))) # start->end left then end->start right
# Note(lberg): this called on all polygons skips some of them, don't know why
cv2.fillPoly(img, [lanes_area], (17, 17, 31),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
lane_type = "default" # no traffic light face is controlling this lane
lane_tl_ids = set(
[MapAPI.id_as_str(la_tc) for la_tc in lane.traffic_controls])
for tl_id in lane_tl_ids.intersection(active_tl_ids):
if self.proto_API.is_traffic_face_colour(tl_id, "red"):
lane_type = "red"
elif self.proto_API.is_traffic_face_colour(tl_id, "green"):
lane_type = "green"
elif self.proto_API.is_traffic_face_colour(tl_id, "yellow"):
lane_type = "yellow"
lanes_lines[lane_type].extend([xy_left, xy_right])
# For debugging
# import IPython; IPython.embed()
cv2.polylines(img,
lanes_lines["default"],
False, (255, 217, 82),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
cv2.polylines(img,
lanes_lines["green"],
False, (0, 255, 0),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
cv2.polylines(img,
lanes_lines["yellow"],
False, (255, 255, 0),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
cv2.polylines(img,
lanes_lines["red"],
False, (255, 0, 0),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
# plot crosswalks
crosswalks = []
for idx in elements_within_bounds(
center_in_world, self.bounds_info["crosswalks"]["bounds"],
raster_radius):
crosswalk = self.proto_API.get_crosswalk_coords(
self.bounds_info["crosswalks"]["ids"][idx])
xy_cross = cv2_subpixel(
transform_points(crosswalk["xyz"][:, :2], raster_from_world))
crosswalks.append(xy_cross)
cv2.polylines(img,
crosswalks,
True, (255, 117, 69),
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
return img
def render_lane(self, ax, lane):
coords = self._map_api.get_lane_coords(MapAPI.id_as_str(lane.id))
self.render_boundary(ax, coords["xyz_left"])
self.render_boundary(ax, coords["xyz_right"])
draw_trajectory(im, target_positions_pixels, data["target_yaws"], TARGET_POINTS_COLOR)
clear_output(wait=True)
display(PIL.Image.fromarray(im[::-1]))
# %% [markdown]
# We can also take a general view of the street from a matplotlib-type perspective. I borrow this from [this wonderful notebook](https://www.kaggle.com/t3nyks/lyft-working-with-map-api)
# %% [code] {"_kg_hide-input":true}
from l5kit.data.map_api import MapAPI
from l5kit.rasterization.rasterizer_builder import _load_metadata
semantic_map_filepath = dm.require(cfg["raster_params"]["semantic_map_key"])
dataset_meta = _load_metadata(cfg["raster_params"]["dataset_meta_key"], dm)
world_to_ecef = np.array(dataset_meta["world_to_ecef"], dtype=np.float64)
map_api = MapAPI(semantic_map_filepath, world_to_ecef)
MAP_LAYERS = ["junction", "node", "segment", "lane"]
def element_of_type(elem, layer_name):
return elem.element.HasField(layer_name)
def get_elements_from_layer(map_api, layer_name):
return [elem for elem in map_api.elements if element_of_type(elem, layer_name)]
class MapRenderer:
def __init__(self, map_api):
self._color_map = dict(drivable_area='#a6cee3',
def render_semantic_map(self,
center_in_world: np.ndarray,
raster_from_world: np.ndarray,
tl_faces: np.ndarray = None,
svg_args=None,
face_color=False) -> draw.Drawing:
"""Renders the semantic map at given x,y coordinates.
Args:
center_in_world (np.ndarray): XY of the image center in world ref system
raster_from_world (np.ndarray):
Returns:
drawvg.Drawing object
"""
# filter using half a radius from the center
raster_radius = float(np.linalg.norm(
self.raster_size * self.pixel_size)) / 2
svg_args = svg_args or dict()
# get active traffic light faces
if face_color:
active_tl_ids = set(
filter_tl_faces_by_status(tl_faces, "ACTIVE")["face_id"].tolist())
# setup canvas
raster_size = self.render_context.raster_size_px
origin = self.render_context.origin
d = draw.Drawing(*raster_size,
origin=tuple(origin),
displayInline=False,
**svg_args)
for idx in elements_within_bounds(center_in_world,
self.bounds_info["lanes"]["bounds"],
raster_radius):
lane = self.proto_API[self.bounds_info["lanes"]["ids"]
[idx]].element.lane
# get image coords
lane_coords = self.proto_API.get_lane_coords(
self.bounds_info["lanes"]["ids"][idx])
xy_left = cv2_subpixel(
transform_points(lane_coords["xyz_left"][:, :2],
raster_from_world))
xy_right = cv2_subpixel(
transform_points(lane_coords["xyz_right"][:, :2],
raster_from_world))
if face_color:
lane_type = "black" # no traffic light face is controlling this lane
lane_tl_ids = set(
[MapAPI.id_as_str(la_tc) for la_tc in lane.traffic_controls])
for tl_id in lane_tl_ids.intersection(active_tl_ids):
if self.proto_API.is_traffic_face_colour(tl_id, "red"):
lane_type = "red"
elif self.proto_API.is_traffic_face_colour(tl_id, "green"):
lane_type = "green"
elif self.proto_API.is_traffic_face_colour(tl_id, "yellow"):
lane_type = "yellow"
for line in [xy_left, xy_right]:
vector = line / CV2_SHIFT_VALUE
vector[:, 1] = raster_size[1] + origin[1] - vector[:, 1]
vector[:, 0] = vector[:, 0] + origin[0]
drawn_shape = draw.Lines(
*vector.reshape(-1)) if not face_color else draw.Lines(
*vector.reshape(-1), close=False, stroke=lane_type)
d.append(drawn_shape)
return d
def _get_frame_data(mapAPI: MapAPI, frame: np.ndarray,
agents_frame: np.ndarray,
tls_frame: np.ndarray) -> FrameVisualization:
"""Get visualisation objects for the current frame.
:param mapAPI: mapAPI object (used for lanes, crosswalks etc..)
:param frame: the current frame (used for ego)
:param agents_frame: agents in this frame
:param tls_frame: the tls of this frame
:return: A FrameVisualization object. NOTE: trajectory are not included here
"""
ego_xy = frame["ego_translation"][:2]
#################
# plot lanes
lane_indices = indices_in_bounds(ego_xy,
mapAPI.bounds_info["lanes"]["bounds"], 50)
active_tl_ids = set(
filter_tl_faces_by_status(tls_frame, "ACTIVE")["face_id"].tolist())
lanes_vis: List[LaneVisualization] = []
for idx, lane_idx in enumerate(lane_indices):
lane_idx = mapAPI.bounds_info["lanes"]["ids"][lane_idx]
lane_tl_ids = set(mapAPI.get_lane_traffic_control_ids(lane_idx))
lane_colour = "gray"
for tl_id in lane_tl_ids.intersection(active_tl_ids):
lane_colour = COLORS[mapAPI.get_color_for_face(tl_id)]
lane_coords = mapAPI.get_lane_coords(lane_idx)
left_lane = lane_coords["xyz_left"][:, :2]
right_lane = lane_coords["xyz_right"][::-1, :2]
lanes_vis.append(
LaneVisualization(xs=np.hstack((left_lane[:, 0], right_lane[:,
0])),
ys=np.hstack((left_lane[:, 1], right_lane[:,
1])),
color=lane_colour))
#################
# plot crosswalks
crosswalk_indices = indices_in_bounds(
ego_xy, mapAPI.bounds_info["crosswalks"]["bounds"], 50)
crosswalks_vis: List[CWVisualization] = []
for idx in crosswalk_indices:
crosswalk = mapAPI.get_crosswalk_coords(
mapAPI.bounds_info["crosswalks"]["ids"][idx])
crosswalks_vis.append(
CWVisualization(xs=crosswalk["xyz"][:, 0],
ys=crosswalk["xyz"][:, 1],
color="yellow"))
#################
# plot ego and agents
agents_frame = np.insert(agents_frame, 0, get_ego_as_agent(frame))
box_world_coords = get_box_world_coords(agents_frame)
# ego
ego_vis = EgoVisualization(xs=box_world_coords[0, :, 0],
ys=box_world_coords[0, :, 1],
color="red",
center_x=agents_frame["centroid"][0, 0],
center_y=agents_frame["centroid"][0, 1])
# agents
agents_frame = agents_frame[1:]
box_world_coords = box_world_coords[1:]
agents_vis: List[AgentVisualization] = []
for agent, box_coord in zip(agents_frame, box_world_coords):
label_index = np.argmax(agent["label_probabilities"])
agent_type = PERCEPTION_LABELS[label_index]
agents_vis.append(
AgentVisualization(xs=box_coord[..., 0],
ys=box_coord[..., 1],
color="#1F77B4" if agent_type not in COLORS else
COLORS[agent_type],
track_id=agent["track_id"],
agent_type=PERCEPTION_LABELS[label_index],
prob=agent["label_probabilities"][label_index]))
return FrameVisualization(ego=ego_vis,
agents=agents_vis,
lanes=lanes_vis,
crosswalks=crosswalks_vis,
trajectories=[])