def get_simple_element_info(self, center_in_world: np.ndarray,
raster_from_world: np.ndarray,
raster_radius: float, element_key: str):
element_indexes = elements_within_bounds(
center_in_world, self.bounds_info[element_key]["bounds"],
raster_radius)
for idx in element_indexes:
xyz = self.get_polyline_coords(
self.bounds_info[element_key]["ids"][idx])
xy_pos = transform_points(xyz[:, :2], raster_from_world)
yield {"centroid": xy_pos}
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 test_elements_within_bounds() -> None:
center = np.zeros(2)
half_side = 0.5 # square centered around origin with side 1
bounds = np.zeros((1, 2, 2))
# non-intersecting
bounds[0, 0] = (2, 2)
bounds[0, 1] = (4, 4)
assert len(elements_within_bounds(center, bounds, half_side)) == 0
# intersecting only x
bounds[0, 0] = (0, 2)
bounds[0, 1] = (4, 4)
assert len(elements_within_bounds(center, bounds, half_side)) == 0
# intersecting only y
bounds[0, 0] = (2, 0)
bounds[0, 1] = (4, 4)
assert len(elements_within_bounds(center, bounds, half_side)) == 0
# intersecting both with min (valid)
bounds[0, 0] = (0.25, 0.25)
bounds[0, 1] = (4, 4)
assert len(elements_within_bounds(center, bounds, half_side)) == 1
# intersecting both with max (valid)
bounds[0, 0] = (-4, -4)
bounds[0, 1] = (0.25, 0.25)
assert len(elements_within_bounds(center, bounds, half_side)) == 1
# inside (valid)
bounds[0, 0] = (-0.25, -0.25)
bounds[0, 1] = (0.25, 0.25)
assert len(elements_within_bounds(center, bounds, half_side)) == 1
# including (valid)
bounds[0, 0] = (-4, -4)
bounds[0, 1] = (4, 4)
assert len(elements_within_bounds(center, bounds, half_side)) == 1
# empty case
bounds = np.empty((0, 2, 2), dtype=np.float32)
assert len(elements_within_bounds(center, bounds, half_side)) == 0
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
def fix_yaw_by_surrounding_lane_and_history(
sat_rast: SemanticRasterizer,
centroid: np.ndarray,
yaw_origin: float,
history_positions: np.ndarray,
angle_diff_th_in_degree: float = 60,
distance_th_in_agent: float = 0.5,
num_use_history: int = 2,
lane_polygon_buffer: float = 1.5,
) -> Optional[float]:
"""
Args:
sat_rast:
centroid: (x, y) in world coord
yaw_origin:
history_positions:
"""
p = Point(centroid)
yaws = []
on_road = False
for k, idx in enumerate(
elements_within_bounds(centroid,
sat_rast.bounds_info["lanes"]["bounds"],
0)):
lane_coords = sat_rast.proto_API.get_lane_coords(
sat_rast.bounds_info["lanes"]["ids"][idx])
coords = np.vstack((lane_coords["xyz_left"][:, :2],
np.flip(lane_coords["xyz_right"][:, :2], 0)))
lane_polygon = Polygon(coords)
if lane_polygon.contains(p):
on_road = True
lane_polygon = lane_polygon.exterior.buffer(lane_polygon_buffer).union(
lane_polygon)
if lane_polygon.contains(p):
min_coord_index = np.argmin(
np.linalg.norm(lane_coords["xyz_left"][:, :2] - centroid,
axis=1))
if min_coord_index == lane_coords["xyz_left"].shape[0] - 1:
min_coord_index -= 1
assert min_coord_index >= 0
yaw = _vec_to_rad(lane_coords["xyz_left"][min_coord_index +
1][:2] -
lane_coords["xyz_left"][min_coord_index][:2])
yaws.append(yaw)
yaws = np.array(yaws)
# 駐車されている車を避けるため、道路に乗っていないものは弾く
if not on_road:
return None
# まず history から yaw を作ることを考える (5とかは適当な定数)
if len(history_positions) >= num_use_history:
vecs = history_positions[:-1] - history_positions[1:]
distances = np.linalg.norm(vecs, axis=1)
if (distances[:num_use_history - 1] >= distance_th_in_agent).all():
yaw = _vec_to_rad(vecs[0]) + yaw_origin
# yaw_origin でうまく行っている
if angle_diff_th_in_degree / 180 * np.pi > np.min(
_angle_diff(yaw, yaw_origin)):
return None
if len(yaws) > 0:
# 無矛盾な道路が存在する
if angle_diff_th_in_degree / 180 * np.pi > np.min(
_angle_diff(yaws, yaw)):
return yaw
if len(yaws) > 0:
# どの道路とも {angle_diff_th_in_degree}度 以上ずれてる場合は修正
if angle_diff_th_in_degree / 180 * np.pi <= np.min(
_angle_diff(yaws, yaw_origin)):
min_yaw = yaws[np.argmin(
np.stack(
[
_angle_diff(yaws, yaw_origin),
# _angle_diff(yaws, yaw_origin + np.pi / 2),
_angle_diff(yaws, yaw_origin - np.pi),
# _angle_diff(yaws, yaw_origin - np.pi / 2),
],
axis=-1).min(axis=-1))]
return min_yaw
return None
def render_semantic_map(
self, center_world: np.ndarray, raster_from_world: np.ndarray,
all_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):
all_history_tl_faces (list of np.ndarray):
Returns:
np.ndarray: RGB raster
"""
tl_faces = all_history_tl_faces[0]
# img = 255 * np.ones(shape=(self.raster_size[1], self.raster_size[0], 3), dtype=np.uint8)
img = np.zeros(shape=(self.raster_channels, self.raster_size[1],
self.raster_size[0]),
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)
lanes_active_frames = 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
# --- lanes ---
# 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)
if self.lane_color_mode == "normal":
# lane is 255, outside is 0
cv2.fillPoly(img[0], [lanes_area],
255,
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
elif self.lane_color_mode == "inverse":
# lane is 0, outside is 255
cv2.fillPoly(img[0], [lanes_area],
255,
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
img[0] = 255 - img[0]
elif self.lane_color_mode == "none":
# Do not draw lane...
pass
else:
raise ValueError(
f"[ERROR] Unexpected value self.lane_color_mode={self.lane_color_mode}"
)
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):
n_active_tl = 0
# This is not exclusive, use all information...
if self.proto_API.is_traffic_face_colour(tl_id, "red"):
lane_type = "red"
lanes_lines[lane_type].append([xy_left, xy_right])
active_frames = active_duration(all_history_tl_faces,
tl_id)
lanes_active_frames[lane_type].append(active_frames)
n_active_tl += 1
if self.proto_API.is_traffic_face_colour(tl_id, "green"):
lane_type = "green"
lanes_lines[lane_type].append([xy_left, xy_right])
active_frames = active_duration(all_history_tl_faces,
tl_id)
lanes_active_frames[lane_type].append(active_frames)
n_active_tl += 1
if self.proto_API.is_traffic_face_colour(tl_id, "yellow"):
lane_type = "yellow"
lanes_lines[lane_type].append([xy_left, xy_right])
active_frames = active_duration(all_history_tl_faces,
tl_id)
lanes_active_frames[lane_type].append(active_frames)
n_active_tl += 1
if n_active_tl > 1:
print("[DEBUG] n_active_tl", n_active_tl)
if lane_type == "default":
lanes_lines[lane_type].extend([xy_left, xy_right])
# --- Traffic lights ---
# 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)
# print("[DEBUG] lanes_active_frames", lanes_active_frames)
cv2.polylines(img[1],
lanes_lines["default"],
False,
255,
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
for ch, color in zip([2, 3, 4], ["green", "yellow", "red"]):
lines = lanes_lines[color]
active_frames = lanes_active_frames[color]
if len(active_frames) == 0:
continue
# if np.all(np.array(active_frames) == active_frames[0]):
# # draw all lines at once, same color.
# color = 255 - active_frames[0] # active_frames 0~250
# cv2.polylines(
# img[ch], list(chain.from_iterable(lines)), False, color, lineType=cv2.LINE_AA, shift=CV2_SHIFT
# )
# else:
# draw separately
for i in range(len(lines)):
color = 255 - active_frames[i] # active_frames 0~250
cv2.polylines(img[ch],
lines[i],
False,
color,
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
# cv2.polylines(img[2], lanes_lines["green"], False, 255, lineType=cv2.LINE_AA, shift=CV2_SHIFT)
# cv2.polylines(img[3], lanes_lines["yellow"], False, 255, lineType=cv2.LINE_AA, shift=CV2_SHIFT)
# cv2.polylines(img[4], lanes_lines["red"], False, 255, lineType=cv2.LINE_AA, shift=CV2_SHIFT)
# 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)
# --- Cross Walks ---
# cv2.polylines(img, crosswalks, True, (255, 117, 69), lineType=cv2.LINE_AA, shift=CV2_SHIFT)
cv2.polylines(img[5],
crosswalks,
True,
255,
lineType=cv2.LINE_AA,
shift=CV2_SHIFT)
# ch, h, w --> h, w, ch
img = img.transpose((1, 2, 0))
return img
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 render_semantic_map_cv(
self, center_world: np.ndarray, world_to_image_space: np.ndarray, history_tl_faces: np.ndarray,
) -> Dict[str, 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
world_to_image_space (np.ndarray):
Returns:
np.ndarray: RGB raster
"""
tl_faces = history_tl_faces[0]
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())
all_tl_ids = set(tl_faces["face_id"].tolist())
# plot lanes
lanes_lines = defaultdict(list)
rasterized_tl_lanes: Dict[str, np.ndarray] = {}
all_tl_ids_from_lanes = set()
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
# 2.1s
# get image coords
lane_coords = self.proto_API.get_lane_coords(self.bounds_info["lanes"]["ids"][idx])
# 2.1 s
# xy_left = cv2_subpixel(transform_points(lane_coords["xyz_left"][:, :2], world_to_image_space))
# xy_right = cv2_subpixel(transform_points(lane_coords["xyz_right"][:, :2], world_to_image_space))
# 5.5 s
xy_left = transform_points_fast_with_cv2_shift(lane_coords["xyz_left"], world_to_image_space)
xy_right = transform_points_fast_with_cv2_shift(lane_coords["xyz_right"], world_to_image_space)
# 2.7s
lanes_area = np.vstack((xy_left, np.flip(xy_right, 0))) # start->end left then end->start right
# 5.7s -> 3.5s
# 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)
# 7s
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])
all_tl_ids_from_lanes.update(lane_tl_ids)
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 tl_id in lane_tl_ids:
if tl_id not in rasterized_tl_lanes:
rasterized_tl_lanes[tl_id] = np.zeros(shape=(self.raster_size[1]//4,
self.raster_size[0]//4), dtype=np.uint8)
cv2.fillPoly(rasterized_tl_lanes[tl_id], [lanes_area], 1, lineType=cv2.LINE_4, shift=CV2_SHIFT+2)
# cv2.polylines(
# rasterized_tl_lines[tl_id], [xy_left, xy_right], False, 1, lineType=cv2.LINE_4, shift=CV2_SHIFT,
# )
lanes_lines[lane_type].extend([xy_left, xy_right])
# 7.4 s
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,
)
# only for testing, positions of TL
# tl_positions_img = np.zeros(shape=(self.raster_size[1], self.raster_size[0]), dtype=np.uint8)
# for tl_id in list(all_tl_ids_from_lanes):
# e = self.proto_API[tl_id].element.traffic_control_element
# xy = self.proto_API.unpack_deltas_cm(
# e.points_x_deltas_cm,
# e.points_y_deltas_cm,
# e.points_z_deltas_cm,
# e.geo_frame)
# xy = transform_points_fast_with_cv2_shift(xy, world_to_image_space)
# print(xy.mean(axis=0) / CV2_SHIFT_VALUE)
# pos = (xy.mean(axis=0) / CV2_SHIFT_VALUE).astype(np.int)
# if pos.min() > 8 and pos.max() < tl_positions_img.shape[0] - 8:
# tl_positions_img[pos[1] - 8:pos[1] + 8, pos[0] - 8:pos[0] + 8] += 1
# 8.0s
# plot crosswalks
# prepare: 0.2s
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 = transform_points_fast_with_cv2_shift(crosswalk["xyz"], world_to_image_space)
crosswalks.append(xy_cross)
cv2.polylines(img, crosswalks, True, (255, 117, 69), lineType=cv2.LINE_AA, shift=CV2_SHIFT)
non_missing_rasterized_tl_lanes = {tl_face: mask.astype(np.bool)
for tl_face, mask in rasterized_tl_lanes.items()
if np.any(mask)}
return {'image_semantic': img, 'tl_lanes_masks4': non_missing_rasterized_tl_lanes}