def pedestrian_hit(trajectory, agent_radius):
"""Returns true if pedestrian is hit in any of the state in
the trajectory.
:param trajectory: trajecory comprised of states_dicts.
:type trajectory: list of state_dicts
:param agent_radius: width of the agent in environment.
:type agent_radius: float.
:return: True if pedestrian hit in last state_dict, false otherwise.
:rtype: Boolean.
"""
for state in trajectory:
pedestrians = state["obstacles"]
agent_position = state["agent_state"]["position"]
for ped in pedestrians:
ped_position = ped["position"]
distance = dist_2d(ped_position, agent_position)
if distance <= (agent_radius):
return True
return False
def goal_reached(trajectory, goal_radius, agent_radius):
"""Returns true if the goal was reached in the last state of this
trajectory. Goal is considered reached if the distance between agent and
goal is less than their respective radii (provided it has not hit a
pedestrian in the way)
:param trajectory: Trajectory of state dictionaries generated by the
environment.
:type trajectory: list of state dictionaries.
:param goal_radius: radius of goal.
:type goal_radius: float
:param agent_radius: radius of agent.
:type agent_radius: float
:return: True of distance between goal and agent is less than goal_radius
+ agent_radius.
:rtype: Boolean.
"""
if pedestrian_hit(trajectory, agent_radius):
return False
else:
agent_position = trajectory[-1]["agent_state"]["position"]
goal_position = trajectory[-1]["goal_state"]
return (dist_2d(agent_position, goal_position) <=
goal_radius + agent_radius)
def count_collisions(trajectory, agent_radius):
"""Counts the number of distinct collisions between agent and
pedestrians in the provided trajectory.
:param trajectory: Trajectory of agent as a list of state dictionaries as generated by the
environment.
:type trajectory: list of (state) dictionaries
:param agent_radius: Radius of agents in the environment.
:type agent_radius: float
:return: number of collisions with pedestrians in trajectory.
:rtype: int
"""
collision_count = 0
collision_list = []
for state in trajectory:
agent_pos = state["agent_state"]["position"]
for pedestrian in state["obstacles"]:
ped_position = pedestrian["position"]
if dist_2d(ped_position, agent_pos) <= agent_radius:
if pedestrian['id'] not in collision_list:
collision_count += 1
collision_list.append(pedestrian["id"])
else:
if pedestrian["id"] in collision_list:
collision_list.remove(pedestrian["id"])
return collision_count
def proxemic_intrusions(trajectory, units_to_meters_ratio):
"""
Calculate number of proxemic intrusions as defined by the proxemics model:
"E. Hall, Handbook of Proxemics Research. Society for the Anthropology of
Visual Communications, 1974."
Based on Vasquez et. al's use in their paper:
"IRL algos and features for robot navigation in crowds"
This is a similified version ignoring public space and near/far phases,
using thresholds found at:
"https://en.wikipedia.org/wiki/Proxemics"
:param trajectory: Trajectory of states generated by environment.
:type trajectory: List of state_dicts
:param units_to_meters_ratio: the ratio (env distance unit length) / (1
meter). e.g. if 1 env unit is 10 cm, this ratio is 0.1.
:type units_to_meters_ratio: float.
:return: Number of intrusions in initimate, personal, and social spaces.
:return type: tuple (int, int, int)
"""
# Thresholds
INTIMIATE_DISTANCE = 0.5 * units_to_meters_ratio
PERSONAL_DISTANCE = 1.2 * units_to_meters_ratio
SOCIAL_DISTANCE = 3.7 * units_to_meters_ratio
# intrusion counts
intimiate_intrusions = 0
personal_intrusions = 0
social_intrusions = 0
for traj in trajectory:
pedestrians = traj["obstacles"]
agent_position = traj["agent_state"]["position"]
for ped in pedestrians:
ped_position = ped["position"]
distance = dist_2d(ped_position, agent_position)
if distance <= INTIMIATE_DISTANCE:
intimiate_intrusions += 1
elif INTIMIATE_DISTANCE < distance <= PERSONAL_DISTANCE:
personal_intrusions += 1
elif PERSONAL_DISTANCE < distance <= SOCIAL_DISTANCE:
social_intrusions += 1
elif distance > SOCIAL_DISTANCE:
continue
else:
raise ValueError("Distance did not fit in any bins.")
return intimiate_intrusions, personal_intrusions, social_intrusions
def find_pedestrian_group(pedestrian_trajectory,
proximity_threshold=40,
temporal_threshold=0.7):
"""
Returns a list containing the id of the pedestrians who are likely to belong to a group
input:
trajectory : a filename containing the trajectory of the said pedestrian.
proximity_threshold : integer containing how close the nearby pedestrians have to
be to the subject inorder to be qualified to be in the same group
temporal_threshold : a float mentioning for what fraction of the total trajectory does a
nearby pedestrian have to be inside the proximity_threshold of a
subject in order to be classified as a fellow group member
output:
group : a list containing the ids of the pedestrians that are in the same group as the
subject pedestrian
"""
#read the trajectory from the file
assert os.path.isfile(pedestrian_trajectory), 'Bad trajectory file!'
traj = np.load(pedestrian_trajectory, allow_pickle=True)
total_frames = len(traj)
#maintain a dictionary of all the pedestrians who pierce the proximity threshold
#and see who stays inside the proximity threshold for how long.
proximity_tracker = {}
for state in traj:
for obs in state['obstacles']:
agent_position = state['agent_state']['position']
ped_dist = dist_2d(agent_position, obs['position'])
if ped_dist <= proximity_threshold:
if obs['id'] not in proximity_tracker.keys():
proximity_tracker[obs['id']] = 1
else:
proximity_tracker[obs['id']] += 1
#check through the proximity tracker to see which of the nearby pedestrians
#qualify to be in a group with the pedestrian at hand using the temporal threshold
temporal_threshold_frames = temporal_threshold * total_frames
ped_group = []
for people in proximity_tracker.keys():
if proximity_tracker[people] > temporal_threshold_frames:
ped_group.append(people)
return ped_group
def trajectory_length(trajectory):
"""
Returns the length of the trajectory.
:param trajectory: Trajectory of agent in envrionment.
:type trajectory: List of state dictionaries.
:return: length of trajectory by calculating the distance between the position of
the agent in consequtive frames.
:rtype: int.
"""
agent_cur_pos = trajectory[0]["agent_state"]["position"]
traj_length = 0
for state in trajectory[1:]:
traj_length += dist_2d(agent_cur_pos, state["agent_state"]["position"])
agent_cur_pos = state["agent_state"]["position"]
return traj_length
def distance_to_nearest_pedestrian_over_time(trajectory):
"""
At each timestep in trajectory, calculates the distances to the
nearest pedestrian.
:param trajectory: trajectory comprised of states_dicts.
:type trajectory: list of state_dicts
:return: list of distances to nearest pedestrian, ordered from t=0 to t=end.
:rtype: list of floats.
"""
min_distances = []
for traj in trajectory:
agent_position = traj["agent_state"]["position"]
pedestrians = traj["obstacles"]
ped_positions = [ped["position"] for ped in pedestrians]
distances = [
dist_2d(ped_pos, agent_position) for ped_pos in ped_positions
]
min_distances.append(np.min(distances))
return min_distances
def agent_drift_analysis(agent,
agent_type,
env,
ped_list,
feat_extractor=None,
pos_reset=20,
):
'''
Performs a drift analysis of a particular agent.
input:
agent: the policy/controller
agent_type: the type of the controller (policy network or an alternate
controller like a Potential field or something else)
env: the environment
feat_extractor: the feature extractor to be used.
ped_list: list of pedestrians on which to perform the drift analysis.
pos_reset: Number of frames after which the position of the agent gets reset
to that of the trajectory of the actual pedestrian.
output: the amount of drift incurred by the agent for the pedestrians
provided in the ped_list.
'''
drift_value = 0
segment_counter = 0
env.cur_ped = None
print('Starting drift analysis of agent :{}. Reset interval :{}'.format(agent_type,
pos_reset))
#an array containing the drift value for each pedestrian
drift_info_detailed = np.zeros(len(ped_list))
for i in tqdm(range(len(ped_list))):
#reset the world
state = env.reset_and_replace(ped=ped_list[i])
env.goal_state = copy.deepcopy(env.return_position(env.cur_ped, env.current_frame + pos_reset)['position'])
env.state['goal_state'] = copy.deepcopy(env.goal_state)
state = copy.deepcopy(env.state)
final_frame = env.final_frame
if feat_extractor is not None:
try:
feat_extractor.reset()
except AttributeError:
pass
state_feat = feat_extractor.extract_features(state)
state_feat = torch.from_numpy(state_feat).type(torch.FloatTensor).to(DEVICE)
#pass
#reset the information collector
done = False
t = 0
drift_per_ped = 0
segment_counter_per_ped = 0
abs_counter = env.current_frame
while abs_counter < final_frame:
#stop_points = []
if feat_extractor is not None:
if agent_type == 'Policy_network':
action = agent.eval_action(state_feat)
else:
#action selection for alternate controller namely potential field
action = agent.eval_action(state)
'''
if args.render:
feat_ext.overlay_bins(state)
'''
else:
action = agent.eval_action(state)
state, _, done, _ = env.step(action)
drift_value += dist_2d(env.ghost_state['position'], env.agent_state['position'])
drift_per_ped += dist_2d(env.ghost_state['position'], env.agent_state['position'])
if feat_extractor is not None:
state_feat = feat_extractor.extract_features(state)
state_feat = torch.from_numpy(state_feat).type(torch.FloatTensor).to(DEVICE)
t += 1
abs_counter += 1
if t%pos_reset == 0:
segment_counter += 1
segment_counter_per_ped += 1
env.agent_state = env.return_position(env.cur_ped, env.current_frame)
env.state['agent_state'] = copy.deepcopy(env.agent_state)
'''
pos = env.agent_state['position']
stop_points.append(pos)
for pos in stop_points:
pygame.draw.circle(pygame.display.get_surface(), (0,0,0), (int(pos[1]), int(pos[0])), 20)
pygame.display.update()
'''
env.goal_state = env.return_position(env.cur_ped, env.current_frame + pos_reset)['position']
env.state['goal_state'] = copy.deepcopy(env.goal_state)
state = copy.deepcopy(env.state)
env.release_control = False
t = 0
done = False
if segment_counter_per_ped == 0:
segment_counter_per_ped = 1
drift_info_detailed[i] = drift_per_ped/segment_counter_per_ped
return drift_info_detailed
def anisotropic_intrusions(trajectory, radius, lambda_param=2.0):
"""
Measures number of times the anisotropic radius of a pedestrian is violated by an agent.
This implementation is based on the one presented in Vasquez's paper:
"IRL algos and features for robot navigation in crowds."
The anisotrpic radius can be though of a circular radius around
pedestrians that is stretched to include more of the front direction of
the pedestrian and less of the back direction. This is because being in
front of a walking pedestrian is considered worse than being behind or to
the sides of a walking pedestrian.
:param trajectory: Trajectory of states generated by environment.
:type trajectory: List of state_dicts.
:param radius: (Base) radius to consider around pedestrians. This will be
stretched according to lambda_param.
:type radius: float.
:param lambda_param: factor by which to stretch radius parameter,
defaults to 2.0
:type lambda_param: float, optional
:raises ValueError: If for some reason the angle between (agent_pos -
pedestrian_pos) and pedestrian_orientation does not fit in [0,2pi]
:return: Number of times front, side, and back intrusions happen in
trajectory.
:rtype: tuple (int, int, int)
"""
# intrusion counts
back_anisotropic_intrusion = 0
side_anisotropic_intrusion = 0
front_anisotropic_intrusion = 0
for traj in trajectory:
agent_position = traj["agent_state"]["position"]
pedestrians = traj["obstacles"]
for ped in pedestrians:
ped_position = ped["position"]
ped_orientation = ped["orientation"]
vector_to_agent = agent_position - ped_position
if ped_orientation is None:
warnings.warn(
"pedestrian orientation is none, setting to (1.0, 0.0)")
ped_orientation = np.array([1.0, 0.0])
angle = angle_between(vector_to_agent, ped_orientation)
distance = dist_2d(agent_position, ped_position)
# evaluate if agent is in anisotropic radius
anisotropy_factor = lambda_param - 0.5 * (1 - lambda_param) * (
1 + np.cos(angle))
anisotropic_radius = radius * anisotropy_factor
if distance < anisotropic_radius:
if angle < 0.25 * np.pi:
front_anisotropic_intrusion += 1
elif 0.25 * np.pi <= angle < 0.75 * np.pi:
side_anisotropic_intrusion += 1
elif 0.75 * np.pi <= angle <= np.pi:
back_anisotropic_intrusion += 1
else:
raise ValueError(
"Cannot bin angle in any of the thresholds.")
return (
front_anisotropic_intrusion,
side_anisotropic_intrusion,
back_anisotropic_intrusion,
)