-
Notifications
You must be signed in to change notification settings - Fork 8
/
nuscenes_data.py
1317 lines (1080 loc) · 51 KB
/
nuscenes_data.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
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
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
"""
code adapted from https://github.com/nv-tlabs/lift-splat-shoot
and also https://github.com/wayveai/fiery/blob/master/fiery/data.py
"""
import os
import random
from functools import reduce
import cv2
import matplotlib.pyplot as plt
import numpy as np
import torch
import torchvision
from nuscenes.map_expansion.map_api import NuScenesMap
from nuscenes.nuscenes import NuScenes
from nuscenes.utils.data_classes import Box, PointCloud, RadarPointCloud
from nuscenes.utils.geometry_utils import transform_matrix
from nuscenes.utils.splits import create_splits_scenes
from PIL import Image
from pyquaternion import Quaternion
from scipy import ndimage
# distributed dataloader
from torch.utils.data.distributed import DistributedSampler
import utils.geom
import utils.py
import utils.vox
# custom eval split
from custom_nuscenes_splits import create_drn_eval_split_scenes
discard_invisible = False
def convert_egopose_to_matrix_numpy(egopose):
transformation_matrix = np.zeros((4, 4), dtype=np.float32)
rotation = Quaternion(egopose['rotation']).rotation_matrix
translation = np.array(egopose['translation'])
transformation_matrix[:3, :3] = rotation
transformation_matrix[:3, 3] = translation
transformation_matrix[3, 3] = 1.0
return transformation_matrix
def get_radar_data(nusc, sample_rec, nsweeps, min_distance, use_radar_filters, dataroot):
"""
Returns at most nsweeps of radar in the ego frame.
Adapted from https://github.com/nutonomy/nuscenes-devkit/blob/master/python-sdk/nuscenes/utils/data_classes.py#L56
"""
# import ipdb; ipdb.set_trace()
# points = np.zeros((5, 0))
points = np.zeros((19, 0)) # 18 plus one for time
# Get reference pose and timestamp.
ref_sd_token = sample_rec['data']['RADAR_FRONT']
ref_sd_rec = nusc.get('sample_data', ref_sd_token)
ref_pose_rec = nusc.get('ego_pose', ref_sd_rec['ego_pose_token'])
ref_cs_rec = nusc.get('calibrated_sensor', ref_sd_rec['calibrated_sensor_token'])
ref_time = 1e-6 * ref_sd_rec['timestamp']
# Homogeneous transformation matrix from global to _current_ ego car frame.
car_from_global = transform_matrix(ref_pose_rec['translation'], Quaternion(ref_pose_rec['rotation']), inverse=True)
if use_radar_filters:
RadarPointCloud.default_filters()
else:
RadarPointCloud.disable_filters()
# Aggregate current and previous sweeps.
# from all radars
radar_chan_list = ["RADAR_BACK_RIGHT", "RADAR_BACK_LEFT", "RADAR_FRONT", "RADAR_FRONT_LEFT", "RADAR_FRONT_RIGHT"]
for radar_name in radar_chan_list:
sample_data_token = sample_rec['data'][radar_name]
current_sd_rec = nusc.get('sample_data', sample_data_token)
for _ in range(nsweeps):
# Load up the pointcloud and remove points close to the sensor.
current_pc = RadarPointCloud.from_file(os.path.join(dataroot, current_sd_rec['filename']))
current_pc.remove_close(min_distance)
# Get past pose.
current_pose_rec = nusc.get('ego_pose', current_sd_rec['ego_pose_token'])
global_from_car = transform_matrix(current_pose_rec['translation'],
Quaternion(current_pose_rec['rotation']), inverse=False)
# Homogeneous transformation matrix from sensor coordinate frame to ego car frame.
current_cs_rec = nusc.get('calibrated_sensor', current_sd_rec['calibrated_sensor_token'])
car_from_current = transform_matrix(current_cs_rec['translation'], Quaternion(current_cs_rec['rotation']),
inverse=False)
# Fuse four transformation matrices into one and perform transform.
trans_matrix = reduce(np.dot, [car_from_global, global_from_car, car_from_current])
current_pc.transform(trans_matrix)
# Add time vector which can be used as a temporal feature.
time_lag = ref_time - 1e-6 * current_sd_rec['timestamp']
times = time_lag * np.ones((1, current_pc.nbr_points()))
new_points = np.concatenate((current_pc.points, times), 0)
points = np.concatenate((points, new_points), 1)
# print('time_lag', time_lag)
# print('new_points', new_points.shape)
# Abort if there are no previous sweeps.
if current_sd_rec['prev'] == '':
break
else:
current_sd_rec = nusc.get('sample_data', current_sd_rec['prev'])
return points
def ego_to_cam(points, rot, trans, intrins=None):
"""Transform points (3 x N) from ego frame into a pinhole camera
"""
points = points - trans.unsqueeze(1)
points = rot.permute(1, 0).matmul(points)
if intrins is not None:
points = intrins.matmul(points)
points[:2] /= points[2:3]
return points
def cam_to_ego(points, rot, trans, intrins):
"""Transform points (3 x N) from pinhole camera with depth
to the ego frame
"""
points = torch.cat((points[:2] * points[2:3], points[2:3]))
points = intrins.inverse().matmul(points)
points = rot.matmul(points)
points += trans.unsqueeze(1)
return points
def get_only_in_img_mask(pts, H, W):
"""pts should be 3 x N
"""
return (pts[2] > 0) & \
(pts[0] > 1) & (pts[0] < W - 1) & \
(pts[1] > 1) & (pts[1] < H - 1)
def get_rot(h):
return torch.Tensor([
[np.cos(h), np.sin(h)],
[-np.sin(h), np.cos(h)],
])
def img_transform(img, resize_dims, crop):
img = img.resize(resize_dims, Image.NEAREST)
img = img.crop(crop)
return img
def img_crop(img, resize_dims, crop):
img = img.crop(crop)
return img
class NormalizeInverse(torchvision.transforms.Normalize):
# https://discuss.pytorch.org/t/simple-way-to-inverse-transform-normalization/4821/8
def __init__(self, mean, std):
mean = torch.as_tensor(mean)
std = torch.as_tensor(std)
std_inv = 1 / (std + 1e-7)
mean_inv = -mean * std_inv
super().__init__(mean=mean_inv, std=std_inv)
def __call__(self, tensor):
return super().__call__(tensor.clone())
denormalize_img = torchvision.transforms.Compose((
NormalizeInverse(mean=[0.485, 0.456, 0.406],
std=[0.229, 0.224, 0.225]),
torchvision.transforms.ToPILImage(),
))
denormalize_img_torch = torchvision.transforms.Compose((
NormalizeInverse(mean=[0.485, 0.456, 0.406],
std=[0.229, 0.224, 0.225]),
))
normalize_img = torchvision.transforms.Compose((
torchvision.transforms.ToTensor(),
torchvision.transforms.Normalize(mean=[0.485, 0.456, 0.406],
std=[0.229, 0.224, 0.225]),
))
totorch_img = torchvision.transforms.Compose((
torchvision.transforms.ToTensor(),
))
normalize_img_torch = torchvision.transforms.Compose((
torchvision.transforms.Normalize(mean=[0.485, 0.456, 0.406],
std=[0.229, 0.224, 0.225]),
))
def gen_dx_bx(xbound, ybound, zbound):
dx = torch.Tensor([row[2] for row in [xbound, ybound, zbound]])
bx = torch.Tensor([row[0] + row[2] / 2.0 for row in [xbound, ybound, zbound]])
nx = torch.LongTensor([int((row[1] - row[0]) / row[2]) for row in [xbound, ybound, zbound]])
return dx, bx, nx
class SimpleLoss(torch.nn.Module):
def __init__(self, pos_weight):
super(SimpleLoss, self).__init__()
self.loss_fn = torch.nn.BCEWithLogitsLoss(pos_weight=torch.Tensor([pos_weight]))
def forward(self, ypred, ytgt):
loss = self.loss_fn(ypred, ytgt)
return loss
def add_ego(bx, dx):
# approximate rear axel
W = 1.85
pts = np.array([
[-4.084 / 2. + 0.5, W / 2.],
[4.084 / 2. + 0.5, W / 2.],
[4.084 / 2. + 0.5, -W / 2.],
[-4.084 / 2. + 0.5, -W / 2.],
])
pts = (pts - bx) / dx
pts[:, [0, 1]] = pts[:, [1, 0]]
plt.fill(pts[:, 0], pts[:, 1], '#76b900')
def add_ego2(bx, dx):
# approximate rear axel
W = 1.85
pts = np.array([
[-4.084 / 2. + 1, W / 2.],
[4.084 / 2. + 1, W / 2.],
[4.084 / 2. + 1, -W / 2.],
[-4.084 / 2. + 1, -W / 2.],
])
pts = (pts - bx) / dx
# pts[:, [0,1]] = pts[:, [1,0]]
plt.fill(pts[:, 0], pts[:, 1], '#76b900')
def get_nusc_maps(map_folder):
nusc_maps = {map_name: NuScenesMap(dataroot=map_folder,
map_name=map_name) for map_name in [
"singapore-hollandvillage",
"singapore-queenstown",
"boston-seaport",
"singapore-onenorth",
]}
return nusc_maps
def fetch_nusc_map2(rec, nusc_maps, nusc, scene2map, car_from_current):
egopose = nusc.get('ego_pose', nusc.get('sample_data', rec['data']['LIDAR_TOP'])['ego_pose_token'])
global_from_car = transform_matrix(egopose['translation'],
Quaternion(egopose['rotation']), inverse=False)
trans_matrix = reduce(np.dot, [global_from_car, car_from_current])
rot = np.arctan2(trans_matrix[1, 0], trans_matrix[0, 0])
center = np.array([trans_matrix[0, 3], trans_matrix[1, 3], np.cos(rot), np.sin(rot)])
map_name = scene2map[nusc.get('scene', rec['scene_token'])['name']]
# Debug only:
# print("Current map: ", map_name)
poly_names = ['drivable_area', 'carpark_area', 'ped_crossing', 'walkway', 'stop_line']
line_names = ['road_divider', 'lane_divider']
# lmap = get_local_map(nusc_maps[map_name], center, 50.0, poly_names, line_names)
# own labels handling
# use build in nuscenes function to get binary map mask
patch_box = (center[0], center[1], 100, 100) # patch of the map (center_x, center_y, height, width)
rot_angle = rot * 180 / np.pi # rotation of the patch -> should match the model output
# print for console debug
# print("Car rot angle: ", rot_angle)
patch_angle = rot_angle # angle in degrees
# patch_angle = 0.0 # for debugging only
canvas_size = (200, 200) # size of generated output mask -> 200x200
layer_names = poly_names + line_names
map_mask = nusc_maps[map_name].get_map_mask(patch_box=patch_box,
patch_angle=patch_angle,
layer_names=layer_names,
canvas_size=canvas_size)
""" Debug: test if mask is correct:
figsize = (12, 4)
fig, ax = nusc_maps[map_name].render_map_mask(patch_box=patch_box,
patch_angle=patch_angle,
layer_names=layer_names,
canvas_size=canvas_size,
figsize=figsize,
n_row=1)
"""
# generate bev view of the ego car for the current patch
ego_in_bev = np.zeros_like(map_mask[0])
W = 1.85
bx = -49.75
dx = 0.5
car_points = np.array([[-4.084 / 2. + 1, W / 2.],
[4.084 / 2. + 1, W / 2.],
[4.084 / 2. + 1, -W / 2.],
[-4.084 / 2. + 1, -W / 2.]])
car_points_scale_shift = (car_points - bx) / dx
round_car_approx = np.round(car_points_scale_shift)
x = abs((round_car_approx[0][1] + 1) - round_car_approx[-1][1])
y = abs(round_car_approx[0][0] - (round_car_approx[1][0] - 1))
car_mask = np.ones((int(y), int(x)))
ego_in_bev[int(round_car_approx[0][0]): int(round_car_approx[1][0] - 1),
int(round_car_approx[-1][1]): int(round_car_approx[0][1] + 1)] = car_mask
rot_angle = 0.0
ego_angle = np.arctan2(car_from_current[1, 0], car_from_current[0, 0])
ego_angle = ego_angle * 180 / np.pi
bev_plane_rot = ndimage.rotate(ego_in_bev, angle=ego_angle + 90.0, reshape=False)
egocar_bev = (bev_plane_rot > 0.1).astype(int)
return map_mask, egocar_bev
def get_local_map(nmap, center, stretch, layer_names, line_names):
# need to get the map here...
box_coords = (
center[0] - stretch,
center[1] - stretch,
center[0] + stretch,
center[1] + stretch,
)
polys = {}
# polygons
records_in_patch = nmap.get_records_in_patch(box_coords,
layer_names=layer_names,
mode='intersect')
for layer_name in layer_names:
polys[layer_name] = []
for token in records_in_patch[layer_name]:
poly_record = nmap.get(layer_name, token)
if layer_name == 'drivable_area':
polygon_tokens = poly_record['polygon_tokens']
else:
polygon_tokens = [poly_record['polygon_token']]
for polygon_token in polygon_tokens:
polygon = nmap.extract_polygon(polygon_token)
polys[layer_name].append(np.array(polygon.exterior.xy).T)
# lines
for layer_name in line_names:
polys[layer_name] = []
for record in getattr(nmap, layer_name):
token = record['token']
line = nmap.extract_line(record['line_token'])
if line.is_empty: # Skip lines without nodes
continue
xs, ys = line.xy
polys[layer_name].append(
np.array([xs, ys]).T
)
# convert to local coordinates in place
rot = get_rot(np.arctan2(center[3], center[2])).T
for layer_name in polys:
for rowi in range(len(polys[layer_name])):
polys[layer_name][rowi] -= center[:2]
polys[layer_name][rowi] = np.dot(polys[layer_name][rowi], rot)
return polys
# BEVCar colors
masks_colors = np.zeros((7, 3))
masks_colors[0] = (255. / 255., 192. / 255., 203. / 255.) # pink
masks_colors[1] = (255. / 255., 165. / 255., 0. / 255.) # orange
masks_colors[2] = (0. / 255., 200. / 255., 0. / 255.) # dimmer green
masks_colors[3] = (154. / 255., 230. / 255., 198. / 255.) # teal
masks_colors[4] = (255. / 255., 0. / 255., 0. / 255.) # red
masks_colors[5] = (255. / 255., 255. / 255., 0. / 255.) # yellow
masks_colors[6] = (128. / 255., 0. / 255., 128. / 255.) # purple
def get_rgba_map_from_mask2(map_masks, threshold=0.8, a=0.4):
"""
class 0: 'drivable_area' --- color in rbg: (1.00, 0.50, 0.31)\n
class 1: 'carpark_area' --- color '#FFD700' in rbg: (255./255., 215./255., 0./255)\n
class 2: 'ped_crossing' --- color '#069AF3' in rbg: (6./255., 154/255., 243/255.) \n
class 3: 'walkway' --- color '#FF00FF' in rbg: (255./255., 0./255., 255./255.) \n
class 4: 'stop_line' --- color '#FF0000' in rbg: (255./255., 0./255., 0./255.) \n
class 5: 'road_divider' --- color in rbg: (0.0, 0.0, 1.0)\n
class 6: 'lane_divider' --- color in rbg: (159./255., 0.0, 1.0)\n
threshold: defines at which probability the pixel belongs to a certain class
a: sets the opacity of the mask layers; if alpha=1.0 no overlapping visible; default: alpha=0.4
"""
masks_shape = map_masks.shape
rgba_image = np.ones((4, masks_shape[1], masks_shape[2]), dtype=np.float32)
for layer, (mask, color) in enumerate(zip(map_masks, masks_colors)):
alpha = a * (mask > threshold).astype(np.float32) # a = 0.4
# Assign RGB values based on the color
rgb = np.full((3, 200, 200), color[..., np.newaxis, np.newaxis])
# Combine the RGB and alpha channels
rgba = np.concatenate([rgb, np.expand_dims(alpha, axis=0)], axis=0)
# Update the RGBA image with the values of the current mask
rgba_image = rgba_image * (1 - alpha) + rgba * alpha
bev_map = rgba_image[:3]
if bev_map.max() > 1.0:
bev_map = np.clip(bev_map, 0.0, 1.0)
return torch.Tensor(bev_map)
def get_rgba_map_from_mask2_on_batch(map_masks: torch.Tensor, threshold: float = 0.8, a: float = 0.4) -> torch.Tensor:
"""
class 0: 'drivable_area' --- color in rbg: (1.00, 0.50, 0.31)\n
class 1: 'carpark_area' --- color '#FFD700' in rbg: (255./255., 215./255., 0./255)\n
class 2: 'ped_crossing' --- color '#069AF3' in rbg: (6./255., 154/255., 243/255.) \n
class 3: 'walkway' --- color '#FF00FF' in rbg: (255./255., 0./255., 255./255.) \n
class 4: 'stop_line' --- color '#FF0000' in rbg: (255./255., 0./255., 0./255.) \n
class 5: 'road_divider' --- color in rbg: (0.0, 0.0, 1.0)\n
class 6: 'lane_divider' --- color in rbg: (159./255., 0.0, 1.0)\n
threshold: defines at which probability the pixel belongs to a certain class
a: sets the opacity of the mask layers; if alpha=1.0 no overlapping visible; default: alpha=0.4
"""
B, classes, length_z, length_x = map_masks.shape # (B,7,200,200)
# (7, 200, 200)
bev_map = np.ones((B, 3, 200, 200)) # (B, 3, 200, 200)
rgba_image = np.ones((4, length_z, length_x), dtype=np.float32) # (B, 4, 200, 200)
for b in range(B):
for layer, (mask, color) in enumerate(zip(map_masks[b], masks_colors)):
alpha = a * (mask > threshold).astype(np.float32) # a = 0.4 # (200, 200)
# Assign RGB values based on the color
rgb = np.full((3, 200, 200), color[..., np.newaxis, np.newaxis]) # (3, 200, 200)
# Combine the RGB and alpha channels
rgba = np.concatenate([rgb, np.expand_dims(alpha, axis=0)], axis=0) # (4, 200, 200)
# Update the RGBA image with the values of the current mask
rgba_image = rgba_image * (1 - alpha) + rgba * alpha # (4, 200, 200)
bev_map[b] = rgba_image[:3]
# clip values
if bev_map.max() > 1.0:
bev_map = np.clip(bev_map, 0.0, 1.0)
return torch.Tensor(bev_map)
class NuscData(torch.utils.data.Dataset):
def __init__(self, nusc: NuScenes, nusc_maps: dict, is_train: bool, data_aug_conf: dict,
centroid: torch.Tensor = None, bounds: tuple = None, res_3d=None, nsweeps: int = 1, seqlen: int = 1,
refcam_id: int = 1, get_tids: bool = False, temporal_aug: bool = False,
use_radar_filters: bool = False, do_shuffle_cams: bool = True, radar_encoder_type: bool = None,
use_shallow_metadata: bool = True, use_pre_scaled_imgs: bool = True, custom_dataroot: str = None,
use_obj_layer_only_on_map: bool = False, vis_full_scenes: bool = False,
use_radar_occupancy_map: bool = False, do_drn_val_split: bool = False, get_val_day: bool = False,
get_val_rain: bool = False, get_val_night: bool = False, print_details: bool = False):
self.nusc = nusc
self.nusc_maps = nusc_maps
self.is_train = is_train
self.data_aug_conf = data_aug_conf
# self.grid_conf = grid_conf
self.nsweeps = nsweeps
self.use_radar_filters = use_radar_filters
self.do_shuffle_cams = do_shuffle_cams
self.res_3d = res_3d
self.bounds = bounds
self.centroid = centroid
self.seqlen = seqlen
self.refcam_id = refcam_id
self.vis_full_scenes = vis_full_scenes
self.use_radar_occupancy_map = use_radar_occupancy_map
self.do_drn_val_split = do_drn_val_split
self.get_val_day = get_val_day
self.get_val_rain = get_val_rain
self.get_val_night = get_val_night
self.print_details = print_details
self.dataroot = self.nusc.dataroot
self.scenes = self.get_scenes()
if self.do_drn_val_split and not self.is_train:
# returns samples in the correct "DAY/RAIN/NIGHT" order
self.ixes = self.get_ordered_drn_samples()
else:
self.ixes = self.prepro()
if temporal_aug:
self.indices = self.get_indices_tempaug()
else:
self.indices = self.get_indices()
self.get_tids = get_tids
self.radar_encoder_type = radar_encoder_type
self.use_shallow_metadata = use_shallow_metadata
self.use_pre_scaled_imgs = use_pre_scaled_imgs
self.custom_dataroot = custom_dataroot
self.use_obj_layer_only_on_map = use_obj_layer_only_on_map
XMIN, XMAX, YMIN, YMAX, ZMIN, ZMAX = self.bounds
Z, Y, X = self.res_3d
self.Z = Z
self.Y = Y
self.X = X
self.vox_util = utils.vox.Vox_util(
Z, Y, X,
scene_centroid=torch.from_numpy(self.centroid).float().cuda(),
bounds=self.bounds,
assert_cube=False)
grid_conf = { # note the downstream util uses a different XYZ ordering
'xbound': [XMIN, XMAX, (XMAX - XMIN) / float(X)],
'ybound': [ZMIN, ZMAX, (ZMAX - ZMIN) / float(Z)],
'zbound': [YMIN, YMAX, (YMAX - YMIN) / float(Y)],
}
dx, bx, nx = gen_dx_bx(grid_conf['xbound'], grid_conf['ybound'], grid_conf['zbound'])
self.dx, self.bx, self.nx = dx.numpy(), bx.numpy(), nx.numpy()
if self.print_details:
# print('ixes', self.ixes.shape)
print('indices', self.indices.shape)
print(self)
def get_scenes(self):
if self.do_drn_val_split and not self.is_train: # only for eval...
if self.get_val_day:
split = 'val_day'
elif self.get_val_rain:
split = 'val_rain'
elif self.get_val_night:
split = 'val_night'
else: # get all val data, but in: day, rain, night order
split = 'val_all'
scenes = create_drn_eval_split_scenes()[split]
else:
# filter by scene split
split = {
'v1.0-trainval': {True: 'train', False: 'val'},
'v1.0-mini': {True: 'mini_train', False: 'mini_val'},
'v1.0-test': {True: 'test', False: 'test'},
}[self.nusc.version][self.is_train]
scenes = create_splits_scenes()[split]
return scenes
def get_drn_scene_tokens(self):
# Create a mapping from scene names to scene tokens
scene_name_to_token = {scene['name']: scene['token'] for scene in self.nusc.scene}
# Retrieve scene tokens in the order of scene names in self.scenes
drn_scene_tokens = []
for scene_name in self.scenes:
if scene_name in scene_name_to_token:
drn_scene_tokens.append(scene_name_to_token[scene_name])
return drn_scene_tokens
def get_ordered_drn_samples(self):
"""
Returns:
all samples in the dataset sorted for scene ids
"""
# Create a mapping from scene names to scene tokens
scene_name_to_token = {scene['name']: scene['token'] for scene in self.nusc.scene}
drn_samples = []
for scene_name in self.scenes:
if scene_name in scene_name_to_token:
scene_token = scene_name_to_token[scene_name]
# Retrieve the first sample token from the scene
scene = self.nusc.get('scene', scene_token)
first_sample_token = scene['first_sample_token']
# Start with the first sample
current_sample_token = first_sample_token
while current_sample_token:
# Retrieve the current sample
sample = self.nusc.get('sample', current_sample_token)
drn_samples.append(sample)
# Move to the next sample in the scene
current_sample_token = sample['next']
return drn_samples
def prepro(self):
samples = [samp for samp in self.nusc.sample]
# remove samples that aren't in this split
samples = [samp for samp in samples if self.nusc.get('scene', samp['scene_token'])['name'] in self.scenes]
return samples
def get_indices(self):
"""
Note: if used for sequence collection -> e.g. get 40 samples of a scene for visualization
the following will happen: scenes with under (<)40 samples will be skipped and scenes with
over (>)40 samples will occur essentially be stored multiple times
this behaviour does not affect single sample sequences, used in training and eval.
For scene - based visualization, please set self.vis_full_scenes to True.
"""
indices = []
if self.vis_full_scenes:
# --> visualize the whole scene and avoid skipped / doubled scenes --> set variable
valid = True
index = 0
while index < len(self.ixes): # as long as we are not over every sample in the current split...
# start with first index, go as long as "valid"
current_indices = []
while valid:
rec = self.ixes[index]
current_indices.append(index)
# check if next sample is in the current scene --> if not -> close scene here
if rec['next'] == '':
valid = False
index += 1
# add length of valid indices
current_indices.insert(0, len(current_indices))
# check if we need to fill up to 42 values
while len(current_indices) != 42:
current_indices.append(0)
indices.append(current_indices)
valid = True
else: # single sample or incomplete scenes
for index in range(len(self.ixes)):
is_valid_data = True
previous_rec = None
current_indices = []
for t in range(self.seqlen):
index_t = index + t
# Going over the dataset size limit.
if index_t >= len(self.ixes):
is_valid_data = False
break
rec = self.ixes[index_t]
# Check if scene is the same
if (previous_rec is not None) and (rec['scene_token'] != previous_rec['scene_token']):
is_valid_data = False
break
current_indices.append(index_t)
previous_rec = rec
if is_valid_data:
index += 38
indices.append(current_indices)
return np.asarray(indices)
def get_indices_tempaug(self):
indices = []
t_patterns = None
if self.seqlen == 1:
return self.get_indices()
elif self.seqlen == 2:
# seq options: (t, t+1), (t, t+2)
t_patterns = [[0, 1], [0, 2]]
elif self.seqlen == 3:
# seq options: (t, t+1, t+2), (t, t+1, t+3), (t, t+2, t+3)
t_patterns = [[0, 1, 2], [0, 1, 3], [0, 2, 3]]
elif self.seqlen == 5:
t_patterns = [
[0, 1, 2, 3, 4], # normal
[0, 1, 2, 3, 5], [0, 1, 2, 4, 5], [0, 1, 3, 4, 5], [0, 2, 3, 4, 5], # 1 skip
# [1,0,2,3,4], [0,2,1,3,4], [0,1,3,2,4], [0,1,2,4,3], # 1 reverse
]
else:
raise NotImplementedError("timestep not implemented")
for index in range(len(self.ixes)):
for t_pattern in t_patterns:
is_valid_data = True
previous_rec = None
current_indices = []
for t in t_pattern:
index_t = index + t
# going over the dataset size limit
if index_t >= len(self.ixes):
is_valid_data = False
break
rec = self.ixes[index_t]
# check if scene is the same
if (previous_rec is not None) and (rec['scene_token'] != previous_rec['scene_token']):
is_valid_data = False
break
current_indices.append(index_t)
previous_rec = rec
if is_valid_data:
indices.append(current_indices)
# indices.append(list(reversed(current_indices)))
# indices += list(itertools.permutations(current_indices))
return np.asarray(indices)
def sample_augmentation(self) -> tuple[tuple[int, int], tuple[int, int, int, int]]:
"""
Applies augmentation on inout image data by resizing and cropping
Returns:
tuple[tuple,tuple]: resize_dims, crop
"""
fH, fW = self.data_aug_conf['final_dim']
if self.is_train:
if 'resize_lim' in self.data_aug_conf and self.data_aug_conf['resize_lim'] is not None:
resize = np.random.uniform(*self.data_aug_conf['resize_lim'])
else:
resize = self.data_aug_conf['resize_scale']
resize_dims = (int(fW * resize), int(fH * resize))
newW, newH = resize_dims
# center it
crop_h = int((newH - fH) / 2)
crop_w = int((newW - fW) / 2)
crop_offset = self.data_aug_conf['crop_offset']
crop_w = crop_w + int(np.random.uniform(-crop_offset, crop_offset))
crop_h = crop_h + int(np.random.uniform(-crop_offset, crop_offset))
crop = (crop_w, crop_h, crop_w + fW, crop_h + fH)
else: # validation/test
# do a perfect resize
resize_dims = (fW, fH)
crop_h = 0
crop_w = 0
crop = (crop_w, crop_h, crop_w + fW, crop_h + fH)
return resize_dims, crop
def get_image_data(self, rec, cams):
imgs = []
rots = []
trans = []
intrins = []
for cam in cams:
resize_dims, crop = self.sample_augmentation()
samp = self.nusc.get('sample_data', rec['data'][cam])
if not self.use_pre_scaled_imgs:
imgname = os.path.join(self.dataroot, samp['filename'])
img = Image.open(imgname)
W, H = img.size
else:
custom_path = os.path.join(self.custom_dataroot, '#CUSTOM_RES#') # TODO: adapt to rescaled imgs folder
imgname = os.path.join(custom_path, samp['filename'])
img = Image.open(imgname)
W, H = (1600, 900) # img.size must be fixed to keep the rest of the code working
sens = self.nusc.get('calibrated_sensor', samp['calibrated_sensor_token'])
intrin = torch.Tensor(sens['camera_intrinsic'])
rot = torch.Tensor(Quaternion(sens['rotation']).rotation_matrix)
tran = torch.Tensor(sens['translation'])
# W and H: original image size to scale intrinsics correctly
sx = resize_dims[0] / float(W)
sy = resize_dims[1] / float(H)
intrin = utils.geom.scale_intrinsics(intrin.unsqueeze(0), sx, sy).squeeze(0)
fx, fy, x0, y0 = utils.geom.split_intrinsics(intrin.unsqueeze(0))
new_x0 = x0 - crop[0]
new_y0 = y0 - crop[1]
pix_T_cam = utils.geom.merge_intrinsics(fx, fy, new_x0, new_y0)
intrin = pix_T_cam.squeeze(0)
img = img_transform(img, resize_dims, crop)
imgs.append(totorch_img(img))
intrins.append(intrin)
rots.append(rot)
trans.append(tran)
return torch.stack(imgs), torch.stack(rots), torch.stack(trans), torch.stack(intrins)
def get_radar_data(self, rec, nsweeps):
pts = get_radar_data(self.nusc, rec, nsweeps=nsweeps, min_distance=2.2,
use_radar_filters=self.use_radar_filters, dataroot=self.dataroot)
return torch.Tensor(pts)
def get_binimg(self, rec):
egopose = self.nusc.get('ego_pose', self.nusc.get('sample_data', rec['data']['LIDAR_TOP'])['ego_pose_token'])
trans = -np.array(egopose['translation'])
rot = Quaternion(egopose['rotation']).inverse
img = np.zeros((self.nx[0], self.nx[1]))
for ii, tok in enumerate(rec['anns']):
inst = self.nusc.get('sample_annotation', tok)
# NuScenes filter
if 'vehicle' not in inst['category_name']:
continue
if discard_invisible and int(inst['visibility_token']) == 1:
# filter invisible vehicles
continue
box = Box(inst['translation'], inst['size'], Quaternion(inst['rotation']))
box.translate(trans)
box.rotate(rot)
pts = box.bottom_corners()[:2].T
pts = np.round(
(pts - self.bx[:2] + self.dx[:2] / 2.) / self.dx[:2]
).astype(np.int32)
pts[:, [1, 0]] = pts[:, [0, 1]]
# noinspection PyUnresolvedReferences
cv2.fillPoly(img, [pts], ii + 1.0)
return torch.Tensor(img).unsqueeze(0), torch.Tensor(convert_egopose_to_matrix_numpy(egopose))
def get_seg_bev(self, lrtlist_cam, vislist):
B, N, D = lrtlist_cam.shape
assert (B == 1)
seg = np.zeros((self.Z, self.X))
val = np.ones((self.Z, self.X))
corners_cam = utils.geom.get_xyzlist_from_lrtlist(lrtlist_cam) # B, N, 8, 3
y_cam = corners_cam[:, :, :, 1] # y part; B, N, 8
corners_mem = self.vox_util.Ref2Mem(corners_cam.reshape(B, N * 8, 3), self.Z, self.Y, self.X).reshape(B, N, 8,
3)
# take the xz part
corners_mem = torch.stack([corners_mem[:, :, :, 0], corners_mem[:, :, :, 2]], dim=3) # B, N, 8, 2
# corners_mem = corners_mem[:,:,:4] # take the bottom four
for n in range(N):
_, inds = torch.topk(y_cam[0, n], 4, largest=False) # returns indices of the 4 smallest values
pts = corners_mem[0, n, inds].numpy().astype(np.int32) # 4, 2
# if this messes in some later conditions,
# the solution is to draw all combos
pts = np.stack([pts[0], pts[1], pts[3], pts[2]])
# pts[:, [1, 0]] = pts[:, [0, 1]]
# noinspection PyUnresolvedReferences
cv2.fillPoly(seg, [pts], n + 1.0)
if vislist[n] == 0:
# draw a black rectangle if it's invisible
# noinspection PyUnresolvedReferences
cv2.fillPoly(val, [pts], 0.0)
return torch.Tensor(seg).unsqueeze(0), torch.Tensor(val).unsqueeze(0) # 1, Z, X
def get_lrtlist(self, rec):
egopose = self.nusc.get('ego_pose', self.nusc.get('sample_data', rec['data']['LIDAR_TOP'])['ego_pose_token'])
trans = -np.array(egopose['translation'])
rot = Quaternion(egopose['rotation']).inverse
lrtlist = []
boxlist = []
vislist = []
tidlist = []
for tok in rec['anns']:
inst = self.nusc.get('sample_annotation', tok)
# NuScenes filter
if 'vehicle' not in inst['category_name']:
continue
if int(inst['visibility_token']) == 1:
vislist.append(torch.tensor(0.0)) # invisible
else:
vislist.append(torch.tensor(1.0)) # visible
box = Box(inst['translation'], inst['size'], Quaternion(inst['rotation']))
box.translate(trans)
box.rotate(rot)
tidlist.append(inst['instance_token'])
# print('rotation', inst['rotation'])
r = box.rotation_matrix
t = box.center
l = box.wlh
l = np.stack([l[1], l[0], l[2]])
lrt = utils.py.merge_lrt(l, utils.py.merge_rt(r, t))
lrt = torch.Tensor(lrt)
lrtlist.append(lrt)
ry, _, _ = Quaternion(inst['rotation']).yaw_pitch_roll
rs = np.stack([ry * 0, ry, ry * 0])
box_ = torch.from_numpy(np.stack([t, l, rs])).reshape(9)
boxlist.append(box_)
if len(lrtlist):
lrtlist = torch.stack(lrtlist, dim=0)
boxlist = torch.stack(boxlist, dim=0)
vislist = torch.stack(vislist, dim=0)
else:
lrtlist = torch.zeros((0, 19))
boxlist = torch.zeros((0, 9))
vislist = torch.zeros(0)
tidlist = []
return lrtlist, boxlist, vislist, tidlist
def choose_cams(self):
if self.is_train and self.data_aug_conf['ncams'] < len(self.data_aug_conf['cams']):
cams = np.random.choice(self.data_aug_conf['cams'], self.data_aug_conf['ncams'],
replace=False)
else:
cams = self.data_aug_conf['cams']
return cams
def __str__(self):
return f"""NuscData: {len(self)} samples. Split: {"train" if self.is_train else "val"}.
Augmentation Conf: {self.data_aug_conf}"""
def __len__(self):
return len(self.indices)
class VizData(NuscData):
def __init__(self, *args, **kwargs):
super(VizData, self).__init__(*args, **kwargs)
Z, Y, X = self.res_3d
self.vox_util = utils.vox.Vox_util(
Z, Y, X,
scene_centroid=torch.from_numpy(self.centroid).float().cuda(),
bounds=self.bounds,
assert_cube=False)
self.Z, self.Y, self.X = Z, Y, X
def get_single_item(self, index, cams, refcam_id: int = None):
rec = self.ixes[index]
imgs, rots, trans, intrins = self.get_image_data(rec, cams)
binimg, egopose = self.get_binimg(rec)
if refcam_id is None:
if self.is_train:
# randomly sample the ref cam
refcam_id = np.random.randint(1, len(cams))
else:
refcam_id = self.refcam_id
# move the target refcam_id to the zeroth slot
img_ref = imgs[refcam_id].clone()
img_0 = imgs[0].clone()
imgs[0] = img_ref
imgs[refcam_id] = img_0
rot_ref = rots[refcam_id].clone()
rot_0 = rots[0].clone()
rots[0] = rot_ref
rots[refcam_id] = rot_0
tran_ref = trans[refcam_id].clone()
tran_0 = trans[0].clone()
trans[0] = tran_ref
trans[refcam_id] = tran_0
intrin_ref = intrins[refcam_id].clone()
intrin_0 = intrins[0].clone()
intrins[0] = intrin_ref
intrins[refcam_id] = intrin_0
# get map for current scene
scene2map = {}
scene_token = rec['scene_token']
scene = self.nusc.get('scene', scene_token)
log = self.nusc.get('log', scene['log_token'])
scene2map[scene['name']] = log['location']
car_from_current = np.eye(4)
car_from_current[:3, :3] = rots[0].numpy()
car_from_current[:3, 3] = np.transpose(trans[0].numpy())