def load_protobuf(filename): """ read a file in protobuf binary """ offline_features = offline_features_pb2.Features() with open(filename, 'rb') as file_in: offline_features.ParseFromString(file_in.read()) return offline_features.feature
def LabelJunctionExit(self): ''' label feature trajectory according to real future lane sequence in 7s ''' output_features = offline_features_pb2.Features() for obs_id, feature_sequence in self.feature_dict.items(): feature_seq_len = len(feature_sequence) for i, fea in enumerate(feature_sequence): # Sanity check. if not fea.HasField('junction_feature') or \ not len(fea.junction_feature.junction_exit): # print("No junction_feature, junction_exit, or junction_mlp_feature, not labeling this frame.") continue curr_pos = np.array([fea.position.x, fea.position.y]) # Only keep speed > 1 # TODO(all) consider recovery # if fea.speed <= 1: # continue heading = math.atan2(fea.raw_velocity.y, fea.raw_velocity.x) # Construct dictionary of all exit with dict[exit_lane_id] = np.array(exit_position) exit_dict = dict() exit_pos_dict = dict() for junction_exit in fea.junction_feature.junction_exit: if junction_exit.HasField('exit_lane_id'): exit_dict[junction_exit.exit_lane_id] = \ BoundingRectangle(junction_exit.exit_position.x, junction_exit.exit_position.y, junction_exit.exit_heading, 0.01, junction_exit.exit_width) exit_pos_dict[junction_exit.exit_lane_id] = np.array( [junction_exit.exit_position.x, junction_exit.exit_position.y]) # Searching for up to 100 frames (10 seconds) for j in range(i, min(i + 100, feature_seq_len)): car_bounding = BoundingRectangle(feature_sequence[j].position.x, feature_sequence[j].position.y, math.atan2(feature_sequence[j].raw_velocity.y, feature_sequence[j].raw_velocity.x), feature_sequence[j].length, feature_sequence[j].width) for key, value in exit_dict.items(): if car_bounding.overlap(value): exit_pos = exit_pos_dict[key] delta_pos = exit_pos - curr_pos angle = math.atan2( delta_pos[1], delta_pos[0]) - heading d_idx = int((angle / (2.0 * np.pi)) * 12 % 12) label = [0 for idx in range(12)] label[d_idx] = 1 fea.junction_feature.junction_mlp_label.extend(label) self.junction_label_dict["{}@{:.3f}".format(fea.id, fea.timestamp)] = label break # actually break two level else: continue break np.save(self.filepath + '.junction_label.npy', self.junction_label_dict)
def LabelTrajectory(self, period_of_interest=3.0): output_features = offline_features_pb2.Features() for obs_id, feature_sequence in self.feature_dict.items(): for idx, feature in enumerate(feature_sequence): # Observe the subsequent Features if "{}@{:.3f}".format(feature.id, feature.timestamp) not in self.observation_dict: continue observed_val = self.observation_dict["{}@{:.3f}".format(feature.id, feature.timestamp)] self.future_status_dict["{}@{:.3f}".format(feature.id, feature.timestamp)] = observed_val['obs_traj'] np.save(self.filepath + '.future_status.npy', self.future_status_dict)
def LoadPBFeatures(filepath): ''' @brief: parse the pb file of Feature of all obstacles at all times. @input filepath: the path of the pb file that contains all the features of every obstacle at every timestamp. @output: python readable format of the same content. ''' offline_features = offline_features_pb2.Features() with open(filepath, 'rb') as file_in: offline_features.ParseFromString(file_in.read()) return offline_features.feature
def load_protobuf(filename): """ read a file in protobuf binary """ features = [] offline_features = offline_features_pb2.Features() with open(filename, 'rb') as file_in: offline_features.ParseFromString(file_in.read()) for i in range(len(offline_features.feature)): features.append(offline_features.feature[i]) return features
def LabelTrajectory(self, period_of_interest=3.0): output_dict = self.feature_dict output_features = offline_features_pb2.Features() for obs_id, feature_sequence in output_dict.items(): for idx, feature in enumerate(feature_sequence): # Observe the subsequent Features if (feature.id, feature.timestamp) not in self.observation_dict: continue observed_val = self.observation_dict[(feature.id, feature.timestamp)] for point in observed_val['obs_traj']: traj_point = feature.future_trajectory_points.add() traj_point.path_point.x = point[0] traj_point.path_point.y = point[1] traj_point.path_point.velocity_heading = point[2] traj_point.timestamp = point[3] output_features.feature.add().CopyFrom(feature) output_dict[obs_id] = feature_sequence self.SaveOutputPB(self.filepath + '.future_status.label', output_features)
def LoadPBFeatures(self, filepath): self.filepath = filepath offline_features = offline_features_pb2.Features() with open(filepath, 'rb') as file_in: offline_features.ParseFromString(file_in.read()) return offline_features.feature
def LabelSingleLane(self, period_of_interest=3.0): output_features = offline_features_pb2.Features() for obs_id, feature_sequence in self.feature_dict.items(): feature_seq_len = len(feature_sequence) for idx, feature in enumerate(feature_sequence): if not feature.HasField('lane') or \ not feature.lane.HasField('lane_feature'): # print "No lane feature, cancel labeling" continue # Observe the subsequent Features if "{}@{:.3f}".format( feature.id, feature.timestamp) not in self.observation_dict: continue observed_val = self.observation_dict["{}@{:.3f}".format( feature.id, feature.timestamp)] lane_sequence_dict = dict() # Based on the observation, label data. for lane_sequence in feature.lane.lane_graph.lane_sequence: # Sanity check. if len(lane_sequence.lane_segment) == 0: print('There is no lane segment in this sequence.') continue # Handle jittering data if observed_val['is_jittering']: lane_sequence.label = -10 lane_sequence.time_to_lane_center = -1.0 lane_sequence.time_to_lane_edge = -1.0 continue # Handle the case that we didn't obesrve enough Features to label if observed_val['total_observed_time_span'] < period_of_interest and \ not observed_val['has_started_lane_change']: lane_sequence.label = -20 lane_sequence.time_to_lane_center = -1.0 lane_sequence.time_to_lane_edge = -1.0 # The current lane is obstacle's original lane. (labels: 0,2,4) if lane_sequence.vehicle_on_lane: # Obs is following ONE OF its original lanes: if not observed_val['has_started_lane_change'] or \ observed_val['lane_change_start_time'] > period_of_interest: # Record this lane_sequence's lane_ids current_lane_ids = [] for k in range(len(lane_sequence.lane_segment)): if lane_sequence.lane_segment[k].HasField( 'lane_id'): current_lane_ids.append( lane_sequence.lane_segment[k].lane_id) is_following_this_lane = True for l_id in range(1, min(len(current_lane_ids), \ len(observed_val['obs_actual_lane_ids']))): if current_lane_ids[l_id] != observed_val[ 'obs_actual_lane_ids'][l_id]: is_following_this_lane = False break # Obs is following this original lane: if is_following_this_lane: # Obstacle is following this original lane and moved to lane-center if observed_val[ 'lane_change_finish_time'] is not None: lane_sequence.label = 4 lane_sequence.time_to_lane_edge = -1.0 lane_sequence.time_to_lane_center = -1.0 # Obstacle is following this original lane but is never at lane-center: else: lane_sequence.label = 2 lane_sequence.time_to_lane_edge = -1.0 lane_sequence.time_to_lane_center = -1.0 # Obs is following another original lane: else: lane_sequence.label = 0 lane_sequence.time_to_lane_edge = -1.0 lane_sequence.time_to_lane_center = -1.0 # Obs has stepped out of this lane within period_of_interest. else: lane_sequence.label = 0 lane_sequence.time_to_lane_edge = -1.0 lane_sequence.time_to_lane_center = -1.0 # The current lane is NOT obstacle's original lane. (labels: -1,1,3) else: # Obstacle is following the original lane. if not observed_val['has_started_lane_change'] or \ observed_val['lane_change_start_time'] > period_of_interest: lane_sequence.label = -1 lane_sequence.time_to_lane_edge = -1.0 lane_sequence.time_to_lane_center = -1.0 else: new_lane_id_is_in_this_lane_seq = False for lane_segment in lane_sequence.lane_segment: if lane_segment.lane_id == observed_val[ 'new_lane_id']: new_lane_id_is_in_this_lane_seq = True break # Obstacle has changed to this lane. if new_lane_id_is_in_this_lane_seq: # Obstacle has finished lane changing within time_of_interest. if observed_val['has_finished_lane_change'] and \ observed_val['lane_change_finish_time'] < period_of_interest: lane_sequence.label = 3 lane_sequence.time_to_lane_edge = observed_val[ 'lane_change_start_time'] lane_sequence.time_to_lane_center = observed_val[ 'lane_change_finish_time'] # Obstacle started lane changing but haven't finished yet. else: lane_sequence.label = 1 lane_sequence.time_to_lane_edge = observed_val[ 'lane_change_start_time'] lane_sequence.time_to_lane_center = -1.0 # Obstacle has changed to some other lane. else: lane_sequence.label = -1 lane_sequence.time_to_lane_edge = -1.0 lane_sequence.time_to_lane_center = -1.0 for lane_sequence in feature.lane.lane_graph.lane_sequence: lane_sequence_dict[lane_sequence.lane_sequence_id] = [lane_sequence.label, \ lane_sequence.time_to_lane_center, lane_sequence.time_to_lane_edge] self.cruise_label_dict["{}@{:.3f}".format( feature.id, feature.timestamp)] = lane_sequence_dict np.save(self.filepath + '.cruise_label.npy', self.cruise_label_dict)