Spaces:
Sleeping
Sleeping
File size: 3,598 Bytes
5769ee4 |
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 |
import math
import os
from mmcv import Config
import matplotlib.pyplot as plt
import numpy as np
from risk_biased.scene_dataset.scene import RandomScene, RandomSceneParams
from risk_biased.scene_dataset.scene_plotter import ScenePlotter
from risk_biased.utils.cost import (
DistanceCostNumpy,
DistanceCostParams,
TTCCostNumpy,
TTCCostParams,
)
from risk_biased.utils.risk import get_risk_level_sampler
if __name__ == "__main__":
working_dir = os.path.dirname(os.path.realpath(__file__))
config_path = os.path.join(
working_dir, "..", "..", "risk_biased", "config", "learning_config.py"
)
config = Config.fromfile(config_path)
risk_sampler = get_risk_level_sampler(config.risk_distribution)
is_torch = False
n_samples = 1000
sample_every = 10
scene_params = RandomSceneParams.from_config(config)
scene_params.batch_size = n_samples
# Get a batch of random pedestrians
scene = RandomScene(
scene_params,
is_torch=is_torch,
)
# Define the initial positions of pedestrians
# Slow dangerous, fast safe settings:
percent_right = 0.8
percent_top = 0.6
angle = 5 * np.pi / 4
# Slow safe, fast dangerous settings:
# percent_right = 0.8
# percent_top = 1.1
# angle = 5 * np.pi / 4
positions = np.array([[[percent_right, percent_top]]] * n_samples)
angles = np.array([[angle]] * n_samples)
scene.set_pedestrians_states(positions, angles)
dist_cost_func = DistanceCostNumpy(DistanceCostParams.from_config(config))
ttc_cost_func = TTCCostNumpy(TTCCostParams.from_config(config))
len_traj = int(config.time_scene / scene.dt)
ped_trajs = scene.get_pedestrians_trajectories()
ego_traj = scene.get_ego_ref_trajectory(config.sample_times)
travel_distances = np.sqrt(
np.square(ped_trajs[..., -1, :] - ped_trajs[..., 0, :]).sum(-1)
)
dist_cost, dist = dist_cost_func(
ego_traj[:, :, config.num_steps :], ped_trajs[:, :, config.num_steps :]
)
ttc_cost, (ttc, dist) = ttc_cost_func(
ego_traj[:, :, config.num_steps :],
ped_trajs[:, :, config.num_steps :],
scene.get_ego_ref_velocity(),
scene.get_pedestrians_velocities(),
)
fig, ax = plt.subplots()
plotter = ScenePlotter(scene, ax)
plotter.draw_scene(0, time=config.num_steps * config.dt)
# plotter.draw_trajectory(ped_trajs[0, config.num_steps :], color="g")
plotter.draw_all_trajectories(
ped_trajs[:, :, config.num_steps :], color_value=ttc_cost
)
def plot_histograms(travel_distances, dist_cost, ttc_cost, label=""):
# Open the plots for the sampled future times
fig, ax = plt.subplots(1, 3)
fig.suptitle(label)
# Plot histograms of traveled distances, depending on the parameters.
# It should be multi-modal. There is a minimum distance and a maximum distance and travel distance variations within these bounds.
ax[0].set_title("Travel distance")
ax[1].set_title("Distance cost")
ax[2].set_title("TTC cost")
ax[0].hist(travel_distances[:, -1], bins=30)
ax[1].hist(dist_cost[:], bins=30)
ax[1].set_ylim([0, 3 * math.sqrt(n_samples)])
ax[2].hist(ttc_cost[:], bins=30)
ax[2].set_ylim([0, 3 * math.sqrt(n_samples)])
agent_selected = 0
plot_histograms(travel_distances[:, agent_selected], dist_cost, ttc_cost, "Data")
print(f"Average ttc cost: {ttc_cost.mean()}")
print(f"Average distance cost: {dist_cost.mean()}")
plt.tight_layout()
plt.show()
|