def render_bev_labels_mpl( self, city_name: str, ax: plt.Axes, axis: str, lidar_pts: np.ndarray, local_lane_polygons: np.ndarray, local_das: np.ndarray, log_id: str, timestamp: int, city_to_egovehicle_se3: SE3, avm: ArgoverseMap, ) -> None: """Plot nearby lane polygons and nearby driveable areas (da) on the Matplotlib axes. Args: city_name: The name of a city, e.g. `"PIT"` ax: Matplotlib axes axis: string, either 'ego_axis' or 'city_axis' to demonstrate the lidar_pts: Numpy array of shape (N,3) local_lane_polygons: Polygons representing the local lane set local_das: Numpy array of objects of shape (N,) where each object is of shape (M,3) log_id: The ID of a log timestamp: In nanoseconds city_to_egovehicle_se3: Transformation from egovehicle frame to city frame avm: ArgoverseMap instance """ if axis is not "city_axis": # rendering instead in the egovehicle reference frame for da_idx, local_da in enumerate(local_das): local_da = city_to_egovehicle_se3.inverse_transform_point_cloud( local_da) local_das[da_idx] = rotate_polygon_about_pt( local_da, city_to_egovehicle_se3.rotation, np.zeros(3)) for lane_idx, local_lane_polygon in enumerate(local_lane_polygons): local_lane_polygon = city_to_egovehicle_se3.inverse_transform_point_cloud( local_lane_polygon) local_lane_polygons[lane_idx] = rotate_polygon_about_pt( local_lane_polygon, city_to_egovehicle_se3.rotation, np.zeros(3)) draw_lane_polygons(ax, local_lane_polygons) draw_lane_polygons(ax, local_das, color="tab:pink") if axis is not "city_axis": lidar_pts = rotate_polygon_about_pt( lidar_pts, city_to_egovehicle_se3.rotation, np.zeros((3, ))) draw_point_cloud_bev(ax, lidar_pts) objects = self.log_timestamp_dict[log_id][timestamp] all_occluded = True for frame_rec in objects: if frame_rec.occlusion_val != IS_OCCLUDED_FLAG: all_occluded = False if not all_occluded: for i, frame_rec in enumerate(objects): bbox_city_fr = frame_rec.bbox_city_fr bbox_ego_frame = frame_rec.bbox_ego_frame color = frame_rec.color if frame_rec.occlusion_val != IS_OCCLUDED_FLAG: bbox_ego_frame = rotate_polygon_about_pt( bbox_ego_frame, city_to_egovehicle_se3.rotation, np.zeros((3, ))) if axis is "city_axis": plot_bbox_2D(ax, bbox_city_fr, color) if self.plot_lane_tangent_arrows: bbox_center = np.mean(bbox_city_fr, axis=0) tangent_xy, conf = avm.get_lane_direction( query_xy_city_coords=bbox_center[:2], city_name=city_name) dx = tangent_xy[0] * LANE_TANGENT_VECTOR_SCALING dy = tangent_xy[1] * LANE_TANGENT_VECTOR_SCALING ax.arrow(bbox_center[0], bbox_center[1], dx, dy, color="r", width=0.5, zorder=2) else: plot_bbox_2D(ax, bbox_ego_frame, color) cuboid_lidar_pts, _ = filter_point_cloud_to_bbox_2D_vectorized( bbox_ego_frame[:, :2], copy.deepcopy(lidar_pts)) if cuboid_lidar_pts is not None: draw_point_cloud_bev(ax, cuboid_lidar_pts, color) else: logger.info(f"all occluded at {timestamp}") xcenter = city_to_egovehicle_se3.translation[0] ycenter = city_to_egovehicle_se3.translation[1] ax.text(xcenter - 146, ycenter + 50, "ALL OBJECTS OCCLUDED", fontsize=30)
def render_bev_labels_mpl( self, city_name: str, ax: plt.Axes, axis: str, # lidar_pts: np.ndarray, local_lane_polygons: np.ndarray, local_das: np.ndarray, log_id: str, timestamp: int, timestamp_ind: int, city_to_egovehicle_se3: SE3, avm: ArgoverseMap, ) -> None: """Plot nearby lane polygons and nearby driveable areas (da) on the Matplotlib axes. Args: city_name: The name of a city, e.g. `"PIT"` ax: Matplotlib axes axis: string, either 'ego_axis' or 'city_axis' to demonstrate the lidar_pts: Numpy array of shape (N,3) local_lane_polygons: Polygons representing the local lane set local_das: Numpy array of objects of shape (N,) where each object is of shape (M,3) log_id: The ID of a log timestamp: In nanoseconds city_to_egovehicle_se3: Transformation from egovehicle frame to city frame avm: ArgoverseMap instance """ if axis is not "city_axis": # rendering instead in the egovehicle reference frame for da_idx, local_da in enumerate(local_das): local_da = city_to_egovehicle_se3.inverse_transform_point_cloud( local_da) # local_das[da_idx] = rotate_polygon_about_pt(local_da, city_to_egovehicle_se3.rotation, np.zeros(3)) local_das[da_idx] = local_da for lane_idx, local_lane_polygon in enumerate(local_lane_polygons): local_lane_polygon = city_to_egovehicle_se3.inverse_transform_point_cloud( local_lane_polygon) # local_lane_polygons[lane_idx] = rotate_polygon_about_pt( # local_lane_polygon, city_to_egovehicle_se3.rotation, np.zeros(3) # ) local_lane_polygons[lane_idx] = local_lane_polygon draw_lane_polygons(ax, local_lane_polygons, color="tab:gray") draw_lane_polygons(ax, local_das, color=(1, 0, 1)) # render ego vehicle bbox_ego = np.array([[-1.0, -1.0, 0.0], [3.0, -1.0, 0.0], [-1.0, 1.0, 0.0], [3.0, 1.0, 0.0]]) # bbox_ego = rotate_polygon_about_pt(bbox_ego, city_to_egovehicle_se3.rotation, np.zeros((3,))) plot_bbox_2D(ax, bbox_ego, 'g') # render surrounding vehicle and pedestrians objects = self.log_timestamp_dict[log_id][timestamp] hist_objects = {} all_occluded = True for frame_rec in objects: if frame_rec.occlusion_val != IS_OCCLUDED_FLAG: all_occluded = False hist_objects[frame_rec.track_uuid] = [ None for i in range(0, SURR_TIME_STEP + 1) ] hist_objects[ frame_rec.track_uuid][SURR_TIME_STEP] = frame_rec.bbox_city_fr if not all_occluded: for ind, i in enumerate( range(timestamp_ind - SURR_TIME_STEP, timestamp_ind)): objects = self.log_timestamp_dict[log_id][self.timestamps[i]] for frame_rec in objects: if frame_rec.track_uuid in hist_objects: hist_objects[ frame_rec.track_uuid][ind] = frame_rec.bbox_city_fr # render color with decreasing lightness color_lightness = np.linspace(1, 0, SURR_TIME_STEP + 2)[1:].tolist() for track_id in hist_objects: for ind_color, bbox_city_fr in enumerate( hist_objects[track_id]): if bbox_city_fr is None: continue bbox_relative = city_to_egovehicle_se3.inverse_transform_point_cloud( bbox_city_fr) x = bbox_relative[:, 0].tolist() y = bbox_relative[:, 1].tolist() x[1], x[2], x[3], y[1], y[2], y[3] = x[2], x[3], x[1], y[ 2], y[3], y[1] ax.fill(x, y, c=(1, 1, color_lightness[ind_color]), alpha=1, zorder=1) else: logger.info(f"all occluded at {timestamp}") xcenter = city_to_egovehicle_se3.translation[0] ycenter = city_to_egovehicle_se3.translation[1] ax.text(xcenter - 146, ycenter + 50, "ALL OBJECTS OCCLUDED", fontsize=30)