-
Notifications
You must be signed in to change notification settings - Fork 34
/
Copy pathscenes.py
1245 lines (1078 loc) · 57 KB
/
scenes.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
"""
@file scenes.py
@author Jianfei Guo, Shanghai AI Lab
@brief Basic scene node structure
"""
__all__ = [
'Scene',
'namedtuple_ind_id_obj',
'namedtuple_observer_infos'
]
import os
import pickle
import functools
import numpy as np
from numbers import Number
from typing import Callable, Literal, Union, List, Dict, NamedTuple, Tuple, Type
import torch
import torch.nn as nn
from nr3d_lib.models.attributes import *
from nr3d_lib.utils import IDListedDict, get_shape, import_str, check_to_torch
from nr3d_lib.models.accelerations.occgrid_accel import OccGridAccel
from app.resources import SceneNode
from app.resources.observers import OBSERVER_CLASS_NAMES, OBSERVER_TYPE, Camera, Lidar, RaysLidar
# namedtuple_ind_id_obj = namedtuple("namedtuple_ind_id_obj", "inds ids objs")
# namedtuple_ind_id_obj = NamedTuple("namedtuple_ind_id_obj", [('inds', List[int]), ('ids', List[str]), ('objs', List[SceneNode])])
class namedtuple_ind_id_obj(NamedTuple):
inds: List[int]
ids: List[str]
objs: List[SceneNode]
class namedtuple_observer_infos(NamedTuple):
tracks: torch.Tensor
all_tracks: torch.Tensor
all_frustum_pts: torch.Tensor
class Scene(object):
def __init__(self, unique_id: str='WORLD', device=None, dtype=torch.float) -> None:
self.device = device
self.dtype = dtype
self.id = unique_id
#-------- Init scene graph basic structures
self._init_graph()
#-------- For scene with loaded sequence/log data
"""
NOTE: Switching between timestamp interpolation mode or frame index slicing mode.
When `use_ts_interp` is True: \
using continuous timestamp to interpolate nodes' data.
Can NOT call slice_at(). \
i.e. Can not use frame index since different nodes could have in-consistent keyframes.
Each node can have different frame definition and frame length. \
This allows nodes having denser or sparser data compared to others. \
In this case, can use `global_fi` and `global_ts` to mark the corresponding \
global frame indices and timestamps of each *valid* frame of the node's local data.\
For example, for a scene with 200 frame length, the `ego_car` node can have frame length = 400, \
which means the data of `ego_car` is 2x denser compared to other nodes' data. \
The `global_fi` of `ego_car` could be `0,0,1,1,2,2,...`, \
which means for each global frame, there are two frames of local data for `ego_car`.
When `use_ts_interp` is False: \
using consecutive frame index to slice nodes' data.
Can call interp_at(). \
i.e. Can use timestamps to interplate nodes' data if each `node.frame_global_ts` is provided.
For both cases, the validness of the node's local data is stored in `node.subattr.frame_valid_flags` \
Note:
`node.i_valid_flags` is the sliced/interpolated result. \
`node.frame_valid_flags` has the overall value, un-affected by slice_at()/interp_at().
This value is determined by the segment data's 'start_frame' and 'n_frames' when loading.\
Find the variable `all_seg_local_fi` in `nodes.py` for more details.
"""
self.use_ts_interp: bool = False
# The frame ind(s) sliced at / the timestamp interpolated at
self.i = None
# Whether the `self.i` represents timestamps. If False, `i` represents the frame indices.
self.i_is_timestamp = None
# self.i_is_single = None # See below
# Total data frames of this scene (if this scene has a `dataset`)
self.n_frames: int = 0
# The offset between scene's frames and dataset's frames (if this scene has a `dataset`)
self.data_frame_offset: int = 0
# The offset between scene's timestamps and dataset's timestamps \
# (only if this scene has a `dataset` and `dataset` has timestamp definitions)
self.data_timestamp_offset: float = 0
# Convert between raw timestamps and [-1,1] timestamps
self.data_ts_offset: float = 0.0
self.data_ts_scale: float = 1.0
# (Optional) Universal timestamp of each frame for this scene.
# Timestamps across different nodes of one single scene is universally defined. \
# (Might have data at different time, but only one unique sacred timeline)
self.frame_global_ts: torch.Tensor = None
# # Not needed. Always equals to torch.arange(self.n_frames)
# self.frame_global_fi: torch.LongTensor = None
#-------- Scene meta data
self.metas: dict = {}
#-------- Optional scene-level misc models
# app/models_misc/learnable_params.py; Scene parameter (pose, intr, extr) refinement
self.learnable_params: nn.Module = None
# app/models_misc/image_embeddings.py; Per-frame image embeddings, as in NeRF in the wild
self.image_embeddings: nn.Module = None
#-------- For code_single, the single object that we focus on.
# Load from scenario; different tasks could have different settings.
# e.g. "Street" for street views, "Main" for other
self.main_class_name = 'Main'
#-------- The overall asset_bank that contains models of this scene
self.asset_bank = None
def _init_graph(self):
#-------- Scene-Graph basic structures
self.root = SceneNode(self.id, scene=self, device=self.device)
# self.root.transform = None
# self.root.world_transform = None
self.drawables: IDListedDict[SceneNode] = IDListedDict()
self.drawable_groups_by_class_name: Dict[str, IDListedDict[SceneNode]] = {}
self.drawable_groups_by_model_id: Dict[str, IDListedDict[SceneNode]] = {}
self.observers: IDListedDict[OBSERVER_TYPE] = IDListedDict()
self.observer_groups_by_class_name: Dict[str, IDListedDict[OBSERVER_TYPE]] = {}
self.all_nodes: IDListedDict[SceneNode] = IDListedDict([self.root])
self.all_nodes_by_class_name: Dict[str, IDListedDict[SceneNode]] = {self.root.class_name: IDListedDict([self.root])}
#------------------------------------------------------------------
#---------------------- Loading, updating, reading
@property
def slice_prefix(self):
# The tensor shape of currently sliced frame indice(s) / interpolated timestamp(s)
return get_shape(self.i)
@property
def i_is_single(self):
# Whether frozen at single frame index / timestamp
return len(self.slice_prefix) == 0
def __len__(self):
return self.n_frames
def _make_default_timestamps(self, n_frames: int = None, device=None):
n_frames = n_frames or self.n_frames
ts = torch.linspace(-1, 1, n_frames, dtype=torch.float, device=self.device)
return ts
"""
slice_at functions
"""
# @profile
def slice_at(self, i: Union[int, torch.LongTensor]):
""" Frozen at a single time frame or multiple time frames, and update the scene graph.
This will first retrieve data at the given frame for each nodes' attributes,
and then update the scene graph from the root to leaves,
in which the `world_transform` of each node will also be calculated from root to leaf.
Args:
i (Union[int, torch.LongTensor]): The frame indice(s) to freeze the scene at
"""
if self.use_ts_interp:
raise RuntimeError(f"You should not call `slice_at()`. Use timestamp interpolation `interp_at()` instead.")
self.i = i
self.i_is_timestamp = False
# self.root.slice_at(i)
# NOTE: Frozen each node's attr, without updating the scene graph.
for n in self.all_nodes:
n._slice_at(i)
# NOTE: Update the scene graph, from root to leaf.
# The `world_transform` of each node will also be calculated from root to leaf.
self.root.update()
def slice_at_full(self):
"""
Frozen at full indices, from start to end
"""
self.slice_at(torch.arange(len(self), device=self.device, dtype=torch.long))
"""
interp_at functions
"""
def interp_at(self, ts: Union[Number, torch.FloatTensor]):
"""
Similar to `_slice_at`, but performs interpolation at given novel timestamps `ts` \
among valid keyframe timestamps (`self.frame_global_ts`).
Args:
ts (Union[Number, torch.FloatTensor]): Timestamps at which to perform interpolation.
"""
ts = check_to_torch(ts, dtype=torch.float, device=self.device)
self.i = ts # TODO: Check BUGs
self.i_is_timestamp = True
# NOTE: Interpolate each node's attr, without updating the scene graph.
# Different nodes might have different `global_ts`, and might be different from `scene.frame_global_ts`
for n in self.all_nodes:
n._interp_at(ts)
# NOTE: Update the scene graph, from root to leaf.
# The `world_transform` of each node will also be calculated from root to leaf.
self.root.update()
def interp_at_full(self, node_id: str = None):
if node_id is not None:
all_ts = self.all_nodes[node_id].frame_global_ts
else:
all_ts = self.frame_global_ts
self.interp_at(all_ts)
"""
frozen_at_xxx_frame functions: Support auto decide between interp_at and slice_at.
- if self.use_ts_interp: freeze the scene using timestamp interpolating.
- if not self.use_ts_interp: freeze the scene using frame indices slicing.
"""
def frozen_at_node_frame(self, node_id: str, i: Union[int, torch.LongTensor]):
if self.use_ts_interp:
ts = self.all_nodes[node_id].frame_global_ts[i]
self.interp_at(ts)
else:
self.slice_at(i)
def frozen_at_full_node_frame(self, node_id: str):
if self.use_ts_interp:
self.interp_at_full(node_id)
else:
self.slice_at_full()
def frozen_at_global_frame(self, i: Union[int, torch.LongTensor]):
if self.use_ts_interp:
ts = self.frame_global_ts[i]
self.interp_at(ts)
else:
self.slice_at(i)
def frozen_at_full_global_frame(self):
if self.use_ts_interp:
self.interp_at_full()
else:
self.slice_at_full()
def unfrozen(self):
"""
Un-freeze a scene; i.e. resets all its nodes to default in-active state
"""
self.reset_attr()
def reset_attr(self):
"""
Resets all self nodes to default in-active state
"""
# Reset frozen status
del self.i
self.i = None
for n in self.all_nodes:
n.reset_attr()
def clear_graph(self):
"""
Complemenly cleans this scene graph and makes self a empty scene
"""
del self.root
del self.drawables, self.drawable_groups_by_class_name, self.drawable_groups_by_model_id
del self.observers, self.observer_groups_by_class_name
del self.all_nodes, self.all_nodes_by_class_name
self._init_graph()
def _replicate_for_parallel(self, device) -> 'Scene':
"""
Make one shallow copy of the current scene and its nodes
"""
replica = self.__new__(type(self))
replica.__dict__ = self.__dict__.copy()
replica.device = device
if self.frame_global_ts is not None:
replica.frame_global_ts = self.frame_global_ts.clone().to(device)
if self.i is not None and isinstance(self.i, torch.Tensor):
replica.i = self.i.clone().to(device)
#---- Replicate all the node objects
# NOTE: Similar to pytorch, their Attr object in the new replicated nodes' frame_data are still handles from the old object,
# Which will only be replaced in replicate_scene() after the nodes' Attr data got "broadcast_coalesced_reshape"-ed
replica.all_nodes = IDListedDict([o._replicate_for_parallel(device) for o in replica.all_nodes])
#---- Replicate the scene graph by replacing old nodes' handles with the new replicated nodes' handles from root to leaves
def recursive_update_node_handle(o: SceneNode):
if len(o.children) > 0:
o.children = replica.all_nodes[list(o.children.keys())]
for child in o.children:
child.parent = o
recursive_update_node_handle(child)
replica.root = replica.all_nodes[replica.root.id]
recursive_update_node_handle(replica.root)
#---- Replicate all the other node handle storage dicts
if len(replica.drawables.keys()) > 0:
replica.drawables = replica.all_nodes[list(replica.drawables.keys())]
if len(replica.observers.keys()) > 0:
replica.observers = replica.all_nodes[list(replica.observers.keys())]
replica.drawable_groups_by_class_name = {k: replica.all_nodes[list(v.keys())] for k, v in replica.drawable_groups_by_class_name.items()}
replica.drawable_groups_by_model_id = {k: replica.all_nodes[list(v.keys())] for k, v in replica.drawable_groups_by_model_id.items()}
replica.observer_groups_by_class_name = {k: replica.all_nodes[list(v.keys())] for k, v in replica.observer_groups_by_class_name.items()}
replica.all_nodes_by_class_name = {k: replica.all_nodes[list(v.keys())] for k, v in replica.all_nodes_by_class_name.items()}
return replica
def load_from_scenario(self, scenario: dict, device=None):
"""
Load one scene from the given scenario description dict.
NOTE: DO NOT let any useful node to be not-used parent's child,
because all children and descendants of a not-used node will be ignored.
TODO: Consider making this a classmethod.
Args:
scenario (dict): The given scenario description dict.
device (torch.device, optional): The target torch.device to store this scene's nodes' attributes.
Defaults to None.
"""
device = device or self.device
#---- Load scene meta
# NOTE: Scene's id is changed when loading a scenario
self.id: str = scenario['scene_id']
self.n_frames: int = scenario['metas']['n_frames']
self.data_frame_offset: int = scenario['metas'].get('data_frame_offset', 0)
self.data_ts_offset: float = scenario['metas'].get('data_timestamp_offset', 0.0)
self.data_ts_scale: float = scenario['metas'].get('data_timestamp_scale', 1.0)
self.use_ts_interp: bool = scenario['metas'].get('use_ts_interp', False)
if 'frame_timestamps' in scenario['metas']:
global_ts = scenario['metas']['frame_timestamps']
global_ts = check_to_torch(global_ts, dtype=torch.float, device=device)
else:
global_ts = self._make_default_timestamps(self.n_frames, device=device)
self.frame_global_ts = global_ts
self.metas: dict = scenario['metas']
# NOTE: Pass the `default_global_ts` to the root node, \
# Then for all remaning descendants, use `parent.frame_global_ts` instead.
self.root.fill_default_data(self.n_frames, default_global_ts=self.frame_global_ts, device=device)
# NOTE: For code_single, the single object that we focus on.
# e.g. "Street" for street views, "Obj" for single-object reconstruction, "Room" for in-door tasks
self.main_class_name = scenario['metas'].get('main_class_name', None)
#---- Process objects
def load_objects(oid: str, odict: dict, parent:SceneNode=self.root):
"""
Recursively load
"""
o = SceneNode(oid, class_name=odict['class_name'], scene=self, device=device)
# Load object attr data
o.load_from_odict(self.n_frames, odict, default_global_ts=parent.frame_global_ts, device=device)
# Add node to scene_graph
self.add_node(o, parent=parent)
# Recursively load node's childrens
if 'children' in odict:
for cid, cdict in odict['children'].items():
load_objects(cid, cdict, parent=o)
for oid, odict in scenario['objects'].items():
load_objects(oid, odict, parent=self.root)
#---- Process observers
def load_observers(oid: str, odict: dict, parent:SceneNode=self.root):
"""
Recursively load
"""
if odict['class_name'] == 'Camera':
o = Camera(oid, scene=self, device=device)
elif odict['class_name'] == 'Lidar':
o = Lidar(oid, scene=self, device=device)
elif odict['class_name'] == 'RaysLidar':
o = RaysLidar(oid, scene=self, device=device)
else:
o = SceneNode(oid, class_name=odict['class_name'], scene=self, device=device)
# Load observer attr data
o.load_from_odict(self.n_frames, odict, default_global_ts=parent.frame_global_ts, device=device)
# Add node to scene_graph
self.add_node(o, parent=parent)
# Recursively load node's childrens
if 'children' in odict:
for cid, cdict in odict['children'].items():
load_observers(cid, cdict, parent=o)
for oid, odict in scenario['observers'].items():
load_observers(oid, odict, parent=self.root)
# NOTE: Check which node uses the default timestamps
# for o in self.all_nodes:
# print(o.id, torch.equal(o.frame_global_ts, self.frame_global_ts))
def load_from_scenario_file(self, scenario_file: str, device=None):
"""
Load one scene from the given path string to the scenario description dict (pickle).
TODO: Consider making this a classmethod.
Args:
scenario_file (str): The given path string to the scenario description dict (pickle).
device (torch.device, optional): The target torch.device to store this scene's nodes' attributes.
Defaults to None.
"""
self.clear_graph()
if not os.path.exists(scenario_file):
raise RuntimeError(f"Not exist: {scenario_file}")
with open(scenario_file, 'rb') as f:
scenario = pickle.load(f)
self.load_from_scenario(scenario, device=device)
def load_from_nodes(self, nodes: List[SceneNode]):
"""
Load one scene from the given list of SceneNode
TODO: Consider making this a classmethod.
"""
self.clear_graph()
for o in nodes:
self.add_node(o)
def load_assets(self, asset_bank):
"""
Load the corresponding asset models for each node in the this scene from the asset bank.
"""
from app.models.asset_base import AssetModelMixin # To avoid circular import
self.asset_bank = asset_bank
#---- Load object-level models
for class_name, obj_group in self.all_nodes_by_class_name.items():
# NOTE: The same functionality with asset_bank.asset_compute_id
if (cfg:=asset_bank.class_name_configs.get(class_name, None)) is not None:
model_class: Type[AssetModelMixin] = import_str(cfg.model_class)
for o in obj_group:
model_id = model_class.asset_compute_id(scene=self, obj=o, class_name=class_name)
o.model = asset_bank[model_id]
self.add_node_to_drawable(o)
#---- Load scene-level models
# LearnableParams, for pose-refinement and intrinsic's self-calibration, etc.
class_name = 'LearnableParams'
if (cfg := asset_bank.class_name_configs.get(class_name, None)) is not None:
model_class: Type[AssetModelMixin] = import_str(cfg.model_class)
model_id = model_class.asset_compute_id(scene=self, obj=None, class_name=class_name)
model = asset_bank[model_id]
# NOTE: !!! Important !!! Models from asset_bank might be initialized with other scene(s)
model.scene = self
if model.is_enabled:
# NOTE: Make sure the learnable params are correctly loaded into current scene's nodes' frame_data
# This is essential when replicating scenes for render_parallel
model.enable(self)
self.learnable_params = model
# Per-frame image appearance embeddings
class_name = 'ImageEmbeddings'
if (cfg := asset_bank.class_name_configs.get(class_name, None)) is not None:
model_class: Type[AssetModelMixin] = import_str(cfg.model_class)
model_id = model_class.asset_compute_id(scene=self, obj=None, class_name=class_name)
model = asset_bank[model_id]
# NOTE: !!! Important !!! Models from asset_bank might be initialized with other scene(s)
model.scene = self
self.image_embeddings = model
#------------------------------------------------------------------
#---------------------- Node management
def add_node(self, node: SceneNode, parent: SceneNode=None):
"""
Add one node to this scene.
If the node has non-empty `.model`, it will be added to the drawable list.
If the node has a observer class_name, it will be added to the observer list.
Args:
node (SceneNode): The node to add.
parent (SceneNode, optional): Optionally specify a parent for the given node. Defaults to None.
"""
node.scene = self
# self.all_nodes[node.id] = node
self.all_nodes.append(node)
if parent is None: parent = self.root
parent.add_child(node)
class_name = node.class_name
if class_name not in self.all_nodes_by_class_name.keys():
self.all_nodes_by_class_name[class_name] = IDListedDict()
self.all_nodes_by_class_name[class_name][node.id] = node
# Timestamps
if node.frame_global_ts is None and parent is not None and parent.frame_global_ts is not None:
node.frame_global_ts = parent.frame_global_ts.clone()
# Drawables
if node.model is not None:
self.add_node_to_drawable(node)
# Observers
if class_name in OBSERVER_CLASS_NAMES:
self.add_node_to_observer(node)
def add_node_to_drawable(self, node: SceneNode, model_id: str = None):
"""
Add one node to the drawable list (nodes with non-empty `.model`).
Will automatically group nodes by their `model_id`.
Args:
node (SceneNode): The given node.
model_id (str, optional): Bypass `node.model.id`. Defaults to None.
"""
assert node.model is not None, 'only nodes with `model` is drawable'
self.drawables[node.id] = node
class_name = node.class_name
if class_name not in self.drawable_groups_by_class_name.keys():
self.drawable_groups_by_class_name[class_name] = IDListedDict()
self.drawable_groups_by_class_name[class_name][node.id] = node
if model_id is None: model_id = node.model.id
if model_id not in self.drawable_groups_by_model_id.keys():
self.drawable_groups_by_model_id[model_id] = IDListedDict()
self.drawable_groups_by_model_id[model_id][node.id] = node
def add_node_to_observer(self, node: SceneNode):
"""
Add one node to the observer list (nodes with observer class_name)
Will automatically group nodes by their class_name.
Args:
node (SceneNode): The given node.
"""
assert node.class_name in OBSERVER_CLASS_NAMES, f"only nodes of class_name in {OBSERVER_CLASS_NAMES} is an observer"
self.observers[node.id] = node
class_name = node.class_name
if class_name not in self.observer_groups_by_class_name.keys():
self.observer_groups_by_class_name[class_name] = IDListedDict()
self.observer_groups_by_class_name[class_name][node.id] = node
# @profile
def get_drawables(self, only_valid=True) -> IDListedDict[SceneNode]:
if only_valid:
return IDListedDict([o for o in self.drawables if o.i_valid])
else:
return self.drawables
# @profile
def get_drawable_groups_by_class_name(self, class_name: str, only_valid=True) -> IDListedDict[SceneNode]:
if class_name in self.drawable_groups_by_class_name.keys():
if only_valid:
return IDListedDict([o for o in self.drawable_groups_by_class_name[class_name] if o.i_valid])
else:
return self.drawable_groups_by_class_name[class_name]
else:
return IDListedDict()
def get_drawable_groups_by_class_name_list(self, class_name_list: List[str], only_valid=True) -> IDListedDict[SceneNode]:
if only_valid:
node_list = [o for o in self.drawables if o.i_valid and o.class_name in class_name_list]
else:
node_list = [o for o in self.drawables if o.class_name in class_name_list]
return IDListedDict(node_list)
# @profile
def get_drawable_groups_by_model_id(self, model_id: str, only_valid=True) -> IDListedDict[SceneNode]:
if only_valid:
return IDListedDict([o for o in self.drawable_groups_by_model_id[model_id] if o.i_valid])
else:
return self.drawable_groups_by_model_id[model_id]
@staticmethod
def group_drawables_by_class_name(drawables: List[SceneNode]) -> Dict[str, namedtuple_ind_id_obj]:
def key_fn(o: SceneNode):
return o.class_name
return Scene.group_drawables_by_key_fn(drawables, key_fn)
@staticmethod
def group_drawables_by_model_id(drawables: List[SceneNode]) -> Dict[str, namedtuple_ind_id_obj]:
def key_fn(o: SceneNode):
return o.model.id
return Scene.group_drawables_by_key_fn(drawables, key_fn)
@staticmethod
def group_drawables_by_key_fn(drawables: List[SceneNode], key_fn: Callable[[SceneNode],str]) -> Dict[str, namedtuple_ind_id_obj]:
map_groups: Dict[str, namedtuple_ind_id_obj] = {}
for ind, o in enumerate(drawables):
key = key_fn(o)
item = [ind, o.id, o]
if not key in map_groups:
map_groups[key] = [item]
else:
map_groups[key].append(item)
for n, g in map_groups.items():
map_groups[n] = namedtuple_ind_id_obj(*[list(x) for x in zip(*g)])
return map_groups
def get_drawable_class_ind_map(self):
# TODO: This could be adapted into a more scientific mapping method, such as following COCO.
all_classnames = self.drawable_groups_by_class_name.keys()
return {cn: i for i, cn in enumerate(all_classnames)}
def get_drawable_instance_ind_map(self):
all_ids = self.drawables.keys()
return {id:i for i, id in enumerate(all_ids)}
# @profile
def get_observers(self, only_valid=True) -> IDListedDict[SceneNode]:
if only_valid:
return IDListedDict([o for o in self.observers if o.i_valid])
else:
return self.observers
# @profile
def get_observer_groups_by_class_name(self, class_name: str, only_valid=True) -> IDListedDict[SceneNode]:
if class_name in self.observer_groups_by_class_name.keys():
if only_valid:
return IDListedDict([o for o in self.observer_groups_by_class_name[class_name] if o.i_valid])
else:
return self.observer_groups_by_class_name[class_name]
else:
return IDListedDict()
def get_cameras(self, only_valid=True) -> IDListedDict[Camera]:
if only_valid:
return IDListedDict([o for o in self.observer_groups_by_class_name['Camera'] if o.i_valid])
else:
return self.observer_groups_by_class_name['Camera']
#------------------------------------------------------------------
#---------------------- Render & ray query
@staticmethod
# @profile
def convert_rays_in_nodes_list(
rays_o: torch.Tensor, rays_d: torch.Tensor, node_list: List[SceneNode]
) -> Tuple[torch.Tensor, torch.Tensor]:
""" Convert rays in world coords to node object local coords
Args:
rays_o (torch.Tensor): [..., 3], Ray origins in world coords
rays_d (torch.Tensor): [..., 3], Ray directions in world coords
node_list (List[SceneNode]): List of nodes
Returns:
Tuple[torch.Tensor, torch.Tensor]:
rays_o_o: [num_objs, ..., 3], Ray origins in object local coords
rays_d_o: [num_objs, ..., 3], Ray directions in object local coords
"""
num_objs = len(node_list)
rotations = [obj.world_transform.rotation() for obj in node_list]
translations = [obj.world_transform.translation() for obj in node_list]
scales = [obj.scale.vec_3() for obj in node_list]
rotations_inv = torch.stack(rotations, 0).transpose(-1,-2).view(num_objs, -1, 3, 3)
translations = torch.stack(translations, 0).view(num_objs, -1, 3)
scales = torch.stack(scales, 0).view(num_objs, -1, 3)
"""
DEBUG:
[*] When slice_at single frame_ind:
rotations_inv: [num_objs, 3, 3] -> [num_objs, 1, 3, 3]
translations: [num_objs, 3] -> [num_objs, 1, 3]
scales: [num_objs, 3] -> [num_objs, 1, 3]
rays_o/rays_d: [num_rays, 3]
i_valid_flags: [] (e.g. node_list[0].i_valid_flags)
[*] When slice_at joint frame_ind and pixel locations:
rotations_inv: [num_objs, num_rays, 3, 3]
translations: [num_objs, num_rays, 3]
scales: [num_objs, num_rays, 3]
rays_o/rays_d: [num_rays, 3]
i_valid_flags: [num_rays]
"""
# rays_o_o = torch.einsum('...ij,...j->...i', rotations_inv, rays_o.unsqueeze(0)-translations)
# rays_d_o = torch.einsum('...ij,...j->...i', rotations_inv, rays_d.unsqueeze(0))
# [num_objs, num_rays, 3]
rays_o_o = (rotations_inv * (rays_o.unsqueeze(0)-translations).unsqueeze(-2)).sum(-1)
rays_d_o = (rotations_inv * rays_d[None,...,None,:]).sum(-1)
rays_o_o = rays_o_o / scales
rays_d_o = rays_d_o / scales
return rays_o_o, rays_d_o
@staticmethod
def convert_rays_in_node(
rays_o: torch.Tensor, rays_d: torch.Tensor, node: SceneNode
) -> Tuple[torch.Tensor, torch.Tensor]:
""" Convert rays in world coords to node object local coords
Args:
rays_o (torch.Tensor): [..., 3], Ray origins in world coords
rays_d (torch.Tensor): [..., 3], Ray directions in world coords
node (SceneNode): The given node
Returns:
Tuple[torch.Tensor, torch.Tensor]:
rays_o_o: [..., 3], Ray origins in object local coords
rays_d_o: [..., 3], Ray directions in object local coords
"""
rotations_inv = node.world_transform.rotation().transpose(-1,-2)
translations = node.world_transform.translation()
scales = node.scale.vec_3()
# [3, 3] @ [..., 1, 3]
rays_o = (rotations_inv * (rays_o - translations).unsqueeze(-2)).sum(-1) / scales
rays_d = (rotations_inv * rays_d.unsqueeze(-2)).sum(-1) / scales
return rays_o, rays_d
@torch.no_grad()
def process_observer_infos(self, **kwargs) -> namedtuple_observer_infos:
"""
Iterate through each frame of each camera, calculating information related to the camera trajectory.
class_name: [default=]'Camera'
far_clip: [default=]None
all: [defalt=]True
"""
# return namedtuple_observer_infos(
# tracks=torch.stack([
# torch.tensor([-1,-1,-1], dtype=torch.float, device=self.device),
# torch.tensor([1,1,1], dtype=torch.float, device=self.device)
# ], 0),
# all_frustum_pts=torch.stack([
# torch.tensor([-1,-1,-1], dtype=torch.float, device=self.device),
# torch.tensor([1,1,1], dtype=torch.float, device=self.device)
# ], 0))
# NOTE: Caching related
# if hasattr(self.process_observer_infos, '_cache_kwargs'):
# # NOTE: Since frozing scenes costs, we cache the first returns,
# # and use them if input is not changed
# changed = False
# cached_kwargs = self.process_observer_infos._cache_kwargs
# for k, v in kwargs.items():
# if k not in cached_kwargs.keys() or v != cached_kwargs[k]:
# changed = True
# break
# if not changed:
# return self.process_observer_infos._cache_ret
class_name = kwargs.setdefault('class_name', 'Camera')
far_clip = kwargs.setdefault('far_clip', None)
assert self.i is None, f"Can not call process_camera_infos() in the middle of a frozen scene"
cams = self.get_cameras(False)
frustum_extend_pts = []
cam_poses = []
self.slice_at_full()
cam0_poses = cams[0].world_transform.mat_4x4()
for cam in cams:
cam_poses.append(cam.world_transform.mat_4x4())
frustum_extend_pts.append(cam.get_view_frustum_pts(near=0., far=far_clip))
frustum_extend_pts = torch.stack(frustum_extend_pts, 0)
cam_poses = torch.stack(cam_poses, 0)
all_cam_tracks = cam_poses[..., :3, 3]
cam0_tracks = cam0_poses[..., :3, 3]
self.reset_attr()
ret = namedtuple_observer_infos(tracks=cam0_tracks,
all_tracks=all_cam_tracks,
all_frustum_pts=frustum_extend_pts)
# NOTE: Caching related
# self.process_observer_infos._cache_kwargs = deepcopy(kwargs)
# self.process_observer_infos._cache_ret = deepcopy(ret)
return ret
#------------------------------------------------------------------
#---------------------- Pytorch Misc
def _apply(self, fn):
# NOTE: Already inplace
for k in self.all_nodes.keys():
self.all_nodes[k]._apply(fn)
return self
@functools.wraps(torch.Tensor.to)
def to(self, *args, **kwargs):
# NOTE: Already inplace
for k in self.all_nodes.keys():
self.all_nodes[k].to(*args, **kwargs)
return self
@functools.wraps(nn.Module.cuda)
def cuda(self, device=None):
return self._apply(lambda t: t.cuda(device))
@functools.wraps(nn.Module.cpu)
def cpu(self):
return self._apply(lambda t: t.cpu())
@functools.wraps(nn.Module.float)
def float(self):
return self._apply(lambda t: t.float() if t.is_floating_point() else t)
@functools.wraps(nn.Module.double)
def double(self):
return self._apply(lambda t: t.double() if t.is_floating_point() else t)
@functools.wraps(nn.Module.double)
def half(self):
return self._apply(lambda t: t.half() if t.is_floating_point() else t)
#------------------------------------------------------
#--------------- DEBUG Functionalities ----------------
#------------------------------------------------------
@torch.no_grad()
def debug_vis_scene_graph(self, frame_ind=None, timestamp=None, arrow_length: float = 4.0, font_scale: float = 0.5):
from nr3d_lib.plot import create_camera_frustum_o3d
import open3d as o3d
import open3d.visualization.gui as gui
app = gui.Application.instance
app.initialize()
w = app.create_window(f"Scene-graph: {self.id}", 1024, 768)
widget3d = gui.SceneWidget()
w.add_child(widget3d)
widget3d.scene = o3d.visualization.rendering.Open3DScene(w.renderer)
# red = o3d.visualization.rendering.MaterialRecord()
# red.base_color = [1., 0., 0., 1.0]
# red.shader = "defaultUnlit"
# green = o3d.visualization.rendering.MaterialRecord()
# green.base_color = [0., 1., 0., 1.0]
# green.shader = "defaultUnlit"
# blue = o3d.visualization.rendering.MaterialRecord()
# blue.base_color = [0., 0., 1., 1.0]
# blue.shader = "defaultUnlit"
# black = o3d.visualization.rendering.MaterialRecord()
# black.base_color = [0., 0., 0., 1.]
# black.line_width = 10
# black.shader = "defaultUnlit"
line_mat = o3d.visualization.rendering.MaterialRecord()
line_mat.line_width = 3
line_mat.shader = "unlitLine"
edge_line_mat = o3d.visualization.rendering.MaterialRecord()
edge_line_mat.line_width = 10
edge_line_mat.shader = "unlitLine"
mat = o3d.visualization.rendering.MaterialRecord()
mat.shader = "defaultUnlit"
if frame_ind is not None:
self.slice_at(frame_ind)
elif timestamp is not None:
self.interp_at(timestamp)
cam0: Camera = self.get_observer_groups_by_class_name('Camera', False)['camera_FRONT']
all_drawables = self.get_drawables(True)
filtered_drawables = IDListedDict(cam0.filter_drawable_groups(all_drawables))
geometry_camera = None
# things_to_draw = vis_camera_o3d(
# colored_camera_dicts=[
# {
# 'intr': cam0.intr.mat_4x4().data.cpu().numpy(),
# 'c2w': cam0.world_transform.mat_4x4().data.cpu().numpy(),
# 'img_wh': (cam0.intr.W, cam0.intr.H)
# }
# ],
# cam_size=cam0.far, per_cam_axis=False, show=False)
def draw_node(o:SceneNode, parent:SceneNode=None):
"""
Recursively adds to things to draw
"""
# nonlocal things_to_draw
nonlocal geometry_camera
if o.i_valid:
#---- Coordinate frame
coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=arrow_length)
# coord_frame.compute_vertex_normals()
coord_frame.transform(o.world_transform.mat_4x4().data.cpu().numpy())
# things_to_draw.append(coord_frame)
widget3d.scene.add_geometry(o.id, coord_frame, mat)
if o.class_name in OBSERVER_CLASS_NAMES:
l = widget3d.add_3d_label(
o.world_transform(torch.tensor([0.,0.,arrow_length],
device=self.device, dtype=self.dtype)).data.cpu().numpy(),
o.class_name + ' : ' + o.id)
else:
l = widget3d.add_3d_label(
o.world_transform.translation().data.cpu().numpy(),
o.class_name + ' : ' + o.id)
l.scale = font_scale
#---- Draw Camera frustum
if o.class_name == 'Camera' and o.id == 'camera_FRONT':
geometry_camera = create_camera_frustum_o3d(
img_wh=(o.intr.W, o.intr.H),
intr=o.intr.mat_4x4().data.cpu().numpy(),
c2w=o.world_transform.mat_4x4().data.cpu().numpy(),
frustum_length=o.far if o.far is not None else 120.0, color=[0,0,1])
widget3d.scene.add_geometry(f"{o.id}.frustum", geometry_camera, line_mat)
#---- Draw OBB, if needed
# if o.model_bounding_sphere is not None:
if o.frame_data is not None and 'scale' in o.frame_data.subattr.keys():
# OBB
OBB = o3d.geometry.OrientedBoundingBox(
center=o.world_transform.translation().data.cpu().numpy(),
R=o.world_transform.rotation().data.cpu().numpy(),
extent=o.scale.vec_3().data.cpu().numpy()
)
# OBB.compute_vertex_normals()
if o.id in filtered_drawables.keys():
OBB.color = [1.0,0.0,0.0] # Drawables remained after frustum culling
else:
OBB.color = [0.0,0.0,0.0] # Drawables culled away
# things_to_draw.append(OBB)
widget3d.scene.add_geometry(f"{o.id}.box", OBB, line_mat)
#---- Draw parent-child connection edge, if needed
if parent is not None:
p_center = parent.world_transform.translation().data.cpu().numpy()
o_center = o.world_transform.translation().data.cpu().numpy()
if np.linalg.norm(p_center-o_center) > 0.01:
points = [p_center, o_center]
lines = [[0,1]]
colors = [[0.7,0.7,0.7]]
connection = o3d.geometry.LineSet()
connection.points = o3d.utility.Vector3dVector(points)
connection.lines = o3d.utility.Vector2iVector(lines)
connection.colors = o3d.utility.Vector3dVector(colors)
widget3d.scene.add_geometry(f"{parent.id}-{o.id}", connection, edge_line_mat)
for child in o.children:
draw_node(child, parent=o)
for child in self.root.children:
draw_node(child)
# o3d.visualization.draw_geometries(things_to_draw)
# (Optional) Try debug background space
if len(lst:=self.get_drawable_groups_by_class_name('Street')) > 0:
from nr3d_lib.models.spatial import ForestBlockSpace
bg_obj = lst[0]
bg_model = bg_obj.model
if (space:=getattr(bg_model, 'space', None)) and isinstance((space:=bg_model.space), ForestBlockSpace):
lineset_bg = space.debug_vis(show=False, draw_lines=True, draw_mesh=False)[0]
widget3d.scene.add_geometry(f"background.space", lineset_bg, line_mat)
bbox = widget3d.scene.bounding_box
widget3d.setup_camera(60.0, bbox, bbox.get_center())
app.run()
if frame_ind is not None or timestamp is not None:
self.unfrozen()
@torch.no_grad()
def debug_vis_multi_frame(
self,
plot_details_at: int = 0, # The frame at watch to plot dynamic objects and other details
plot_freq: int = 5 # In case things are too dense, only plot at every N frames. Set to 1 to plot all
):
import vedo
from nr3d_lib.plot import create_camera_frustum_vedo
fi = torch.arange(len(self), device=self.device, dtype=torch.long)
self.slice_at(fi)
if 'cam_id_list' in self.metas.keys():
cam0 = self.observer_groups_by_class_name['Camera'][self.metas['cam_id_list'][0]]
else:
cam0 = self.observer_groups_by_class_name['Camera'][0]
intrs = cam0.intr.mat_3x3().data.cpu().numpy()
c2ws = cam0.world_transform.mat_4x4().data.cpu().numpy()
WHs = cam0.intr.unscaled_wh().data.cpu().numpy().astype(int)
#---- Plot all camera frustums
cam_actors = []
for i, (WH, intr, c2w) in enumerate(zip(WHs, intrs, c2ws)):
if i == plot_details_at:
color = 'red'
lw = 4
else:
if plot_freq > 1 and (i % plot_freq < plot_freq):
continue
color = 'k4'
lw = 2
cam = create_camera_frustum_vedo(WH, intr, c2w, 0.2, color, lw)
cam_actors.append(cam)
self.unfrozen()
veh_boxes = []
self.slice_at(plot_details_at)
for node in self.all_nodes_by_class_name['Vehicle']:
if not node.i_valid:
continue
scale = node.scale.vec_3().data.cpu().numpy()
bound = np.stack([-scale/2.,scale/2.], axis=-1).reshape(6)
box = vedo.Box(size=bound)
box.apply_transform(node.world_transform.mat_4x4().data.cpu().numpy())
length = 2.
ax = vedo.Arrow([0,0,0], [length,0,0], c='r')
ay = vedo.Arrow([0,0,0], [0,length,0], c='g')