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]