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}