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
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
| class ArgoDataset(Dataset):
def __init__(self, split, config, train=True):
self.config = config
self.train = train
if 'preprocess' in config and config['preprocess']:
if train:
self.split = np.load(self.config['preprocess_train'], allow_pickle=True)
else:
self.split = np.load(self.config['preprocess_val'], allow_pickle=True)
else:
self.avl = ArgoverseForecastingLoader(split)
self.avl.seq_list = sorted(self.avl.seq_list)
self.am = ArgoverseMap()
if 'raster' in config and config['raster']:
#TODO: DELETE
self.map_query = MapQuery(config['map_scale'])
def __getitem__(self, idx):
if 'preprocess' in self.config and self.config['preprocess']:
data = self.split[idx]
if self.train and self.config['rot_aug']:
new_data = dict()
for key in ['city', 'orig', 'gt_preds', 'has_preds']:
if key in data:
new_data[key] = ref_copy(data[key])
dt = np.random.rand() * self.config['rot_size']#np.pi * 2.0
theta = data['theta'] + dt
new_data['theta'] = theta
new_data['rot'] = np.asarray([
[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]], np.float32)
rot = np.asarray([
[np.cos(-dt), -np.sin(-dt)],
[np.sin(-dt), np.cos(-dt)]], np.float32)
new_data['feats'] = data['feats'].copy()
new_data['feats'][:, :, :2] = np.matmul(new_data['feats'][:, :, :2], rot)
new_data['ctrs'] = np.matmul(data['ctrs'], rot)
graph = dict()
for key in ['num_nodes', 'turn', 'control', 'intersect', 'pre', 'suc', 'lane_idcs', 'left_pairs', 'right_pairs', 'left', 'right']:
graph[key] = ref_copy(data['graph'][key])
graph['ctrs'] = np.matmul(data['graph']['ctrs'], rot)
graph['feats'] = np.matmul(data['graph']['feats'], rot)
new_data['graph'] = graph
data = new_data
else:
new_data = dict()
for key in ['city', 'orig', 'gt_preds', 'has_preds', 'theta', 'rot', 'feats', 'ctrs', 'graph']:
if key in data:
new_data[key] = ref_copy(data[key])
data = new_data
if 'raster' in self.config and self.config['raster']:
data.pop('graph')
x_min, x_max, y_min, y_max = self.config['pred_range']
cx, cy = data['orig']
region = [cx + x_min, cx + x_max, cy + y_min, cy + y_max]
raster = self.map_query.query(region, data['theta'], data['city'])
data['raster'] = raster
return data
data = self.read_argo_data(idx)
data = self.get_obj_feats(data)
data['idx'] = idx
if 'raster' in self.config and self.config['raster']:
x_min, x_max, y_min, y_max = self.config['pred_range']
cx, cy = data['orig']
region = [cx + x_min, cx + x_max, cy + y_min, cy + y_max]
raster = self.map_query.query(region, data['theta'], data['city'])
data['raster'] = raster
return data
data['graph'] = self.get_lane_graph(data)
return data
def __len__(self):
if 'preprocess' in self.config and self.config['preprocess']:
return len(self.split)
else:
return len(self.avl)
def read_argo_data(self, idx):
city = copy.deepcopy(self.avl[idx].city)
"""TIMESTAMP,TRACK_ID,OBJECT_TYPE,X,Y,CITY_NAME"""
df = copy.deepcopy(self.avl[idx].seq_df)
agt_ts = np.sort(np.unique(df['TIMESTAMP'].values))
mapping = dict()
for i, ts in enumerate(agt_ts):
mapping[ts] = i
trajs = np.concatenate((
df.X.to_numpy().reshape(-1, 1),
df.Y.to_numpy().reshape(-1, 1)), 1)
steps = [mapping[x] for x in df['TIMESTAMP'].values]
steps = np.asarray(steps, np.int64)
objs = df.groupby(['TRACK_ID', 'OBJECT_TYPE']).groups
keys = list(objs.keys())
obj_type = [x[1] for x in keys]
agt_idx = obj_type.index('AGENT')
idcs = objs[keys[agt_idx]]
agt_traj = trajs[idcs]
agt_step = steps[idcs]
del keys[agt_idx]
ctx_trajs, ctx_steps = [], []
for key in keys:
idcs = objs[key]
ctx_trajs.append(trajs[idcs])
ctx_steps.append(steps[idcs])
data = dict()
data['city'] = city
data['trajs'] = [agt_traj] + ctx_trajs
data['steps'] = [agt_step] + ctx_steps
return data
def get_obj_feats(self, data):
orig = data['trajs'][0][19].copy().astype(np.float32)
if self.train and self.config['rot_aug']:
theta = np.random.rand() * np.pi * 2.0
else:
pre = data['trajs'][0][18] - orig
theta = np.pi - np.arctan2(pre[1], pre[0])
rot = np.asarray([
[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]], np.float32)
feats, ctrs, gt_preds, has_preds = [], [], [], []
for traj, step in zip(data['trajs'], data['steps']):
if 19 not in step:
continue
gt_pred = np.zeros((30, 2), np.float32)
has_pred = np.zeros(30, np.bool)
future_mask = np.logical_and(step >= 20, step < 50)
post_step = step[future_mask] - 20
post_traj = traj[future_mask]
gt_pred[post_step] = post_traj
has_pred[post_step] = 1
obs_mask = step < 20
step = step[obs_mask]
traj = traj[obs_mask]
idcs = step.argsort()
step = step[idcs]
traj = traj[idcs]
for i in range(len(step)):
if step[i] == 19 - (len(step) - 1) + i:
break
step = step[i:]
traj = traj[i:]
feat = np.zeros((20, 3), np.float32)
feat[step, :2] = np.matmul(rot, (traj - orig.reshape(-1, 2)).T).T
feat[step, 2] = 1.0
x_min, x_max, y_min, y_max = self.config['pred_range']
if feat[-1, 0] < x_min or feat[-1, 0] > x_max or feat[-1, 1] < y_min or feat[-1, 1] > y_max:
continue
ctrs.append(feat[-1, :2].copy())
feat[1:, :2] -= feat[:-1, :2]
feat[step[0], :2] = 0
feats.append(feat)
gt_preds.append(gt_pred)
has_preds.append(has_pred)
feats = np.asarray(feats, np.float32)
ctrs = np.asarray(ctrs, np.float32)
gt_preds = np.asarray(gt_preds, np.float32)
has_preds = np.asarray(has_preds, np.bool)
data['feats'] = feats
data['ctrs'] = ctrs
data['orig'] = orig
data['theta'] = theta
data['rot'] = rot
data['gt_preds'] = gt_preds
data['has_preds'] = has_preds
return data
def get_lane_graph(self, data):
"""Get a rectangle area defined by pred_range."""
x_min, x_max, y_min, y_max = self.config['pred_range']
radius = max(abs(x_min), abs(x_max)) + max(abs(y_min), abs(y_max))
lane_ids = self.am.get_lane_ids_in_xy_bbox(data['orig'][0], data['orig'][1], data['city'], radius)
lane_ids = copy.deepcopy(lane_ids)
lanes = dict()
for lane_id in lane_ids:
lane = self.am.city_lane_centerlines_dict[data['city']][lane_id]
lane = copy.deepcopy(lane)
centerline = np.matmul(data['rot'], (lane.centerline - data['orig'].reshape(-1, 2)).T).T
x, y = centerline[:, 0], centerline[:, 1]
if x.max() < x_min or x.min() > x_max or y.max() < y_min or y.min() > y_max:
continue
else:
"""Getting polygons requires original centerline"""
polygon = self.am.get_lane_segment_polygon(lane_id, data['city'])
polygon = copy.deepcopy(polygon)
lane.centerline = centerline
lane.polygon = np.matmul(data['rot'], (polygon[:, :2] - data['orig'].reshape(-1, 2)).T).T
lanes[lane_id] = lane
lane_ids = list(lanes.keys())
ctrs, feats, turn, control, intersect = [], [], [], [], []
for lane_id in lane_ids:
lane = lanes[lane_id]
ctrln = lane.centerline
num_segs = len(ctrln) - 1
ctrs.append(np.asarray((ctrln[:-1] + ctrln[1:]) / 2.0, np.float32))
feats.append(np.asarray(ctrln[1:] - ctrln[:-1], np.float32))
x = np.zeros((num_segs, 2), np.float32)
if lane.turn_direction == 'LEFT':
x[:, 0] = 1
elif lane.turn_direction == 'RIGHT':
x[:, 1] = 1
else:
pass
turn.append(x)
control.append(lane.has_traffic_control * np.ones(num_segs, np.float32))
intersect.append(lane.is_intersection * np.ones(num_segs, np.float32))
node_idcs = []
count = 0
for i, ctr in enumerate(ctrs):
node_idcs.append(range(count, count + len(ctr)))
count += len(ctr)
num_nodes = count
pre, suc = dict(), dict()
for key in ['u', 'v']:
pre[key], suc[key] = [], []
for i, lane_id in enumerate(lane_ids):
lane = lanes[lane_id]
idcs = node_idcs[i]
pre['u'] += idcs[1:]
pre['v'] += idcs[:-1]
if lane.predecessors is not None:
for nbr_id in lane.predecessors:
if nbr_id in lane_ids:
j = lane_ids.index(nbr_id)
pre['u'].append(idcs[0])
pre['v'].append(node_idcs[j][-1])
suc['u'] += idcs[:-1]
suc['v'] += idcs[1:]
if lane.successors is not None:
for nbr_id in lane.successors:
if nbr_id in lane_ids:
j = lane_ids.index(nbr_id)
suc['u'].append(idcs[-1])
suc['v'].append(node_idcs[j][0])
lane_idcs = []
for i, idcs in enumerate(node_idcs):
lane_idcs.append(i * np.ones(len(idcs), np.int64))
lane_idcs = np.concatenate(lane_idcs, 0)
pre_pairs, suc_pairs, left_pairs, right_pairs = [], [], [], []
for i, lane_id in enumerate(lane_ids):
lane = lanes[lane_id]
nbr_ids = lane.predecessors
if nbr_ids is not None:
for nbr_id in nbr_ids:
if nbr_id in lane_ids:
j = lane_ids.index(nbr_id)
pre_pairs.append([i, j])
nbr_ids = lane.successors
if nbr_ids is not None:
for nbr_id in nbr_ids:
if nbr_id in lane_ids:
j = lane_ids.index(nbr_id)
suc_pairs.append([i, j])
nbr_id = lane.l_neighbor_id
if nbr_id is not None:
if nbr_id in lane_ids:
j = lane_ids.index(nbr_id)
left_pairs.append([i, j])
nbr_id = lane.r_neighbor_id
if nbr_id is not None:
if nbr_id in lane_ids:
j = lane_ids.index(nbr_id)
right_pairs.append([i, j])
pre_pairs = np.asarray(pre_pairs, np.int64)
suc_pairs = np.asarray(suc_pairs, np.int64)
left_pairs = np.asarray(left_pairs, np.int64)
right_pairs = np.asarray(right_pairs, np.int64)
graph = dict()
graph['ctrs'] = np.concatenate(ctrs, 0)
graph['num_nodes'] = num_nodes
graph['feats'] = np.concatenate(feats, 0)
graph['turn'] = np.concatenate(turn, 0)
graph['control'] = np.concatenate(control, 0)
graph['intersect'] = np.concatenate(intersect, 0)
graph['pre'] = [pre]
graph['suc'] = [suc]
graph['lane_idcs'] = lane_idcs
graph['pre_pairs'] = pre_pairs
graph['suc_pairs'] = suc_pairs
graph['left_pairs'] = left_pairs
graph['right_pairs'] = right_pairs
for k1 in ['pre', 'suc']:
for k2 in ['u', 'v']:
graph[k1][0][k2] = np.asarray(graph[k1][0][k2], np.int64)
for key in ['pre', 'suc']:
if 'scales' in self.config and self.config['scales']:
#TODO: delete here
graph[key] += dilated_nbrs2(graph[key][0], graph['num_nodes'], self.config['scales'])
else:
graph[key] += dilated_nbrs(graph[key][0], graph['num_nodes'], self.config['num_scales'])
return graph
|