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)
