def assert_no_crossbatch_gradients(arr_out,
arr_in,
axis=0,
n_max=100,
name_out='',
name_in=''):
shape = tensoru.shape(arr_out)
shape1 = tensoru.shape(arr_in)
bdim = shape[axis]
bdim1 = shape1[axis]
# Ensure batch dimensionality is the same.
assert (bdim == bdim1)
# TODO temp.
assert (bdim == 10)
checks = []
for b_in in range(bdim):
for b_out in range(bdim):
if b_in == b_out: continue
grad = compute_crossbatch_gradient(arr_out=arr_out,
arr_in=arr_in,
b_in=b_in,
b_out=b_out,
axis=axis)
checks.append(
tf.compat.v1.assert_near(
grad,
0.0,
message=
"Cross-batch gradient (d[{}]_b={}/d[{}]_b={})is nonzero".
format(name_out, b_in, name_in, b_out)))
if len(checks) > n_max: break
return checks
def validate(self):
for k, v in self.items():
ks = tensoru.shape(k)
vs = tensoru.shape(v)
if ks != vs:
raise ValueError(
"Feed will fail because of key of shape {} has value of shape {}"
.format(ks, vs))
def __setitem__(self, k, v):
ks = tensoru.shape(k)
vs = tensoru.shape(v)
if ks != vs:
raise ValueError(
"Cannot assign to key of shape {} with value of shape {}".
format(ks, vs))
return dict.__setitem__(self, k, v)
def __init__(self, phi, should_augment=False, n_augments=1):
"""
Parameters
----------
phi : ESPPhiData
should_augment : bool
"""
_, _, self.T_past, _ = tensoru.shape(self.phi.S_past_world_frame)
self.B, self.A, self.T, self.D = tensoru.shape(
self.phi.S_future_world_frame)
self.B, self.H, self.W, self.C = tensoru.shape(
self.phi.overhead_features)
def plot_feature_map(feature_map,
partial_write_np_image_to_tb,
b=0,
nrows=3,
ncols=3,
fig=None,
axes=None):
if feature_map is None: return
figsize = (2 * ncols, 2 * nrows)
fig, axes = get_figure(fig=fig,
axes=axes,
figsize=figsize,
nrows=nrows,
ncols=ncols)
B, H, W, C = tensoru.shape(feature_map)
scalar_maps = feature_map[b]
for c in range(C):
ax = axes.ravel()[c]
scalar_map = scalar_maps[..., c]
ax.imshow(scalar_map, origin='upper')
for ax in axes.ravel():
ax.axis('off')
ax.set_xticks([])
ax.set_yticks([])
fig.tight_layout()
plot_figure('CNN_features_item_{}'.format(b),
fig,
partial_write_np_image_to_tb=partial_write_np_image_to_tb)
def frames_init(S_past_world_frame, overhead_features, yaws,
yaws_in_degrees, feature_pixels_per_meter):
agent_origins_world_frame = S_past_world_frame[..., -1, :]
# Object to convert from local vehicle frames to world frame.
local2world = similarityu.SimilarityTransform.from_origin_and_rotation(
origin=agent_origins_world_frame,
theta=yaws,
degrees=yaws_in_degrees,
scale=1.)
# Object to convert from world frame to frames of each vehicle.
world2local = local2world.invert()
H, W = tensoru.shape(overhead_features)[-3:-1]
world_origin_grid_frame = tf.constant((H // 2, W // 2),
dtype=tf.float64)
# Object to convert from world frame to grid frame
world2grid = similarityu.SimilarityTransform.from_origin_and_rotation(
world_origin_grid_frame,
tf.constant(0., dtype=tf.float64),
scale=feature_pixels_per_meter)
# N.B. this transformation means:
# 1. The world origin is in the center of the overhead features
# 2. There's no rotation ->
# 2a. the traj_x coordinate indexes into the _WIDTH_ dimension of the feature map.
# 2b. the traj_y coordinate indexes into the _HEIGHT_ dimension of the feature map
# Object to convert from local frames of each vehicle to grid frame.
local2grid = world2grid * local2world
return (local2world, world2local, world2grid, local2grid)
def __init__(self, S_car_frames_list, Z_list, m_list, mu_list, sigma_list,
sigel_list, metadata_list, phi):
"""
NB that S_car_frames_list is in the original coordinate frame at t=0.
"""
# (..., A, T, D) [i.e. (..., Agents, Timesteps, State Dimension) ]
# Stack along time.
self.S_car_frames = tf.stack(S_car_frames_list,
axis=-2,
name='S_car_frames')
self.Z = tf.stack(Z_list, axis=-2, name='Z')
self.m = tf.stack(m_list, axis=-2, name='m')
self.mu = tf.stack(mu_list, axis=-2, name='mu')
self.sigma = tf.stack(sigma_list, axis=-3, name='sigma')
self.sigel = tf.stack(sigel_list, axis=-3, name='sigel')
# Convert coords.
self.S_world_frame = tf.identity(phi.local2world.apply(
self.S_car_frames),
name='S_world_frame')
self.S_grid_frame = tf.identity(phi.world2grid.apply(
self.S_world_frame),
name='S_grid_frame')
# Metadata.
self.shape = tensoru.shape(self.Z)
self.event_shape = self.shape[-3:]
self.batch_shape = self.shape[:-3]
# The dimensionality of the rollout.
self.dimensionality = self.event_size = functools.reduce(
operator.mul, self.event_shape)
self.rollout_outputs = set(self.tensors)
def __init__(self,
S_past_world_frame,
yaws,
overhead_features,
agent_presence,
feature_pixels_per_meter,
is_training,
light_strings=None,
yaws_in_degrees=True,
past_perturb_epsilon=5e-2,
name=False):
"""
:param S_past_world_frame: (B, A, T, D)
:param yaws: (B, A)
:param overhead_features: (B, H, W, C)
:param agent_presence: (B, A)
:param yaws_in_degrees: bool
"""
assert (feature_pixels_per_meter >= 1)
if light_strings is None:
light_strings = np.array(['NONE'] * tensoru.size(yaws, 0),
dtype=np.unicode_)
# Overwrite these members with tensorized versions of them.
self.tensor_init(S_past_world_frame, yaws, overhead_features,
agent_presence, light_strings, is_training)
self._frames_init()
past_noise_world_frame = tf.random.normal(
mean=0.,
stddev=self.past_perturb_epsilon,
shape=tensoru.shape(self.S_past_world_frame),
dtype=tf.float64)
# Always create an alternative noisy-past (learning may not use it).
self.S_past_world_frame_noisy = self.S_past_world_frame + past_noise_world_frame
# (B, A, T, D)
self.S_past_car_frames = self.world2local.apply(
self.S_past_world_frame)
self.S_past_car_frames_noisy = self.world2local.apply(
self.S_past_world_frame_noisy)
# (B, A, T, D)
self.S_past_grid_frame = self.world2grid.apply(self.S_past_world_frame,
dtype=tf.float64)
self.S_past_grid_frame_noisy = self.world2grid.apply(
self.S_past_world_frame_noisy, dtype=tf.float64)
self.light_features = one_hotify_light_strings(self.light_strings)
if name:
# Name some intermediate computations (not placeholders)
self.S_past_grid_frame = tf.identity(self.S_past_grid_frame,
name='S_past_grid_frame')
self.S_past_car_frames = tf.identity(self.S_past_car_frames,
name='S_past_car_frames')
self.light_features = tf.identity(self.light_features,
name='light_features')
def get_stationary_z(self, feed_dict):
raise NotImplementedError(
"Haven't reimplemented inverse-graph computation")
fd2 = copy.copy(feed_dict)
if self.Z_inverse_output in feed_dict:
del feed_dict[self.Z_inverse_output]
xzero = np.zeros(tensoru.shape((self.planning_graph.x_inverse_input)),
dtype=np.float32)
xzero += np.random.normal(size=xzero.shape, loc=0., scale=1e-3)
fd2[(self.planning_graph.x_inverse_input)] = xzero
return self.sess.run(self.Z_inverse_output, fd2)
def call(self, model_distribution_samples, data_distribution_samples):
S_hats = model_distribution_samples.rollout.S_world_frame
S_star = data_distribution_samples.S_future_world_frame
delta = S_star[:, None] - S_hats
# Reshape (..., K, A, T, d) -> (..., K, ATd)
delta_vec = tf.reshape(delta, tensoru.shape(delta)[:-3] + (-1, ))
# <z,z>
msd = tf.einsum('...z,...z->...', delta_vec, delta_vec)
# (..., K) -> (...,)
minmsd = tf.reduce_min(msd, axis=-1)
AT = float((S_star.shape[-3] * S_star.shape[-2]).value)
mhat = tf.identity(minmsd / AT, name='mhat')
return mhat
def safe_det(x, *args, **kwargs):
"""Computes determinants of a batch of matrices. Allows for some matrices to be singular.
:param x:
:returns:
:rtype:
"""
raise NotImplementedError(
"For some reason, scatter_nd fails sometimes even when the input shapes are correct"
)
xs = tensoru.shape(x)
assert (xs[-1] == xs[-2])
# The shape of the matrices
D = xs[-1]
# Flatten batch dim.
x_flat = tf.reshape(x, (-1, D, D))
# Use SVD to count number of nonzero singular values -> invertible matrices have D nonsingular values.
nonsingular_matrix_bools = tf.equal(
tf.reduce_sum(tf.cast(
tf.linalg.svd(x_flat, compute_uv=False) > 1.5e-7, tf.int32),
axis=-1), D)
# Get the indices of the invertible matrices
nonsingular_matrix_inds = tf.cast(
tf.squeeze(tf.where(nonsingular_matrix_bools)), tf.int32)
# Gather the invertible matrices
x_flat_nonsingular = tf.gather(x_flat, nonsingular_matrix_inds)
# Compute determinants of the invertible matrices
det_x_nonsingular_matrix = tf.linalg.det(x_flat_nonsingular)
# Scatter the nonzero determinants into a vector. Unreference entries default to 0.
dets = tf.scatter_nd(indices=nonsingular_matrix_inds,
updates=det_x_nonsingular_matrix,
shape=tf.shape(nonsingular_matrix_bools))
# Reshape the resulting determinants.
dets_full = tf.reshape(dets, shape=tensoru.shape(x)[:-2])
return dets_full
def call(self, density_distribution, density_distribution_samples,
target_distribution_proxy, minibatch):
"""
:param density_distribution:
:param minibatch:
:returns: ESPObjectiveReturn(minimization criterion, H(p,q), ehat, rollout)
:rtype:
"""
assert (str(target_distribution_proxy) == 'EmptyProxy()')
# Compute log prob (in car frames!)
if not self.perturb:
log.warning("Not perturbing!")
log_prob, expert_roll = density_distribution.log_prob(
S_future=minibatch.experts.S_future_car_frames,
phi=minibatch.phi)
else:
noise = tf.random.normal(
mean=0.,
stddev=self.perturb_epsilon,
shape=(self.K_perturb, ) +
tensoru.shape(minibatch.experts.S_future_car_frames),
dtype=tf.float64)
noisy_experts = tensoru.swap_axes(
minibatch.experts.S_future_car_frames + noise, 0, 1)
log_prob, expert_roll = density_distribution.log_prob(
S_future=noisy_experts, phi=minibatch.phi)
forward_cross_entropy = tf.identity(-1 * log_prob, 'H_pq')
H_lb = npu.entropy_lower_bound(k=expert_roll.dimensionality,
stddev=self.perturb_epsilon)
ehat = tf.identity(
(forward_cross_entropy - H_lb) / expert_roll.dimensionality,
'ehat')
return interface.ESPObjectiveReturn(
min_criterion=forward_cross_entropy,
forward_cross_entropy=forward_cross_entropy,
ehat=ehat,
rollout=expert_roll)
def __init__(self,
frame,
phi,
world,
other_vehicles,
player_transforms,
others_transforms,
player_bbox,
other_id_ordering=None,
radius=200):
"""
1. generates a history of player and other vehicle position coordinates
of size len(player_transforms)
Parameters
----------
frame : int
Frame ID of observation
world : carla.World
other_vehicles : list of carla.Vehicle
player_transform : collections.deque of carla.Transform
Collection of transforms of player
Ordered by timestep where last in deque is current timestep
others_transforms : collections.deque of (dict of int : carla.Trajectory)
Collection of transforms of other vehicles by vehicle ID
Ordered by timestep where last in deque is current timestep
A : int
Number of vehicles in observation, including ego vehicle. Must be A > 1.
other_id_ordering : list of int
IDs of other (not player) vehicles (not walkers).
All IDs in other_id_ordering belong to some vehicle ID
radius : int
"""
_, _, self.T_past, _ = tensoru.shape(self.phi.S_past_world_frame)
self.B, self.A, self.T, self.D = tensoru.shape(
self.phi.S_future_world_frame)
self.B, self.H, self.W, self.C = tensoru.shape(
self.phi.overhead_features)
assert (len(player_transforms) == len(others_transforms))
assert (self.A > 0)
# player_transform : carla.Transform
# transform of current player
self.player_transform = player_transforms[-1]
# player_positions_world : ndarray of shape (len(player_transforms), 3)
self.player_positions_world = carlautil.transforms_to_location_ndarray(
self.player_transforms)
# player_positions_local : ndarray of shape (len(player_transforms), 3)
self.player_positions_local = self.player_positions_world \
- carlautil.transform_to_location_ndarray(self.player_transform)
# player_yaw : float
self.player_yaw = carlautil.transform_to_yaw(self.player_transform)
if self.other_id_ordering is None:
# get list of A agents within radius close to us
# note that other_id_ordering may have size smaller than A-1
ids = self.__get_nearby_agent_ids()
self.other_id_ordering = ids[:self.A - 1]
if self.other_id_ordering.shape != (0, ):
others_positions_list = [None] * len(self.others_transforms)
for idx, others_transform in enumerate(self.others_transforms):
others_positions_list[idx] = util.map_to_list(
lambda aid: carlautil.transform_to_location_ndarray(
others_transform[aid]), self.other_id_ordering)
# agent_positions_world : ndarray of shape (A-1, len(self.others_transforms), 3)
self.agent_positions_world = np.array(
others_positions_list).transpose(1, 0, 2)
others_transform = self.others_transforms[-1]
# agent_yaws : ndarray of shape (A-1,)
self.agent_yaws = util.map_to_ndarray(
lambda aid: carlautil.transform_to_yaw(others_transform[aid]),
self.other_id_ordering)
self.n_missing = max(
self.A - 1 - self.agent_positions_world.shape[0], 0)
self.has_other_agents = True
else:
self.agent_positions_world = np.array([])
self.agent_yaws = np.array([])
self.n_missing = self.A - 1
self.has_other_agents = False
if self.n_missing > 0:
faraway_position = carlautil.transform_to_location_ndarray(self.player_transform) \
+ np.array([0, 300, 0])
faraway_tile = np.tile(
faraway_position,
(self.n_missing, len(self.others_transforms), 1))
if self.n_missing == self.A - 1:
self.agent_positions_world = faraway_tile
self.agent_yaws = np.zeros(self.A - 1)
else:
self.agent_positions_world = np.concatenate(
(self.agent_positions_world, faraway_tile), axis=0)
self.agent_yaws = np.concatenate(
(self.agent_yaws, np.zeros(self.n_missing)), axis=0)
# agent_positions_local : ndarray of shape (A-1, len(self.others_transforms), 3)
self.agent_positions_local = self.agent_positions_world \
- carlautil.transform_to_location_ndarray(self.player_transform)
def _traj_to_s_T(self, trajectories):
assert (tensoru.rank(trajectories) == 5)
s_T = tf.squeeze(trajectories[..., -1, :])
assert (tensoru.shape(s_T) == self.full_shape)
return s_T
def __init__(self, tensor_collections):
"""Use the tensors from the graph to mock objects that were used to build the graph.
:param tensor_collections: dict: collection_name (str) -> collection (list)
:returns:
:rtype:
"""
self.joint_coll_dict = tfutil.get_multicollection_dict(
tensor_collections)
# Mock the objects we used when we originally constructed the graph.
self.add('phi', ESPPhi)
# Sampled rollout
self.add('rollout', ESPRollout)
# Sampled Z's
self.add('base_and_log_q', ESPBaseSamplesAndLogQ)
self.add('phi_metadata', ESPPhiMetadata)
self.add('experts', ESPExperts)
self.add('experts_metadata', ESPExpertsMetadata)
# This object is important to construct -- we'll need it for plotting.
self.sampled_output = ESPSampledOutput(
rollout=self.rollout,
phi=self.phi,
phi_metadata=self.phi_metadata,
base_and_log_q=self.base_and_log_q)
self.training_input = Mock(
ESPTrainingInput.__name__, {
'phi': self.phi,
'phi_m': self.phi_metadata,
'experts': self.experts,
'experts_m': self.experts_metadata
})
self.test_input = Mock(ESPTestInput.__name__, {
'phi': self.phi,
'phi_m': self.phi_metadata
})
# Create a to_feed_dict member function, modelled after the real `to_feed_dict` function.
# TODO this is annoying and brittle to hardcode to match the to_feed_dict above.
def to_feed_dict(xself, S_past_world_frame, yaws, overhead_features,
agent_presence, light_strings, S_future_world_frame,
metadata_list, is_training):
return dict(
zip(xself.placeholders, [
S_past_world_frame, yaws, overhead_features,
agent_presence, light_strings, S_future_world_frame,
is_training
]))
# Uses the fact that this collection was created in order...
self.training_input.placeholders = tensor_collections['infer_input']
self.training_input.to_feed_dict = types.MethodType(
to_feed_dict, self.training_input)
self.sampled_output.placeholders = tensor_collections['sample_input']
# TODO some params are hardcoded.
self.phi.local2world, self.phi.world2local, self.phi.world2grid, self.phi.local2grid = ESPPhi.frames_init(
self.phi.S_past_world_frame,
self.phi.overhead_features,
self.phi.yaws,
yaws_in_degrees=True,
feature_pixels_per_meter=2)
# Hack in missing things here.
self.phi_metadata.B, self.phi_metadata.H, *_ = tensoru.shape(
self.phi.overhead_features)
# Store some sample-rollout shape metadata
B, K, A, T, D = tensoru.shape(
self.sampled_output.rollout.S_world_frame)
self.metadata = attrdict.AttrDict({
'B': B,
'K': K,
'A': A,
'T': T,
'D': D
})
def log_prob(self, trajectories, *args, **kwargs):
return tf.squeeze(
tf.zeros(shape=tensoru.shape(trajectories)[:-2], dtype=tf.float64))
def __init__(self,
player_actor,
intersection_reader,
save_frequency=10,
save_directory='out',
n_burn_frames=60,
episode=0,
exclude_samples=SampleLabelFilter(),
phi_attributes=DEFAULT_PHI_ATTRIBUTES,
should_augment=False,
n_augments=1,
debug=False):
"""
Parameters
----------
player_actor : carla.Vehicle
Vehicle that the data collector is following around, and should collect data for.
intersection_reader : IntersectionReader
One IntersectionReader instance should be shared by all data collectors.
save_frequency : int
Frequency (wrt. to CARLA simulator frame) to save data.
n_burn_frames : int
The number of initial frames to skip before saving data.
episode : int
Index of current episode to collect data from.
exclude_samples : SampleLabelFilter
Filter to exclude saving samples by label.
phi_attributes : attrdict.AttrDict
Attributes T, T_past, B, A, ...etc to construct Phi object.
"""
self._player = player_actor
self._intersection_reader = intersection_reader
self.save_frequency = save_frequency
self._save_directory = save_directory
self.n_burn_frames = n_burn_frames
self.episode = episode
self.exclude_samples = exclude_samples
self._debug = debug
self.lidar_params = LidarParams()
self._phi = create_phi(phi_attributes)
_, _, self.T_past, _ = tensoru.shape(self._phi.S_past_world_frame)
self.B, self.A, self.T, self.D = tensoru.shape(
self._phi.S_future_world_frame)
self.B, self.H, self.W, self.C = tensoru.shape(
self._phi.overhead_features)
self._make_sample_name = lambda frame: "{}/ep{:03d}/agent{:03d}/frame{:08d}".format(
self._intersection_reader.map_name, self.episode, self._player.id,
frame)
self._world = self._player.get_world()
self._other_vehicles = list()
self._trajectory_size = max(self.T, self.T_past) + 1
# player_transforms : collections.deque of carla.Transform
self.player_transforms = collections.deque(
maxlen=self._trajectory_size)
# others_transforms : collections.deque of (dict of int : carla.Transform)
# Container of dict where key is other vehicle ID and value is
# carla.Transform
self.others_transforms = collections.deque(
maxlen=self._trajectory_size)
self.trajectory_feeds = collections.OrderedDict()
# lidar_feeds collections.OrderedDict
# Where int key is frame index and value
# is a carla.LidarMeasurement or carla.SemanticLidarMeasurement
self.lidar_feeds = collections.OrderedDict()
# __n_feeds : int
# Size of trajectory/lidar feed dict
self.__n_feeds = self.T + 1
self.streaming_generator = generate_observation.StreamingGenerator(
self._phi, should_augment=should_augment, n_augments=n_augments)
self.sensor = self.__create_segmentation_lidar_sensor()
self._first_frame = None
def forward(self, Z, phi):
"""Implements the forward component of the bijection. Implemented in __CAR FRAMES__.
:param Z: latent states
:param phi: context
:returns:
:rtype:
"""
r = tensoru.rank(Z)
with contextlib.ExitStack() as stack:
if getattr(self, 'debug_eager', False):
tape = tf.GradientTape(persistent=True)
stack.enter_context(tape)
tape.watch(phi.S_past_car_frames)
tape.watch(phi.overhead_features)
assert (r in (4, 5))
if r == 4:
B, A, T, D = tensoru.shape(Z)
S_0 = phi.S_past_car_frames[..., -1, :]
S_m1 = phi.S_past_car_frames[..., -2, :]
batch_shape = (B, )
elif r == 5:
B, K, A, T, D = tensoru.shape(Z)
# (B, A, T, D) -> (B, K, A, T, D)
S_0 = tensoru.expand_and_tile_axis(
phi.S_past_car_frames[..., -1, :], N=K, axis=1)
S_m1 = tensoru.expand_and_tile_axis(
phi.S_past_car_frames[..., -2, :], N=K, axis=1)
batch_shape = (B, K)
else:
raise ValueError("Unhandled rank of latents to warp.")
S_history = [S_m1, S_0]
Z_history = []
m_history = []
mu_history = []
sigel_history = []
sigma_history = []
metadata_history = []
phi.prepare(batch_shape)
self._prepare(batch_shape, phi)
for t_idx in range(T):
m_t, sigel_t, sigma_t = self.step_generate(S_history, phi)
mu_t = m_t + 2 * S_history[-1] - S_history[-2]
# Z first, then compute S.
Z_t = Z[..., t_idx, :]
S_t = mu_t + tf.einsum('...ij,...j->...i', sigma_t, Z_t)
phi.update_frames(S_t_car_frames=S_t,
S_tm1_car_frames=S_history[-1])
m_history.append(m_t)
mu_history.append(mu_t)
sigel_history.append(sigel_t)
sigma_history.append(sigma_t)
S_history.append(S_t)
Z_history.append(Z_t)
metadata_history.append(self.current_metadata)
if getattr(self, 'debug_eager', False): pdb.set_trace()
roll = interface.ESPRollout(S_car_frames_list=S_history[2:],
Z_list=Z_history,
m_list=m_history,
mu_list=mu_history,
sigma_list=sigma_history,
sigel_list=sigel_history,
metadata_list=metadata_history,
phi=phi)
return roll
def inverse(self, S, phi):
"""Implements the inverse component of the bijection.
:param S: output states. Must be in __CAR FRAMES__.
:param phi: context
:returns:
:rtype:
"""
r = tensoru.rank(S)
assert (r in (4, 5))
if r == 4:
B, A, T, D = tensoru.shape(S)
S_0 = phi.S_past_car_frames[..., -1, :]
S_m1 = phi.S_past_car_frames[..., -2, :]
batch_shape = (B, )
elif r == 5:
B, K, A, T, D = tensoru.shape(S)
S_0 = tensoru.expand_and_tile_axis(phi.S_past_car_frames[...,
-1, :],
N=K,
axis=1)
S_m1 = tensoru.expand_and_tile_axis(phi.S_past_car_frames[...,
-2, :],
N=K,
axis=1)
batch_shape = (B, K)
else:
raise ValueError("Unhandled rank of data to invert.")
S_history = [S_m1, S_0]
Z_history = []
m_history = []
mu_history = []
sigel_history = []
sigma_history = []
metadata_history = []
phi.prepare(batch_shape)
self._prepare(batch_shape, phi)
for t_idx in range(T):
m_t, sigel_t, sigma_t = self.step_generate(S_history, phi)
mu_t = m_t + 2 * S_history[-1] - S_history[-2]
# S first, then compute Z.
S_t = S[..., t_idx, :]
# expm(X) expm(-X) = I -> (expm(X))^{-1} = expm(-X). Avoids computing any inverses explicitly.
sigma_t_inv = tf.linalg.expm(-1 * sigel_t)
Z_t = tf.einsum('...ij,...j->...i', sigma_t_inv, S_t - mu_t)
phi.update_frames(S_t_car_frames=S_t,
S_tm1_car_frames=S_history[-1])
m_history.append(m_t)
mu_history.append(mu_t)
sigel_history.append(sigel_t)
sigma_history.append(sigma_t)
S_history.append(S_t)
Z_history.append(Z_t)
metadata_history.append(self.current_metadata)
roll = interface.ESPRollout(S_car_frames_list=S_history[2:],
Z_list=Z_history,
m_list=m_history,
mu_list=mu_history,
sigma_list=sigma_history,
sigel_list=sigel_history,
metadata_list=metadata_history,
phi=phi)
return roll
def populate_phi_feeds(self,
phi,
sensor_data,
measurement_buffer,
observations,
frame,
with_bev=True,
with_lights=True):
"""Populates the feed_dict values of phi for the current frame.
:param sensor_data:
:param with_bev:
:returns:
:rtype:
"""
assert (measurement_buffer.maxlen > self.T)
feed_dict = tfutil.FeedDict()
# Store these feeds for potential later expert population.
self.feed_dicts_and_transforms[frame] = (feed_dict,
observations.inv_tform_t)
B, H, W, C = tensoru.shape(phi.overhead_features)
_, A = tensoru.shape(phi.yaws)
# Extract robot pasts.
pasts = observations.player_positions_local[-self.T_past:, :2][None]
# Extract other agent pasts.
pasts_other = observations.agent_positions_local[:, -self.T_past:, :2]
# Indicate all present
agent_presence = np.ones(shape=tensoru.shape(phi.agent_presence),
dtype=np.float32)
# Combine ego and other pasts.
pasts_joint = np.concatenate((pasts, pasts_other))[:A][None]
# Tile pasts.
pasts_batch = np.tile(pasts_joint, (B, 1, 1, 1))
yaws = np.tile(np.asarray(observations.yaws_local[:A])[None], (B, 1))
feed_dict[phi.S_past_world_frame] = pasts_batch
feed_dict[phi.yaws] = yaws
feed_dict[phi.agent_presence] = agent_presence
feed_dict[phi.is_training] = np.array(False)
# Feed the BEV features.
if with_bev:
bevs = build_BEV(frame_data=sensor_data,
measurements=measurement_buffer[-1],
**self.bev_and_lidar_kwargs)[None]
assert (bevs.ndim == 4)
if bevs.shape[1] > H:
# Center crop the bev's preserving the pixels/meter of input.
log.debug("Center-cropping...")
# TODO move inside BEV building?
bevs = imgu.batch_center_crop(bevs, target_h=H, target_w=H)
if self.with_sdt:
# TODO hardcoded and bad! Currently set to match the SDT params from the precog repo.
for b in range(B):
serialized_dataset._create_sdt(bevs=bevs,
sdt_filename=None,
b=0,
H=H,
W=W,
C=C,
sdt_clip_thresh=0.5,
stamp=True,
save=False,
sdt_zero_h=3,
sdt_zero_w=8,
sdt_params={
'clip_top': 1,
'clip_bottom': -3,
'normalize': True
})
# TODO make the model more efficient by using B=1.
bevs = np.tile(bevs, (B, 1, 1, 1))
# Add the bevs to the feed dict.
feed_dict[phi.overhead_features] = bevs
if with_lights:
light_string = observations.traffic_light_state.upper()
light_string_batch = np.tile(np.asarray(light_string), (B, ))
feed_dict[phi.light_strings] = light_string_batch
log.debug("Feeding traffic light state '{}' to model".format(
light_string))
else:
log.warning("Not feeding traffic light state to model!")
return feed_dict
def run_carla_client(cfg, server_log_fn):
pidconf = cfg.pid
dimconf = cfg.dim
mainconf = cfg.main
dataconf = cfg.data
plotconf = cfg.plotting
serverconf = cfg.server
expconf = cfg.experiment
waypointerconf = cfg.waypointer
altcfg = cfg
tag = _fmt_cfg(cfg)
del cfg
# Set the seed.
log.debug("seed: {}".format(expconf.seed))
log.info("Command-line arguments: {}".format(' '.join(sys.argv)))
randu.seed(int(expconf.seed))
# How many frames total to run for each episode.
frames_per_episode = expconf.frames_per_episode
# List of the start frame indices of each datum.
starts = list(
range(dataconf.record_wait_frames, frames_per_episode - dataconf.TT,
dataconf.save_period_frames))
# Total \approx n_frames / (save_period_seconds*Hz). Eg desired=1000 -> n_frames = 1000 * (5 * 2)
log.info("Will collect {} examples per episode".format(len(starts)))
if plotconf.plot:
plt.close('all')
if plotconf.hires_plot:
fig, axes = cplot.create_agent_figure(
figsize=(6, 6),
image_size=None,
n_wide=2,
remove_second_row=plotconf.remove_second_row)
else:
fig, axes = cplot.create_agent_figure(
figsize=(3, 3),
image_size=None,
n_wide=2,
remove_second_row=plotconf.remove_second_row)
else:
fig, axes = None, None
# Instantiate the object used to track all metrics.
metrics = runtime_metrics.MultiepisodeMetrics(altcfg)
model = None
# Create and bundle the PID controllers.
car_pid_controllers = pid_controller.CarPIDControllers.frompidconf(pidconf)
lidar_params = preproc.LidarParams()
if mainconf.pilot == 'dim':
sess = tfutil.create_session(allow_growth=True,
per_process_gpu_memory_fraction=.2)
log.info("Loading DIM.")
if dimconf.old_version:
raise NotImplementedError("Old model loading not implemented yet")
else:
# Create model+inference network.
_, _, tensor_collections = tfutil.load_annotated_model(
dimconf.model_path, sess, dimconf.checkpoint_path)
model = precog.interface.ESPInference(tensor_collections)
model_config = yaml.safe_load(
open("{}/.hydra/config.yaml".format(dimconf.model_path), 'r'))
del tensor_collections
phi = model.phi
# TODO this is wrong, but we shouldn't need to use it if we're using the model?
S_future_world_frame = None
if mainconf.debug_post_load: pdb.set_trace()
# Extract the sizes of the past and future trajectories.
B, A, T_past, D = tensoru.shape(
model.test_input.phi.S_past_world_frame)
_B, K, _A, T, _D = tensoru.shape(
model.sampled_output.rollout.S_world_frame)
assert (_B == B and _A == A and _D == D)
# Create goal likelihood.
goal_likelihood = goal_distributions.create(model=model,
dimconf=dimconf)
# The object that generates plans.
log.info("Not building multiagent planner")
dim_planner = dim_plan.DIMPlanner(model,
goal_likelihood=goal_likelihood,
dimconf=dimconf,
sess=sess)
# Package the planner and the PID controllers into a single controller.
midlow_controller = dim_controller.DIMJointMiddleLowController(
dim_planner=dim_planner,
model=model,
replan_period=dimconf.replan_period,
car_pid_controllers=car_pid_controllers,
dimconf=dimconf)
elif mainconf.pilot == 'auto':
s = dataconf.shapes
T = s.T
T_past = s.T_past
# Reset the graph here so that these names are always valid even if this function called multiple times by same program.
tf.compat.v1.reset_default_graph()
S_past_world_frame = tf.zeros((s.B, s.A, s.T_past, s.D),
dtype=tf.float64,
name="S_past_world_frame")
S_future_world_frame = tf.zeros((s.B, s.A, s.T, s.D),
dtype=tf.float64,
name="S_future_world_frame")
yaws = tf.zeros((s.B, s.A), dtype=tf.float64, name="yaws")
overhead_features = tf.zeros((s.B, s.H, s.W, s.C),
dtype=tf.float64,
name="overhead_features")
agent_presence = tf.zeros((s.B, s.A),
dtype=tf.float64,
name="agent_presence")
light_strings = tf.zeros((s.B, ),
dtype=tf.string,
name="light_strings")
phi = precog.interface.ESPPhi(
S_past_world_frame=S_past_world_frame,
yaws=yaws,
overhead_features=overhead_features,
agent_presence=agent_presence,
light_strings=light_strings,
feature_pixels_per_meter=lidar_params.pixels_per_meter,
yaws_in_degrees=True)
midlow_controller = None
else:
raise ValueError(mainconf.pilot)
with make_carla_client(serverconf.host,
serverconf.port,
timeout=serverconf.client_timeout) as client:
log.info('CarlaClient connected')
root_dir = os.path.realpath(os.getcwd())
# possible cleanup
# atexit.register(query_remove_logdir, root_dir)
server_tag = '{:05d}'.format(int.from_bytes(os.urandom(2), 'big'))
os.symlink(server_log_fn,
root_dir + '/server_log_link_{}.log'.format(server_tag))
# Save the git info.
dir_path = os.path.dirname(os.path.realpath(__file__))
repo = git.Repo(dir_path, search_parent_directories=True)
sha = repo.head.object.hexsha
with open(root_dir + '/hexsha.txt', 'w') as f:
f.write(sha + '\n' + 'dirty: {}\n'.format(repo.is_dirty()))
log.info("Building settings programmatically")
settings_carla = carla_settings.create_settings(
n_vehicles=expconf.n_vehicles,
n_pedestrians=expconf.n_pedestrians,
quality_level=serverconf.quality_level,
image_size=dataconf.image_size,
use_lidar=dataconf.use_lidar,
root_dir=root_dir,
lidar_params=lidar_params)
# TODO hack
if 'val_obstacle' not in settings_carla.lidar_params:
settings_carla.lidar_params['val_obstacle'] = 1.
# Build the object that will process data and create feed_dicts for R2P2 forecasting.
streaming_loader = preproc.StreamingCARLALoader(
settings=settings_carla,
T_past=T_past,
T=T,
# TODO assumes model config
with_sdt=model_config['dataset']['params']['sdt_bev'])
# Manage the things we plot.
plottable_manager = cplot.PlottableManager(plotconf)
# TODO using gsheets is hardcoded to my username. You can either configure it for yourself or ignore that functionality.
record_google_sheets = mainconf.pilot != 'auto' and getpass.getuser(
) == 'nrhinehart' and expconf.record_google_sheets and gu.gsheets_util.have_pygsheets
if record_google_sheets:
gsheet_results = gu.MultiepisodeResults(tag=tag + 't_' +
server_tag,
multiep_metrics=metrics,
root_dir=root_dir)
for episode in range(0, expconf.n_episodes):
log.info('Starting episode {}/{}'.format(episode,
expconf.n_episodes))
# Bundle the parameters of the episode.
episode_params = agent_util.EpisodeParams(
episode=episode,
frames_per_episode=frames_per_episode,
root_dir=root_dir,
settings=settings_carla)
# Start running the episode.
run_carla_episode.run_episode(
client=client,
streaming_loader=streaming_loader,
model=model,
phi=phi,
future=S_future_world_frame,
midlow_controller=midlow_controller,
car_pid_controllers=car_pid_controllers,
plottable_manager=plottable_manager,
waypointerconf=waypointerconf,
episode_params=episode_params,
metrics=metrics,
fig=fig,
axes=axes,
cfg=altcfg)
log.info("Episode {} complete".format(episode))
if record_google_sheets: gsheet_results.update()
# Store the fact that we've run another episode.
expconf.n_episodes_run += 1
# Save metrics once we're done with the episodes.
with open(root_dir + "/final_metrics.dill".format(episode), 'wb') as f:
dill.dump(metrics, f)
with open(root_dir + "/metrics.dill".format(episode), 'wb') as f:
dill.dump(metrics, f)
log.info("Done with running CARLA client")
def validate_shape(self, objective_shape):
# Ensure each term matches our shape expectations.
assert (tensoru.shape(self.log_goal_likelihood) == objective_shape)
assert (tensoru.shape(self.log_prior) == objective_shape)
assert (tensoru.shape(self.log_posterior) == objective_shape)
