493 lines
19 KiB
Python
493 lines
19 KiB
Python
import numpy as np
|
|
from scipy.spatial.distance import pdist, squareform
|
|
import scipy.signal as ss
|
|
from collections import defaultdict
|
|
import warnings
|
|
from .node import Node
|
|
|
|
|
|
class Edge(object):
|
|
def __init__(self, curr_node, other_node):
|
|
self.id = self.get_edge_id(curr_node, other_node)
|
|
self.type = self.get_edge_type(curr_node, other_node)
|
|
self.curr_node = curr_node
|
|
self.other_node = other_node
|
|
|
|
@staticmethod
|
|
def get_edge_id(n1, n2):
|
|
raise NotImplementedError("Use one of the Edge subclasses!")
|
|
|
|
@staticmethod
|
|
def get_str_from_types(nt1, nt2):
|
|
raise NotImplementedError("Use one of the Edge subclasses!")
|
|
|
|
@staticmethod
|
|
def get_edge_type(n1, n2):
|
|
raise NotImplementedError("Use one of the Edge subclasses!")
|
|
|
|
def __eq__(self, other):
|
|
return (isinstance(other, self.__class__)
|
|
and self.id == other.id)
|
|
|
|
def __ne__(self, other):
|
|
return not self.__eq__(other)
|
|
|
|
def __hash__(self):
|
|
return hash(self.id)
|
|
|
|
def __repr__(self):
|
|
return self.id
|
|
|
|
|
|
class UndirectedEdge(Edge):
|
|
def __init__(self, curr_node, other_node):
|
|
super(UndirectedEdge, self).__init__(curr_node, other_node)
|
|
|
|
@staticmethod
|
|
def get_edge_id(n1, n2):
|
|
return '-'.join(sorted([str(n1), str(n2)]))
|
|
|
|
@staticmethod
|
|
def get_str_from_types(nt1, nt2):
|
|
return '-'.join(sorted([nt1.name, nt2.name]))
|
|
|
|
@staticmethod
|
|
def get_edge_type(n1, n2):
|
|
return '-'.join(sorted([n1.type.name, n2.type.name]))
|
|
|
|
|
|
class DirectedEdge(Edge):
|
|
def __init__(self, curr_node, other_node):
|
|
super(DirectedEdge, self).__init__(curr_node, other_node)
|
|
|
|
@staticmethod
|
|
def get_edge_id(n1, n2):
|
|
return '->'.join([str(n1), str(n2)])
|
|
|
|
@staticmethod
|
|
def get_str_from_types(nt1, nt2):
|
|
return '->'.join([nt1.name, nt2.name])
|
|
|
|
@staticmethod
|
|
def get_edge_type(n1, n2):
|
|
return '->'.join([n1.type.name, n2.type.name])
|
|
|
|
|
|
class TemporalSceneGraph(object):
|
|
def __init__(self,
|
|
edge_radius,
|
|
nodes=None,
|
|
adj_cube=np.zeros((1, 0, 0)),
|
|
weight_cube=np.zeros((1, 0, 0)),
|
|
node_type_mat=np.zeros((0, 0)),
|
|
edge_scaling=None):
|
|
self.edge_radius = edge_radius
|
|
self.nodes = nodes
|
|
if nodes is None:
|
|
self.nodes = np.array([])
|
|
self.adj_cube = adj_cube
|
|
self.weight_cube = weight_cube
|
|
self.node_type_mat = node_type_mat
|
|
self.adj_mat = np.max(self.adj_cube, axis=0).clip(max=1.0)
|
|
self.edge_scaling = edge_scaling
|
|
self.node_index_lookup = None
|
|
self.calculate_node_index_lookup()
|
|
|
|
def calculate_node_index_lookup(self):
|
|
node_index_lookup = dict()
|
|
for i, node in enumerate(self.nodes):
|
|
node_index_lookup[node] = i
|
|
|
|
self.node_index_lookup = node_index_lookup
|
|
|
|
def get_num_edges(self, t=0):
|
|
return np.sum(self.adj_cube[t]) // 2
|
|
|
|
def get_index(self, node):
|
|
return self.node_index_lookup[node]
|
|
|
|
@classmethod
|
|
def create_from_temp_scene_dict(cls,
|
|
scene_temp_dict,
|
|
attention_radius,
|
|
duration=1,
|
|
edge_addition_filter=None,
|
|
edge_removal_filter=None,
|
|
online=False):
|
|
"""
|
|
Construct a spatiotemporal graph from node positions in a dataset.
|
|
|
|
:param scene_temp_dict: Dict with all nodes in scene as keys and np.ndarray with positions as value
|
|
:param attention_radius: Attention radius dict.
|
|
:param duration: Temporal duration of the graph.
|
|
:param edge_addition_filter: -
|
|
:param edge_removal_filter: -
|
|
:return: TemporalSceneGraph
|
|
"""
|
|
|
|
nodes = scene_temp_dict.keys()
|
|
N = len(nodes)
|
|
total_timesteps = duration
|
|
|
|
if N == 0:
|
|
return TemporalSceneGraph(attention_radius)
|
|
|
|
position_cube = np.full((total_timesteps, N, 2), np.nan)
|
|
|
|
adj_cube = np.zeros((total_timesteps, N, N), dtype=np.int8)
|
|
dist_cube = np.zeros((total_timesteps, N, N), dtype=float)
|
|
|
|
node_type_mat = np.zeros((N, N), dtype=np.int8)
|
|
node_attention_mat = np.zeros((N, N), dtype=float)
|
|
|
|
for node_idx, node in enumerate(nodes):
|
|
if online:
|
|
# RingBuffers do not have a fixed constant size. Instead, they grow up to their capacity. Thus,
|
|
# we need to fill the values preceding the RingBuffer values with NaNs to make them fill the
|
|
# position_cube.
|
|
position_cube[-scene_temp_dict[node].shape[0]:, node_idx] = scene_temp_dict[node]
|
|
else:
|
|
position_cube[:, node_idx] = scene_temp_dict[node]
|
|
|
|
node_type_mat[:, node_idx] = node.type.value
|
|
for node_idx_from, node_from in enumerate(nodes):
|
|
node_attention_mat[node_idx_from, node_idx] = attention_radius[(node_from.type, node.type)]
|
|
|
|
np.fill_diagonal(node_type_mat, 0)
|
|
|
|
for timestep in range(position_cube.shape[0]):
|
|
dists = squareform(pdist(position_cube[timestep], metric='euclidean'))
|
|
|
|
# Put a 1 for all agent pairs which are closer than the edge_radius.
|
|
# Can produce a warning as dists can be nan if no data for node is available.
|
|
# This is accepted as nan <= x evaluates to False
|
|
with warnings.catch_warnings():
|
|
warnings.simplefilter("ignore")
|
|
adj_matrix = (dists <= node_attention_mat).astype(np.int8) * node_type_mat
|
|
|
|
# Remove self-loops.
|
|
np.fill_diagonal(adj_matrix, 0)
|
|
|
|
adj_cube[timestep] = adj_matrix
|
|
dist_cube[timestep] = dists
|
|
|
|
dist_cube[np.isnan(dist_cube)] = 0.
|
|
weight_cube = np.divide(1.,
|
|
dist_cube,
|
|
out=np.zeros_like(dist_cube),
|
|
where=(dist_cube > 0.))
|
|
edge_scaling = None
|
|
if edge_addition_filter is not None and edge_removal_filter is not None:
|
|
edge_scaling = cls.calculate_edge_scaling(adj_cube, edge_addition_filter, edge_removal_filter)
|
|
tsg = cls(attention_radius,
|
|
np.array(list(nodes)),
|
|
adj_cube, weight_cube,
|
|
node_type_mat,
|
|
edge_scaling=edge_scaling)
|
|
return tsg
|
|
|
|
@staticmethod
|
|
def calculate_edge_scaling(adj_cube, edge_addition_filter, edge_removal_filter):
|
|
shifted_right = np.pad(adj_cube, ((len(edge_addition_filter) - 1, 0), (0, 0), (0, 0)), 'constant', constant_values=0)
|
|
|
|
new_edges = np.minimum(
|
|
ss.convolve(shifted_right, np.reshape(edge_addition_filter, (-1, 1, 1)), 'full'), 1.
|
|
)[(len(edge_addition_filter) - 1):-(len(edge_addition_filter) - 1)]
|
|
|
|
new_edges[adj_cube == 0] = 0
|
|
|
|
result = np.minimum(
|
|
ss.convolve(new_edges, np.reshape(edge_removal_filter, (-1, 1, 1)), 'full'), 1.
|
|
)[:-(len(edge_removal_filter) - 1)]
|
|
|
|
return result
|
|
|
|
def to_scene_graph(self, t, t_hist=0, t_fut=0):
|
|
"""
|
|
Creates a Scene Graph from a Temporal Scene Graph
|
|
|
|
:param t: Time in Temporal Scene Graph for which Scene Graph is created.
|
|
:param t_hist: Number of history timesteps which are considered to form edges in Scene Graph.
|
|
:param t_fut: Number of future timesteps which are considered to form edges in Scene Graph.
|
|
:return: SceneGraph
|
|
"""
|
|
lower_t = np.clip(t-t_hist, a_min=0, a_max=None)
|
|
higher_t = np.clip(t + t_fut + 1, a_min=None, a_max=self.adj_cube.shape[0] + 1)
|
|
adj_mat = np.max(self.adj_cube[lower_t:higher_t], axis=0)
|
|
weight_mat = np.max(self.weight_cube[lower_t:higher_t], axis=0)
|
|
return SceneGraph(self.edge_radius,
|
|
self.nodes,
|
|
adj_mat,
|
|
weight_mat,
|
|
self.node_type_mat,
|
|
self.node_index_lookup,
|
|
edge_scaling=self.edge_scaling[t] if self.edge_scaling is not None else None)
|
|
|
|
|
|
class SceneGraph(object):
|
|
def __init__(self,
|
|
edge_radius,
|
|
nodes=None,
|
|
adj_mat=np.zeros((0, 0)),
|
|
weight_mat=np.zeros((0, 0)),
|
|
node_type_mat=np.zeros((0, 0)),
|
|
node_index_lookup=None,
|
|
edge_scaling=None):
|
|
self.edge_radius = edge_radius
|
|
self.nodes = nodes
|
|
if nodes is None:
|
|
self.nodes = np.array([])
|
|
self.node_type_mat = node_type_mat
|
|
self.adj_mat = adj_mat
|
|
self.weight_mat = weight_mat
|
|
self.edge_scaling = edge_scaling
|
|
self.node_index_lookup = node_index_lookup
|
|
|
|
def get_index(self, node):
|
|
return self.node_index_lookup[node]
|
|
|
|
def get_num_edges(self):
|
|
return np.sum(self.adj_mat) // 2
|
|
|
|
def get_neighbors(self, node, node_type):
|
|
"""
|
|
Get all neighbors of a node.
|
|
|
|
:param node: Node for which all neighbors are returned.
|
|
:param node_type: Specifies node types which are returned.
|
|
:return: List of all neighbors.
|
|
"""
|
|
node_index = self.get_index(node)
|
|
connection_mask = self.get_connection_mask(node_index)
|
|
mask = ((self.node_type_mat[node_index] == node_type.value) * connection_mask)
|
|
return self.nodes[mask]
|
|
|
|
def get_edge_scaling(self, node=None):
|
|
if node is None:
|
|
return self.edge_scaling
|
|
else:
|
|
node_index = self.get_index(node)
|
|
connection_mask = self.get_connection_mask(node_index)
|
|
return self.edge_scaling[node_index, connection_mask]
|
|
|
|
def get_edge_weight(self, node=None):
|
|
if node is None:
|
|
return self.weight_mat
|
|
else:
|
|
node_index = self.get_index(node)
|
|
connection_mask = self.get_connection_mask(node_index)
|
|
return self.weight_mat[node_index, connection_mask]
|
|
|
|
def get_connection_mask(self, node_index):
|
|
if self.edge_scaling is None: # We do not use edge scaling
|
|
return self.adj_mat[node_index] > 0.
|
|
else:
|
|
return self.edge_scaling[node_index] > 1e-2
|
|
|
|
def __sub__(self, other):
|
|
new_nodes = [node for node in self.nodes if node not in other.nodes]
|
|
removed_nodes = [node for node in other.nodes if node not in self.nodes]
|
|
|
|
our_types = set(node.type for node in self.nodes)
|
|
other_types = set(node.type for node in other.nodes)
|
|
all_node_types = our_types | other_types
|
|
|
|
new_neighbors = defaultdict(lambda: defaultdict(set))
|
|
for node in self.nodes:
|
|
if node in removed_nodes:
|
|
continue
|
|
|
|
if node in other.nodes:
|
|
for node_type in all_node_types:
|
|
new_items = set(self.get_neighbors(node, node_type)) - set(other.get_neighbors(node, node_type))
|
|
if len(new_items) > 0:
|
|
new_neighbors[node][DirectedEdge.get_edge_type(node, Node(node_type, None, None))] = new_items
|
|
else:
|
|
for node_type in our_types:
|
|
neighbors = self.get_neighbors(node, node_type)
|
|
if len(neighbors) > 0:
|
|
new_neighbors[node][DirectedEdge.get_edge_type(node, Node(node_type, None, None))] = set(neighbors)
|
|
|
|
removed_neighbors = defaultdict(lambda: defaultdict(set))
|
|
for node in other.nodes:
|
|
if node in removed_nodes:
|
|
continue
|
|
|
|
if node in self.nodes:
|
|
for node_type in all_node_types:
|
|
removed_items = set(other.get_neighbors(node, node_type)) - set(self.get_neighbors(node, node_type))
|
|
if len(removed_items) > 0:
|
|
removed_neighbors[node][DirectedEdge.get_edge_type(node, Node(node_type, None, None))] = removed_items
|
|
else:
|
|
for node_type in other_types:
|
|
neighbors = other.get_neighbors(node, node_type)
|
|
if len(neighbors) > 0:
|
|
removed_neighbors[node][DirectedEdge.get_edge_type(node, Node(node_type, None, None))] = set(neighbors)
|
|
|
|
return new_nodes, removed_nodes, new_neighbors, removed_neighbors
|
|
|
|
|
|
if __name__ == '__main__':
|
|
from environment import NodeTypeEnum
|
|
import time
|
|
|
|
# # # # # # # # # # # # # # # # #
|
|
# Testing edge mask calculation #
|
|
# # # # # # # # # # # # # # # # #
|
|
B = np.array([[0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0],
|
|
[1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
|
|
[1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0],
|
|
[1, 1, 1, 1, 1, 0, 1, 0, 0, 1, 1, 1, 0]])[:, :, np.newaxis, np.newaxis]
|
|
print(B.shape)
|
|
|
|
edge_addition_filter = [0.25, 0.5, 0.75, 1.0]
|
|
edge_removal_filter = [1.0, 0.5, 0.0]
|
|
for i in range(B.shape[0]):
|
|
A = B[i] # (time, N, N)
|
|
|
|
print(A[:, 0, 0])
|
|
|
|
start = time.time()
|
|
new_edges = np.minimum(ss.convolve(A, np.reshape(edge_addition_filter, (-1, 1, 1)), 'full'), 1.)[(len(edge_addition_filter) - 1):]
|
|
old_edges = np.minimum(ss.convolve(A, np.reshape(edge_removal_filter, (-1, 1, 1)), 'full'), 1.)[:-(len(edge_removal_filter) - 1)]
|
|
res = np.minimum(new_edges + old_edges, 1.)[:, 0, 0]
|
|
end = time.time()
|
|
print(end - start)
|
|
print(res)
|
|
|
|
start = time.time()
|
|
res = TemporalSceneGraph.calculate_edge_scaling(A, edge_addition_filter, edge_removal_filter)[:, 0, 0]
|
|
end = time.time()
|
|
print(end - start)
|
|
print(res)
|
|
|
|
print('-'*40)
|
|
|
|
# # # # # # # # # # # # # # #
|
|
# Testing graph subtraction #
|
|
# # # # # # # # # # # # # # #
|
|
print('\n' + '-' * 40 + '\n')
|
|
|
|
node_type_list = ['PEDESTRIAN',
|
|
'BICYCLE',
|
|
'VEHICLE']
|
|
nte = NodeTypeEnum(node_type_list)
|
|
|
|
attention_radius = dict()
|
|
attention_radius[(nte.PEDESTRIAN, nte.PEDESTRIAN)] = 5.0
|
|
attention_radius[(nte.PEDESTRIAN, nte.VEHICLE)] = 20.0
|
|
attention_radius[(nte.PEDESTRIAN, nte.BICYCLE)] = 10.0
|
|
attention_radius[(nte.VEHICLE, nte.PEDESTRIAN)] = 20.0
|
|
attention_radius[(nte.VEHICLE, nte.VEHICLE)] = 20.0
|
|
attention_radius[(nte.VEHICLE, nte.BICYCLE)] = 20.0
|
|
attention_radius[(nte.BICYCLE, nte.PEDESTRIAN)] = 10.0
|
|
attention_radius[(nte.BICYCLE, nte.VEHICLE)] = 20.0
|
|
attention_radius[(nte.BICYCLE, nte.BICYCLE)] = 10.0
|
|
|
|
scene_dict1 = {Node(nte.PEDESTRIAN, node_id='1'): np.array([1, 0]),
|
|
Node(nte.PEDESTRIAN, node_id='2'): np.array([0, 1])}
|
|
sg1 = TemporalSceneGraph.create_from_temp_scene_dict(
|
|
scene_dict1,
|
|
attention_radius=attention_radius,
|
|
duration=1,
|
|
edge_addition_filter=[0.25, 0.5, 0.75, 1.0],
|
|
edge_removal_filter=[1.0, 0.0]).to_scene_graph(t=0)
|
|
|
|
scene_dict2 = {Node(nte.PEDESTRIAN, node_id='1'): np.array([1, 0]),
|
|
Node(nte.PEDESTRIAN, node_id='2'): np.array([1, 1])}
|
|
sg2 = TemporalSceneGraph.create_from_temp_scene_dict(
|
|
scene_dict2,
|
|
attention_radius=attention_radius,
|
|
duration=1,
|
|
edge_addition_filter=[0.25, 0.5, 0.75, 1.0],
|
|
edge_removal_filter=[1.0, 0.0]).to_scene_graph(t=0)
|
|
|
|
new_nodes, removed_nodes, new_neighbors, removed_neighbors = sg2 - sg1
|
|
print('New Nodes:', new_nodes)
|
|
print('Removed Nodes:', removed_nodes)
|
|
print('New Neighbors:', new_neighbors)
|
|
print('Removed Neighbors:', removed_neighbors)
|
|
|
|
# # # # # # # # # # # # # # #
|
|
print('\n' + '-' * 40 + '\n')
|
|
|
|
scene_dict1 = {Node(nte.PEDESTRIAN, node_id='1'): np.array([1, 0]),
|
|
Node(nte.PEDESTRIAN, node_id='2'): np.array([0, 1])}
|
|
sg1 = TemporalSceneGraph.create_from_temp_scene_dict(
|
|
scene_dict1,
|
|
attention_radius=attention_radius,
|
|
duration=1,
|
|
edge_addition_filter=[0.25, 0.5, 0.75, 1.0],
|
|
edge_removal_filter=[1.0, 0.0]).to_scene_graph(t=0)
|
|
|
|
scene_dict2 = {Node(nte.PEDESTRIAN, node_id='1'): np.array([1, 0]),
|
|
Node(nte.PEDESTRIAN, node_id='2'): np.array([1, 1]),
|
|
Node(nte.PEDESTRIAN, node_id='3'): np.array([20, 1])}
|
|
sg2 = TemporalSceneGraph.create_from_temp_scene_dict(
|
|
scene_dict2,
|
|
attention_radius=attention_radius,
|
|
duration=1,
|
|
edge_addition_filter=[0.25, 0.5, 0.75, 1.0],
|
|
edge_removal_filter=[1.0, 0.0]).to_scene_graph(t=0)
|
|
|
|
new_nodes, removed_nodes, new_neighbors, removed_neighbors = sg2 - sg1
|
|
print('New Nodes:', new_nodes)
|
|
print('Removed Nodes:', removed_nodes)
|
|
print('New Neighbors:', new_neighbors)
|
|
print('Removed Neighbors:', removed_neighbors)
|
|
|
|
# # # # # # # # # # # # # # #
|
|
print('\n' + '-' * 40 + '\n')
|
|
|
|
scene_dict1 = {Node(nte.PEDESTRIAN, node_id='1'): np.array([1, 0]),
|
|
Node(nte.PEDESTRIAN, node_id='2'): np.array([0, 1])}
|
|
sg1 = TemporalSceneGraph.create_from_temp_scene_dict(
|
|
scene_dict1,
|
|
attention_radius=attention_radius,
|
|
duration=1,
|
|
edge_addition_filter=[0.25, 0.5, 0.75, 1.0],
|
|
edge_removal_filter=[1.0, 0.0]).to_scene_graph(t=0)
|
|
|
|
scene_dict2 = {Node(nte.PEDESTRIAN, node_id='1'): np.array([1, 0]),
|
|
Node(nte.PEDESTRIAN, node_id='2'): np.array([10, 1]),
|
|
Node(nte.PEDESTRIAN, node_id='3'): np.array([20, 1])}
|
|
sg2 = TemporalSceneGraph.create_from_temp_scene_dict(
|
|
scene_dict2,
|
|
attention_radius=attention_radius,
|
|
duration=1,
|
|
edge_addition_filter=[0.25, 0.5, 0.75, 1.0],
|
|
edge_removal_filter=[1.0, 0.0]).to_scene_graph(t=0)
|
|
|
|
new_nodes, removed_nodes, new_neighbors, removed_neighbors = sg2 - sg1
|
|
print('New Nodes:', new_nodes)
|
|
print('Removed Nodes:', removed_nodes)
|
|
print('New Neighbors:', new_neighbors)
|
|
print('Removed Neighbors:', removed_neighbors)
|
|
|
|
# # # # # # # # # # # # # # #
|
|
print('\n' + '-' * 40 + '\n')
|
|
|
|
scene_dict1 = {Node(nte.PEDESTRIAN, node_id='1'): np.array([0, 0]),
|
|
Node(nte.PEDESTRIAN, node_id='2'): np.array([0, 1])}
|
|
sg1 = TemporalSceneGraph.create_from_temp_scene_dict(
|
|
scene_dict1,
|
|
attention_radius=attention_radius,
|
|
duration=1,
|
|
edge_addition_filter=[0.25, 0.5, 0.75, 1.0],
|
|
edge_removal_filter=[1.0, 0.0]).to_scene_graph(t=0)
|
|
|
|
scene_dict2 = {Node(nte.PEDESTRIAN, node_id='2'): np.array([10, 1]),
|
|
Node(nte.PEDESTRIAN, node_id='3'): np.array([12, 1]),
|
|
Node(nte.PEDESTRIAN, node_id='4'): np.array([13, 1])}
|
|
sg2 = TemporalSceneGraph.create_from_temp_scene_dict(
|
|
scene_dict2,
|
|
attention_radius=attention_radius,
|
|
duration=1,
|
|
edge_addition_filter=[0.25, 0.5, 0.75, 1.0],
|
|
edge_removal_filter=[1.0, 0.0]).to_scene_graph(t=0)
|
|
|
|
new_nodes, removed_nodes, new_neighbors, removed_neighbors = sg2 - sg1
|
|
print('New Nodes:', new_nodes)
|
|
print('Removed Nodes:', removed_nodes)
|
|
print('New Neighbors:', new_neighbors)
|
|
print('Removed Neighbors:', removed_neighbors)
|