File size: 5,116 Bytes
da2e2ac |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 |
import io
import logging
import os
import pickle
import uuid
from pathlib import Path
import hydra
import matplotlib.pyplot as plt
import numpy as np
import torch
from PIL import Image
from hydra.utils import instantiate
from matplotlib.collections import LineCollection
from nuplan.planning.utils.multithreading.worker_utils import worker_map
from omegaconf import DictConfig
from tqdm import tqdm
from navsim.common.dataclasses import AgentInput, Scene
from navsim.common.dataclasses import SensorConfig
from navsim.common.dataloader import SceneLoader
from navsim.planning.script.builders.worker_pool_builder import build_worker
logger = logging.getLogger(__name__)
CONFIG_PATH = "../../planning/script/config/pdm_scoring"
CONFIG_NAME = "run_pdm_score_ddp"
norm = plt.Normalize(vmin=0.0, vmax=1.0)
cmap = plt.get_cmap('viridis')
def get_distribution(scores, vocab, gt_traj):
# metrics = ['gt', 'noc', 'da', 'tl', 'progress', 'lk', 'dr']
metrics = ['gt', 'noc', 'tl', 'progress', 'lk', 'dr']
fig, axes = plt.subplots(2, 3, figsize=(16.2, 10.8))
for i, ax in enumerate(axes.flat):
metric = metrics[i]
ax.set_xlim(-5, 65)
ax.set_ylim(-25, 25)
ax.set_title(f"Metric {metric}")
if metric == 'gt':
ax.plot(gt_traj[:, 0], gt_traj[:, 1], c='r', alpha=1.0)
continue
vocab_scores = scores[metric]
line_collection = LineCollection(vocab[..., :2],
colors=[cmap(norm(score)) for score in vocab_scores],
alpha=[1.0 if score > 0.1 else 0.001 for score in vocab_scores])
ax.add_collection(line_collection)
fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), cax=fig.add_axes([0.92, 0.15, 0.02, 0.7]))
plt.tight_layout(rect=[0, 0, 0.9, 1])
buf = io.BytesIO()
plt.savefig(buf, format='png')
buf.seek(0)
image = Image.open(buf)
return image
def worker_task(args):
node_id = int(os.environ.get("NODE_RANK", 0))
thread_id = str(uuid.uuid4())
logger.info(f"Starting worker in thread_id={thread_id}, node_id={node_id}")
for arg in tqdm(args, desc="Running visualization"):
token, gt_scores, vocab = arg['token'], arg['gt_scores'], arg['vocab']
scene_loader = arg['scene_loader']
agent_input = AgentInput.from_scene_dict_list(
scene_loader.scene_frames_dicts[token],
scene_loader._sensor_blobs_path,
scene_loader._scene_filter.num_history_frames,
scene_loader._sensor_config
)
gt_traj = Scene.from_scene_dict_list(
scene_loader.scene_frames_dicts[token],
scene_loader._sensor_blobs_path,
scene_loader._scene_filter.num_history_frames,
10,
scene_loader._sensor_config
).get_future_trajectory(int(4 / 0.5))
gt_traj = gt_traj.poses
# inf traj + gt traj
cam = agent_input.cameras[-1].cam_f0
img, cam2lidar_rot, cam2lidar_tran, cam_intrin = cam.image, cam.sensor2lidar_rotation, cam.sensor2lidar_translation, cam.intrinsics
img = Image.fromarray(img.astype('uint8'), 'RGB')
# distributions of vocab
figs = get_distribution(gt_scores, vocab, gt_traj)
# concat
total_width = img.width + figs.width
max_height = max(img.height, figs.height)
new_image = Image.new('RGB', (total_width, max_height))
new_image.paste(img, (0, 0))
new_image.paste(figs, (img.width, 0))
output_dir = args[0]['result_dir']
new_image.save(f'{output_dir}/{token}/{token}.png')
return []
@hydra.main(config_path=CONFIG_PATH, config_name=CONFIG_NAME)
def main(cfg: DictConfig) -> None:
data_path = Path(cfg.navsim_log_path)
sensor_blobs_path = Path(cfg.sensor_blobs_path)
scene_filter = instantiate(cfg.scene_filter)
scene_loader = SceneLoader(
data_path=data_path,
scene_filter=scene_filter,
sensor_blobs_path=sensor_blobs_path,
sensor_config=SensorConfig(
cam_f0=True,
cam_l0=True,
cam_l1=True,
cam_l2=True,
cam_r0=True,
cam_r1=True,
cam_r2=True,
cam_b0=True,
lidar_pc=False,
)
)
worker = build_worker(cfg)
result_dir = f'{os.getenv("NAVSIM_TRAJPDM_ROOT")}/vocab_expanded_{cfg.vocab_size}_{cfg.scene_filter_name}'
vocab = np.load(f'{os.getenv("NAVSIM_DEVKIT_ROOT")}/traj_final/test_{cfg.vocab_size}_kmeans.npy')
data_points = []
valid_tokens = os.listdir(result_dir)
valid_tokens = set(valid_tokens) & set(scene_loader.tokens)
for token in tqdm(valid_tokens):
gt_scores = pickle.load(open(f'{result_dir}/{token}/tmp.pkl', 'rb'))
data_points.append({
'token': token,
'scene_loader': scene_loader,
'result_dir': result_dir,
'vocab': vocab,
'gt_scores': gt_scores,
})
worker_map(worker, worker_task, data_points)
if __name__ == "__main__":
main()
|