-
Notifications
You must be signed in to change notification settings - Fork 5
/
Copy pathgroup_shape_generation.py
172 lines (150 loc) · 6.96 KB
/
group_shape_generation.py
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
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
import cv2
import numpy as np
from scipy.spatial import ConvexHull
from scipy.stats import norm
from scipy.stats import truncnorm
class GroupShapeGeneration(object):
# This class takes the information from grouping and
# generates social group shapes.
# Group shapes are generated by calling the generate_group_shape
# method after classs initialization.
#
# Group shapes are formatted as either a sequence of coordinates
# in the counter clock direction representing the convex shape blob,
# or, if an image is given, formatted as drawing the blob on the image.
def __init__(self, msg):
# Initialization
# Inputs:
# msg: message class object (should have data loaded first)
if msg.if_processed_data:
self.video_position_matrix = msg.video_position_matrix
self.video_velocity_matrix = msg.video_velocity_matrix
self.video_pedidx_matrix = msg.video_pedidx_matrix
else:
raise Exception('Data has not been loaded!')
if msg.if_processed_group:
self.video_labels_matrix = msg.video_labels_matrix
else:
raise Exception('Grouping has not been performed!')
return
def _find_label_properties(self, frame_idx, label):
# Given a frame index and a group membership label (group id),
# find information (positions, velocities, person ids) of
# all the pedestrians in that group and that frame.
# Inputs:
# frame_idx: frame index of the video
# label: group membership label
# Outputs:
# positions: positions of the pedestrians in the given group and frame index.
# velocities: velocities of the pedestrians in the given group and frame index.
# pedidx: person ids of the pedestrians in the given group and frame index.
positions = []
velocities = []
pedidx = []
labels = self.video_labels_matrix[frame_idx]
for i in range(len(labels)):
if label == labels[i]:
positions.append(self.video_position_matrix[frame_idx][i])
velocities.append(self.video_velocity_matrix[frame_idx][i])
pedidx.append(self.video_pedidx_matrix[frame_idx][i])
return positions, velocities, pedidx
@staticmethod
def boundary_dist(velocity, rel_ang, laser_flag, const=0.354163):
# Parameters from Rachel Kirby's thesis
front_coeff = 1.0
side_coeff = 2.0 / 3.0
rear_coeff = 0.5
safety_dist = 0.5
velocity_x = velocity[0]
velocity_y = velocity[1]
velocity_magnitude = np.sqrt(velocity_x ** 2 + velocity_y ** 2)
variance_front = max(0.5, front_coeff * velocity_magnitude)
variance_side = side_coeff * variance_front
variance_rear = rear_coeff * variance_front
rel_ang = rel_ang % (2 * np.pi)
flag = int(np.floor(rel_ang / (np.pi / 2)))
if flag == 0:
prev_variance = variance_front
next_variance = variance_side
elif flag == 1:
prev_variance = variance_rear
next_variance = variance_side
elif flag == 2:
prev_variance = variance_rear
next_variance = variance_side
else:
prev_variance = variance_front
next_variance = variance_side
dist = np.sqrt(const / ((np.cos(rel_ang) ** 2 / (2 * prev_variance)) + (np.sin(rel_ang) ** 2 / (2 * next_variance))))
dist = max(safety_dist, dist)
# Offset pedestrian radius
if laser_flag:
dist = dist - 0.5 + 1e-9
return dist
@classmethod
def draw_social_shapes(cls, position, velocity, laser_flag, const=0.35):
# This function draws social group shapes
# given the positions and velocities of the pedestrians.
total_increments = 20 # controls the resolution of the blobs
quater_increments = total_increments / 4
angle_increment = 2 * np.pi / total_increments
# Draw a personal space for each pedestrian within the group
contour_points = []
for i in range(len(position)):
center_x = position[i][0]
center_y = position[i][1]
velocity_x = velocity[i][0]
velocity_y = velocity[i][1]
velocity_angle = np.arctan2(velocity_y, velocity_x)
# Draw four quater-ovals with the axis determined by front, side and rear "variances"
# The overall shape contour does not have discontinuities.
for j in range(total_increments):
rel_ang = angle_increment * j
value = cls.boundary_dist(velocity[i], rel_ang, laser_flag, const)
addition_angle = velocity_angle + rel_ang
x = center_x + np.cos(addition_angle) * value
y = center_y + np.sin(addition_angle) * value
contour_points.append((x, y))
# Get the convex hull of all the personal spaces
convex_hull_vertices = []
hull = ConvexHull(np.array(contour_points))
for i in hull.vertices:
hull_vertice = (contour_points[i][0], contour_points[i][1])
convex_hull_vertices.append(hull_vertice)
return convex_hull_vertices
@classmethod
def draw_all_social_spaces(cls, gp_labels, positions, velocities, laser_flag, const=None):
all_vertices = []
all_labels = np.unique(gp_labels)
for curr_label in all_labels:
group_positions = []
group_velocities = []
for i, l in enumerate(gp_labels):
if l == curr_label:
group_positions.append(positions[i])
group_velocities.append(velocities[i])
if const == None:
vertices = cls.draw_social_shapes(group_positions, group_velocities, laser_flag)
else:
vertices = cls.draw_social_shapes(group_positions, group_velocities, laser_flag,
const)
all_vertices.append(vertices)
return all_vertices
def generate_group_shape(self, frame_idx, group_label):
# Method that generates group shape
# Inputs
# frame_idx: frame number
# group_label: group id
# frame(optional): an image in numpy array (opencv image format)
# Outputs
# If an image frame is not provided
# vertices: coordinates (in meters) that draws a convex shape blob
# in the clockwise direction.
# If an image frame is provided
# frame: an updated image with a group shape drawn on it.
# (Regardless) pedidx: the person ids of the pedestrians in the given group.
positions, velocities, pedidx = self._find_label_properties(frame_idx, group_label)
if len(positions) == 0:
raise Exception('Group does not exist in the given frame!')
vertices = self.draw_social_shapes(positions, velocities)
return vertices, pedidx