-
Notifications
You must be signed in to change notification settings - Fork 0
/
utils.py
executable file
·173 lines (152 loc) · 6.55 KB
/
utils.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
from typing import List, Optional, Tuple
import torch
import torch.nn as nn
from torch_geometric.data import Data
from datetime import datetime
class TemporalData(Data):
"""
Code adopted from https://github.com/ZikangZhou/HiVT
"""
def __init__(self,
x: Optional[torch.Tensor] = None,
positions: Optional[torch.Tensor] = None,
edge_index: Optional[torch.Tensor] = None,
edge_attrs: Optional[List[torch.Tensor]] = None,
y: Optional[torch.Tensor] = None,
num_nodes: Optional[int] = None,
padding_mask: Optional[torch.Tensor] = None,
bos_mask: Optional[torch.Tensor] = None,
rotate_angles: Optional[torch.Tensor] = None,
lane_vectors: Optional[torch.Tensor] = None,
is_intersections: Optional[torch.Tensor] = None,
turn_directions: Optional[torch.Tensor] = None,
traffic_controls: Optional[torch.Tensor] = None,
lane_actor_index: Optional[torch.Tensor] = None,
lane_actor_vectors: Optional[torch.Tensor] = None,
seq_id: Optional[int] = None,
**kwargs) -> None:
if x is None:
super(TemporalData, self).__init__()
return
super(TemporalData, self).__init__(x=x, positions=positions, edge_index=edge_index, y=y, num_nodes=num_nodes,
padding_mask=padding_mask, bos_mask=bos_mask, rotate_angles=rotate_angles,
lane_vectors=lane_vectors, is_intersections=is_intersections,
turn_directions=turn_directions, traffic_controls=traffic_controls,
lane_actor_index=lane_actor_index, lane_actor_vectors=lane_actor_vectors,
seq_id=seq_id, **kwargs)
if edge_attrs is not None:
for t in range(self.x.size(1)):
self[f'edge_attr_{t}'] = edge_attrs[t]
def __inc__(self, key, value):
if key == 'lane_actor_index':
return torch.tensor([[self['lane_vectors'].size(0)], [self.num_nodes]])
elif key in ['agent_index', 'ood']:
return self.num_nodes
else:
return super().__inc__(key, value)
class DistanceDropEdge(object):
"""
Code adopted from https://github.com/ZikangZhou/HiVT
"""
def __init__(self, max_distance: Optional[float] = None) -> None:
self.max_distance = max_distance
def __call__(self,
edge_index: torch.Tensor,
edge_attr: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]:
if self.max_distance is None:
return edge_index, edge_attr
row, col = edge_index
mask = torch.norm(edge_attr, p=2, dim=-1) < self.max_distance
edge_index = torch.stack([row[mask], col[mask]], dim=0)
edge_attr = edge_attr[mask]
return edge_index, edge_attr
def init_weights(m: nn.Module) -> None:
"""
Initialize network weights for the trajectory prediction model.
Code adopted from https://github.com/ZikangZhou/HiVT
"""
if isinstance(m, nn.Linear):
nn.init.xavier_uniform_(m.weight)
if m.bias is not None:
nn.init.zeros_(m.bias)
elif isinstance(m, (nn.Conv1d, nn.Conv2d, nn.Conv3d)):
fan_in = m.in_channels / m.groups
fan_out = m.out_channels / m.groups
bound = (6.0 / (fan_in + fan_out)) ** 0.5
nn.init.uniform_(m.weight, -bound, bound)
if m.bias is not None:
nn.init.zeros_(m.bias)
elif isinstance(m, nn.Embedding):
nn.init.normal_(m.weight, mean=0.0, std=0.02)
elif isinstance(m, (nn.BatchNorm1d, nn.BatchNorm2d, nn.BatchNorm3d)):
nn.init.ones_(m.weight)
nn.init.zeros_(m.bias)
elif isinstance(m, nn.LayerNorm):
nn.init.ones_(m.weight)
nn.init.zeros_(m.bias)
elif isinstance(m, nn.MultiheadAttention):
if m.in_proj_weight is not None:
fan_in = m.embed_dim
fan_out = m.embed_dim
bound = (6.0 / (fan_in + fan_out)) ** 0.5
nn.init.uniform_(m.in_proj_weight, -bound, bound)
else:
nn.init.xavier_uniform_(m.q_proj_weight)
nn.init.xavier_uniform_(m.k_proj_weight)
nn.init.xavier_uniform_(m.v_proj_weight)
if m.in_proj_bias is not None:
nn.init.zeros_(m.in_proj_bias)
nn.init.xavier_uniform_(m.out_proj.weight)
if m.out_proj.bias is not None:
nn.init.zeros_(m.out_proj.bias)
if m.bias_k is not None:
nn.init.normal_(m.bias_k, mean=0.0, std=0.02)
if m.bias_v is not None:
nn.init.normal_(m.bias_v, mean=0.0, std=0.02)
elif isinstance(m, nn.LSTM):
for name, param in m.named_parameters():
if 'weight_ih' in name:
for ih in param.chunk(4, 0):
nn.init.xavier_uniform_(ih)
elif 'weight_hh' in name:
for hh in param.chunk(4, 0):
nn.init.orthogonal_(hh)
elif 'weight_hr' in name:
nn.init.xavier_uniform_(param)
elif 'bias_ih' in name:
nn.init.zeros_(param)
elif 'bias_hh' in name:
nn.init.zeros_(param)
nn.init.ones_(param.chunk(4, 0)[1])
elif isinstance(m, nn.GRU):
for name, param in m.named_parameters():
if 'weight_ih' in name:
for ih in param.chunk(3, 0):
nn.init.xavier_uniform_(ih)
elif 'weight_hh' in name:
for hh in param.chunk(3, 0):
nn.init.orthogonal_(hh)
elif 'bias_ih' in name:
nn.init.zeros_(param)
elif 'bias_hh' in name:
nn.init.zeros_(param)
def rotate_trajectory(traj: torch.Tensor, rotate_mat: torch.Tensor) -> torch.Tensor:
"""
Rotate trajectory given the rotation matrix.
Args:
traj: trajectory of shap [K, N, T, 2]
rotate_mat: rotation matrix [N, 2, 2]
Return:
Rotated trajectory [K, N, T, 2]
"""
traj_rot = torch.zeros_like(traj)
for i in range(traj_rot.shape[0]):
traj_rot[i] = torch.bmm(traj[i], torch.inverse(rotate_mat))
return traj_rot
def get_current_datetime_as_string() -> str:
"""
Return the current datetime as string.
Return:
Current datetime as string.
"""
return datetime.now().strftime("%Y-%m-%d__%H-%M-%S")