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)