def plot_log_one_at_a_time(self, log_id="", idx=-1, save_video=True, city=""): """ Playback a log in the static context of a map. In the far left frame, we show the car moving in the map. In the middle frame, we show the car's LiDAR returns (in the map frame). In the far right frame, we show the front camera's RGB image. """ avm = ArgoverseMap() for city_name, trajs in self.per_city_traj_dict.items(): if city != "": if city != city_name: continue if city_name not in ["PIT", "MIA"]: logger.info("Unknown city") continue log_ids = [] logger.info(f"{city_name} has {len(trajs)} tracks") if log_id == "": # first iterate over the instance axis for traj_idx, (traj, log_id) in enumerate(trajs): log_ids += [log_id] else: log_ids = [log_id] # eliminate the duplicates for log_id in set(log_ids): logger.info(f"Log: {log_id}") ply_fpaths = sorted( glob.glob(f"{self.dataset_dir}/{log_id}/lidar/PC_*.ply")) # then iterate over the time axis for i, ply_fpath in enumerate(ply_fpaths): if idx != -1: if i != idx: continue if i % 500 == 0: logger.info(f"\tOn file {i} of {log_id}") lidar_timestamp = ply_fpath.split("/")[-1].split( ".")[0].split("_")[-1] lidar_timestamp = int(lidar_timestamp) print("Lidar timestamp = ", lidar_timestamp) # added by Yike print("Log Egopose Dict = ", self.log_egopose_dict) if lidar_timestamp not in self.log_egopose_dict[log_id]: all_available_timestamps = sorted( self.log_egopose_dict[log_id].keys()) diff = (all_available_timestamps[0] - lidar_timestamp) / 1e9 logger.info( f"{diff:.2f} sec before first labeled sweep") continue logger.info(f"\tt={lidar_timestamp}") if self.plot_lidar_bev: fig = plt.figure(figsize=(15, 15)) plt.title( f"Log {log_id} @t={lidar_timestamp} in {city_name}" ) plt.axis("off") # ax_map = fig.add_subplot(131) ax_3d = fig.add_subplot(111) # ax_rgb = fig.add_subplot(133) plt.ion() # added by Yike # need the ego-track here pose_city_to_ego = self.log_egopose_dict[log_id][ lidar_timestamp] xcenter = pose_city_to_ego["translation"][0] ycenter = pose_city_to_ego["translation"][1] ego_center_xyz = np.array(pose_city_to_ego["translation"]) city_to_egovehicle_se3 = SE3( rotation=pose_city_to_ego["rotation"], translation=ego_center_xyz) if self.plot_lidar_bev: xmin = xcenter - 80 # 150 xmax = xcenter + 80 # 150 ymin = ycenter - 80 # 150 ymax = ycenter + 80 # 150 # ax_map.scatter(xcenter, ycenter, 200, color="g", marker=".", zorder=2) # ax_map.set_xlim([xmin, xmax]) # ax_map.set_ylim([ymin, ymax]) local_lane_polygons = avm.find_local_lane_polygons( [xmin, xmax, ymin, ymax], city_name) local_das = avm.find_local_driveable_areas( [xmin, xmax, ymin, ymax], city_name) lidar_pts = load_ply(ply_fpath) if self.plot_lidar_in_img: draw_ground_pts_in_image( self.sdb, copy.deepcopy(lidar_pts), city_to_egovehicle_se3, avm, log_id, lidar_timestamp, city_name, self.dataset_dir, self.experiment_prefix, plot_ground=True, ) if self.plot_lidar_bev: driveable_area_pts = copy.deepcopy(lidar_pts) driveable_area_pts = city_to_egovehicle_se3.transform_point_cloud( driveable_area_pts) # put into city coords driveable_area_pts = avm.remove_non_driveable_area_points( driveable_area_pts, city_name) driveable_area_pts = avm.remove_ground_surface( driveable_area_pts, city_name) driveable_area_pts = city_to_egovehicle_se3.inverse_transform_point_cloud( driveable_area_pts ) # put back into ego-vehicle coords self.render_bev_labels_mpl( city_name, ax_3d, "ego_axis", lidar_pts, copy.deepcopy(local_lane_polygons), copy.deepcopy(local_das), log_id, lidar_timestamp, city_to_egovehicle_se3, avm, ) fig.tight_layout() if not Path( f"{self.experiment_prefix}_per_log_viz/{log_id}" ).exists(): os.makedirs( f"{self.experiment_prefix}_per_log_viz/{log_id}" ) plt.savefig( f"{self.experiment_prefix}_per_log_viz/{log_id}/{city_name}_{log_id}_{lidar_timestamp}.png", dpi=400, ) # plt.show() # plt.close("all") # after all frames are processed, write video with saved images if save_video: if self.plot_lidar_bev: fps = 10 img_wildcard = f"{self.experiment_prefix}_per_log_viz/{log_id}/{city_name}_{log_id}_%*.png" output_fpath = f"{self.experiment_prefix}_per_log_viz/{log_id}_lidar_roi_nonground.mp4" write_nonsequential_idx_video(img_wildcard, output_fpath, fps) if self.plot_lidar_in_img: for camera_name in RING_CAMERA_LIST + STEREO_CAMERA_LIST: image_prefix = ( f"{self.experiment_prefix}_per_log_viz/{log_id}/{camera_name}/{camera_name}_%d.jpg" ) output_prefix = f"{self.experiment_prefix}_per_log_viz/{log_id}_{camera_name}" write_video(image_prefix, output_prefix)
# %% fig = plt.figure(figsize=(10, 10)) ax = fig.add_subplot(111) xcenter, ycenter, _ = argoverse_data.get_pose(idx).translation r = 50 xmin = xcenter - r # 150 xmax = xcenter + r # 150 ymin = ycenter - r # 150 ymax = ycenter + r # 150 ax.scatter(xcenter, ycenter, 200, color="g", marker=".", zorder=2) ax.set_xlim([xmin, xmax]) ax.set_ylim([ymin, ymax]) local_lane_polygons = am.find_local_lane_polygons([xmin, xmax, ymin, ymax], city_name) local_das = am.find_local_driveable_areas([xmin, xmax, ymin, ymax], city_name) domv.render_bev_labels_mpl( city_name=city_name, ax=ax, axis="city_axis", lidar_pts=None, local_lane_polygons=copy.deepcopy(local_lane_polygons), local_das=copy.deepcopy(local_das), log_id=log_id, timestamp=argoverse_data.lidar_timestamp_list[idx], city_to_egovehicle_se3=city_to_egovehicle_se3, avm=am, vis_other_objects=True) prev_timestamp = argoverse_data.lidar_timestamp_list[idx]