def on_imu_update(self, msg):
""" Invoked upon receipt of an IMU sensor update.
Logs the lateral and longitudinal acceleration and jerk.
Args:
msg (:py:class:`pylot.perception.messages.IMUMessage`): The IMU
message sent by the sensor.
"""
sim_time = msg.timestamp.coordinates[0]
lateral_acc, longitudinal_acc = msg.acceleration.y, msg.acceleration.x
# Log the lateral and the longitudinal acceleration.
self._csv_logger.info('{},{},acceleration,lateral,{:.4f}'.format(
time_epoch_ms(), sim_time, lateral_acc))
self._csv_logger.info('{},{},acceleration,longitudinal,{:.4f}'.format(
time_epoch_ms(), sim_time, longitudinal_acc))
# Calculate the jerk, and log both lateral and longitudinal jerk.
if self._last_timestamp:
time_diff_sec = (msg.timestamp.coordinates[0] -
self._last_timestamp.coordinates[0]) / 1000.0
lateral_jerk = (self._last_lateral_acc -
lateral_acc) / time_diff_sec
longitudinal_jerk = (self._last_longitudinal_acc -
longitudinal_acc) / time_diff_sec
self._csv_logger.info('{},{},jerk,lateral,{:.4f}'.format(
time_epoch_ms(), sim_time, lateral_jerk))
self._csv_logger.info('{},{},jerk,longitudinal,{:.4f}'.format(
time_epoch_ms(), sim_time, longitudinal_jerk))
# Save the new acceleration and timestamp.
self._last_lateral_acc = lateral_acc
self._last_longitudinal_acc = longitudinal_acc
self._last_timestamp = msg.timestamp
def on_watermark(self, timestamp):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
assert len(timestamp.coordinates) == 1
op_start_time = time.time()
game_time = timestamp.coordinates[0]
if not self._last_notification:
self._last_notification = game_time
return
else:
self._sim_interval = (game_time - self._last_notification)
self._last_notification = game_time
while len(self._tracker_start_end_times) > 0:
(end_time, start_time) = self._tracker_start_end_times[0]
# We can compute tracker metrics if the endtime is not greater than
# the ground time.
if end_time <= game_time:
# This is the closest ground bounding box to the end time.
heapq.heappop(self._tracker_start_end_times)
ground_obstacles = self.__get_ground_obstacles_at(end_time)
# Get tracker output obstacles.
tracker_obstacles = self.__get_tracked_obstacles_at(start_time)
if (len(tracker_obstacles) > 0 or len(ground_obstacles) > 0):
metrics_summary_df = self.get_tracker_metrics(
tracker_obstacles, ground_obstacles)
# Get runtime in ms
runtime = (time.time() - op_start_time) * 1000
self._csv_logger.info("{},{},{},{}".format(
time_epoch_ms(), self.config.name, "runtime", runtime))
# Write metrics to csv log file
for metric_name in self._flags.tracking_metrics:
if metric_name in metrics_summary_df.columns:
self._csv_logger.info("{},{},{},{}".format(
time_epoch_ms(), self.config.name, metric_name,
metrics_summary_df[metric_name].values[0]))
else:
raise ValueError(
"Unexpected tracking metric: {}".format(
metric_name))
self._logger.debug('Computing accuracy for {} {}'.format(
end_time, start_time))
else:
# The remaining entries require newer ground obstacles.
break
self.__garbage_collect_obstacles()
def on_msg_camera_stream(self, msg, segmented_stream):
"""Camera stream callback method.
Invoked upon the receipt of a message on the camera stream.
"""
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self._name))
start_time = time.time()
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
image = torch.from_numpy(msg.frame.frame.transpose(
[2, 0, 1])).unsqueeze(0).float()
image_var = Variable(image, requires_grad=False, volatile=True)
final = self._model(image_var)[0]
_, pred = torch.max(final, 1)
pred = pred.cpu().data.numpy()[0]
# After we apply the pallete, the image is in RGB format
image_np = self._pallete[pred.squeeze()]
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, msg.timestamp,
runtime))
frame = SegmentedFrame(image_np, 'cityscapes', msg.frame.camera_setup)
if self._flags.visualize_segmentation_output:
frame.visualize(self._name, msg.timestamp)
segmented_stream.send(
SegmentedFrameMessage(frame, msg.timestamp, runtime))
def on_notification(self, timestamp):
assert len(timestamp.coordinates) == 1
game_time = timestamp.coordinates[0]
if not self._last_notification:
self._last_notification = game_time
return
else:
self._sim_interval = (game_time - self._last_notification)
self._last_notification = game_time
while len(self._detector_start_end_times) > 0:
(end_time, start_time) = self._detector_start_end_times[0]
# We can compute mAP if the endtime is not greater than the ground
# time.
if end_time <= game_time:
# This is the closest ground bounding box to the end time.
heapq.heappop(self._detector_start_end_times)
ground_obstacles = self.__get_ground_obstacles_at(end_time)
# Get detector output obstacles.
obstacles = self.__get_obstacles_at(start_time)
if (len(obstacles) > 0 or len(ground_obstacles) > 0):
mAP = get_mAP(ground_obstacles, obstacles)
self._logger.info('mAP is: {}'.format(mAP))
self._csv_logger.info('{},{},{},{}'.format(
time_epoch_ms(), self._name, 'mAP', mAP))
self._logger.debug('Computing accuracy for {} {}'.format(
end_time, start_time))
else:
# The remaining entries require newer ground obstacles.
break
self.__garbage_collect_obstacles()
def on_watermark(self, timestamp):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
assert len(timestamp.coordinates) == 1
op_start_time = time.time()
game_time = timestamp.coordinates[0]
if not self._last_notification:
self._last_notification = game_time
return
else:
self._sim_interval = (game_time - self._last_notification)
self._last_notification = game_time
sim_time = timestamp.coordinates[0]
while len(self._detector_start_end_times) > 0:
(end_time, start_time) = self._detector_start_end_times[0]
# We can compute mAP if the endtime is not greater than the ground
# time.
if end_time <= game_time:
# This is the closest ground bounding box to the end time.
heapq.heappop(self._detector_start_end_times)
ground_obstacles = self.__get_ground_obstacles_at(end_time)
# Get detector output obstacles.
obstacles = self.__get_obstacles_at(start_time)
if (len(obstacles) > 0 or len(ground_obstacles) > 0):
mAP = pylot.perception.detection.utils.get_mAP(
ground_obstacles, obstacles)
# Get runtime in ms
runtime = (time.time() - op_start_time) * 1000
self._csv_logger.info('{},{},{},{},{:.4f}'.format(
time_epoch_ms(), sim_time, self.config.name, 'runtime',
runtime))
self._logger.info('mAP is: {}'.format(mAP))
self._csv_logger.info('{},{},{},{},{:.4f}'.format(
time_epoch_ms(), sim_time, self.config.name, 'mAP',
mAP))
self._logger.debug('Computing accuracy for {} {}'.format(
end_time, start_time))
else:
# The remaining entries require newer ground obstacles.
break
self.__garbage_collect_obstacles()
def on_watermark(self, timestamp: erdos.Timestamp,
prediction_stream: erdos.WriteStream):
self._logger.debug('@{}: received watermark'.format(timestamp))
if timestamp.is_top:
return
point_cloud_msg = self._point_cloud_msgs.popleft()
tracking_msg = self._tracking_msgs.popleft()
start_time = time.time()
nearby_trajectories, nearby_vehicle_ego_transforms, \
nearby_trajectories_tensor, binned_lidars_tensor = \
self._preprocess_input(tracking_msg, point_cloud_msg)
num_predictions = len(nearby_trajectories)
self._logger.info(
'@{}: Getting R2P2 predictions for {} vehicles'.format(
timestamp, num_predictions))
if num_predictions == 0:
prediction_stream.send(PredictionMessage(timestamp, []))
return
# Run the forward pass.
z = torch.tensor(
np.random.normal(size=(num_predictions,
self._flags.prediction_num_future_steps,
2))).to(torch.float32).to(self._device)
model_start_time = time.time()
prediction_array, _ = self._r2p2_model.forward(
z, nearby_trajectories_tensor, binned_lidars_tensor)
model_runtime = (time.time() - model_start_time) * 1000
self._csv_logger.debug("{},{},{},{:.4f}".format(
time_epoch_ms(), timestamp.coordinates[0],
'r2p2-modelonly-runtime', model_runtime))
prediction_array = prediction_array.cpu().detach().numpy()
obstacle_predictions_list = self._postprocess_predictions(
prediction_array, nearby_trajectories,
nearby_vehicle_ego_transforms)
runtime = (time.time() - start_time) * 1000
self._csv_logger.debug("{},{},{},{:.4f}".format(
time_epoch_ms(), timestamp.coordinates[0], 'r2p2-runtime',
runtime))
prediction_stream.send(
PredictionMessage(timestamp, obstacle_predictions_list))
def __compute_mean_iou(self, timestamp, ground_frame, segmented_frame):
ground_frame.transform_to_cityscapes()
(mean_iou,
class_iou) = ground_frame.compute_semantic_iou(segmented_frame)
self._logger.info('IoU class scores: {}'.format(class_iou))
self._logger.info('mean IoU score: {}'.format(mean_iou))
self._csv_logger.info('{},{},{},{},{:.4f}'.format(
time_epoch_ms(), timestamp.coordinates[0], self.config.name,
self._flags.segmentation_metric, mean_iou))
def on_pose_update(self, msg):
vehicle_location = msg.data.transform.location
x = vehicle_location.x
y = vehicle_location.y
z = vehicle_location.z
self._csv_logger.info('{},{},pose,global,{}'.format(
time_epoch_ms(), msg.timestamp.coordinates[0],
"[{:.4f} {:.4f} {:.4f}]".format(x, y, z)))
def on_msg_camera_stream(self, msg, detected_lanes_stream):
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self._name))
start_time = time.time()
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
image_np = msg.frame.frame
# TODO(ionel): Implement lane detection.
edges = self.apply_canny(image_np)
lines_edges = self.apply_hough(image_np, edges)
kernel_size = 3
grad_x = self.apply_sobel(image_np,
orient='x',
sobel_kernel=kernel_size,
thresh_min=0,
thresh_max=255)
grad_y = self.apply_sobel(image_np,
orient='y',
sobel_kernel=kernel_size,
thresh_min=0,
thresh_max=255)
mag_binary = self.magnitude_threshold(image_np,
sobel_kernel=kernel_size,
thresh_min=0,
thresh_max=255)
dir_binary = self.direction_threshold(image_np,
sobel_kernel=kernel_size,
thresh_min=0,
thresh_max=np.pi / 2)
s_binary = self.extract_s_channel(image_np)
# Select the pixels where both x and y gradients meet the threshold
# criteria, or the gradient magnitude and direction are both with
# the threshold values.
combined = np.zeros_like(dir_binary)
combined[((grad_x == 1) & (grad_y == 1)) | ((mag_binary == 1) &
(dir_binary == 1))] = 1
combined_binary = np.zeros_like(grad_x)
combined_binary[(s_binary == 1) | (grad_x == 1)] = 1
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, msg.timestamp,
runtime))
if self._flags.visualize_lane_detection:
frame = CameraFrame(lines_edges, 'BGR', msg.frame.camera_setup)
frame.visualize(self._name, msg.timestamp)
detected_lanes_stream.send(erdos.Message(msg.timestamp, image_np))
def on_lane_invasion_update(self, msg):
""" Invoked upon receipt of a lane invasion update.
Logs the type of invasion and the timestamp.
Args:
msg (:py:class:`pylot.simulation.messages.LaneInvasionMessage`):
The lane invasion message sent by the sensor.
"""
sim_time = msg.timestamp.coordinates[0]
# We log lane invasion events only if they are illegal.
if any(
map(EvalMetricLoggerOperator.check_illegal_lane_invasion,
msg.lane_markings)):
self._csv_logger.info('{},{},invasion,lane'.format(
time_epoch_ms(), sim_time))
if msg.lane_type == LaneType.SIDEWALK:
self._csv_logger.info('{},{},invasion,sidewalk'.format(
time_epoch_ms(), sim_time))
def compute_accuracy(self, frame_time, ground_time, end_anchored):
anchor_type = "end" if end_anchored else "start"
anchor_time = ground_time if end_anchored else frame_time
predictions = self.get_prediction_at(frame_time)
ground_truths = self.get_ground_truth_at(ground_time)
self.scoring_modules[anchor_type].add_datapoint(
predictions, ground_truths)
new_scores = self.scoring_modules[anchor_type].get_scores()
for k, v in new_scores.items():
self._csv_logger.info("{},{},{},{},{},{},{:.4f}".format(
time_epoch_ms(), anchor_time, self.config.name, anchor_type,
self._matching_policy, k, v))
def on_collision_update(self, msg):
""" Invoked upon receipt of a collision update.
Logs the collided actor and the intensity of the collision.
Args:
msg (:py:class:`pylot.simulation.messages.CollisionMessage`): The
collision message sent by the sensor.
"""
self._csv_logger.info('{},{},collision,{},{:.4f}'.format(
time_epoch_ms(), msg.timestamp.coordinates[0],
msg.collided_actor.split('.')[0], msg.intensity))
def on_traffic_light_invasion_update(self, msg):
""" Invoked upon receipt of a traffic light invasion update.
Logs the timestamp of the invasion.
Args:
msg (:py:class:`pylot.simulation.messages.TrafficInfractionMessage`): # noqa: E501
The traffic infraction message sent by the sensor.
"""
sim_time = msg.timestamp.coordinates[0]
self._csv_logger.info('{},{},invasion,red_light'.format(
time_epoch_ms(), sim_time))
def on_ground_segmented_frame(self, msg, iou_stream):
assert len(msg.timestamp.coordinates) == 1
start_time = time.time()
# We don't fully transform it to cityscapes palette to avoid
# introducing extra latency.
frame = msg.frame
sim_time = msg.timestamp[0]
if len(self._ground_frames) > 0:
# Pop the oldest frame if it's older than the max latency
# we're interested in.
if (msg.timestamp.coordinates[0] - self._ground_frames[0][0] >
self._flags.decay_max_latency):
self._ground_frames.popleft()
cur_time = time_epoch_ms()
for timestamp, ground_frame in self._ground_frames:
(mean_iou, class_iou) = \
ground_frame.compute_semantic_iou_using_masks(frame)
time_diff = msg.timestamp.coordinates[0] - timestamp
self._logger.info(
'Segmentation ground latency {} ; mean IoU {}'.format(
time_diff, mean_iou))
self._csv_logger.info('{},{},{},mIoU,{},{:.4f}'.format(
cur_time, sim_time, self.config.name, time_diff, mean_iou))
iou_stream.send(
erdos.Message(msg.timestamp, (time_diff, mean_iou)))
person_key = 4
if person_key in class_iou:
self._logger.info(
'Segmentation ground latency {} ; person IoU {}'.
format(time_diff, class_iou[person_key]))
self._csv_logger.info(
'{},{},{},personIoU,{},{:.4f}'.format(
cur_time, sim_time, self.config.name, time_diff,
class_iou[person_key]))
vehicle_key = 10
if vehicle_key in class_iou:
self._logger.info(
'Segmentation ground latency {} ; vehicle IoU {}'.
format(time_diff, class_iou[vehicle_key]))
self._csv_logger.info(
'{},{},{},vehicleIoU,{},{:.4f}'.format(
cur_time, sim_time, self.config.name, time_diff,
class_iou[vehicle_key]))
# Append the processed image to the buffer.
self._ground_frames.append((msg.timestamp.coordinates[0], frame))
runtime = (time.time() - start_time) * 1000
self._logger.info(
'Segmentation eval ground runtime {}'.format(runtime))
def on_watermark(self, timestamp: Timestamp):
"""Computes and logs the metrics of accuracy for the control module.
This operator uses two different metrics of accuracy, as follows:
1. Crosstrack error: The distance between the waypoint that the
planner intended the vehicle to achieve, and the location
achieved by the control module.
2. Heading error: The angle between the heading of the vehicle,
and the reference trajectory that the planner intended the
vehicle to follow.
The log format is (timestamp, crosstrack_error, heading_error).
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp
of the watermark.
"""
self._logger.debug('@{}: received watermark.'.format(timestamp))
if timestamp.is_top:
return
# Get the transform of the ego vehicle.
pose_msg = self._pose_messages.popleft()
vehicle_transform = pose_msg.data.transform
if len(self.last_waypoints) == 2:
# Compute the metrics of accuracy using the last two waypoints.
self._logger.debug(
"@{}: vehicle location: {}, waypoints: {}".format(
timestamp, vehicle_transform.location,
self.last_waypoints))
crosstrack_err, heading_err = ControlEvalOperator.\
compute_control_metrics(vehicle_transform,
self.last_waypoints)
self._csv_logger.info("{},{},control,{:.4f},{:.4f}".format(
time_epoch_ms(), timestamp.coordinates[0], crosstrack_err,
heading_err))
# Add the first waypoint from the last waypoints received
# by the operator.
waypoints = self._waypoints_msgs.popleft().waypoints.waypoints
if len(waypoints) > 0:
next_waypoint = waypoints.popleft()
while len(self.last_waypoints) >= 1 and len(
waypoints) > 0 and np.isclose(
next_waypoint.location.distance(
self.last_waypoints[-1].location), 0.0):
next_waypoint = waypoints.popleft()
self.last_waypoints.append(next_waypoint)
def on_frame(self, msg, traffic_lights_stream):
""" Invoked when the operator receives a message on the data stream."""
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self._name))
start_time = time.time()
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
# Expand dimensions since the model expects images to have
# shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(msg.frame.as_rgb_numpy_array(),
axis=0)
(boxes, scores, classes, num) = self._tf_session.run(
[
self._detection_boxes, self._detection_scores,
self._detection_classes, self._num_detections
],
feed_dict={self._image_tensor: image_np_expanded})
num_detections = int(num[0])
labels = [self._labels[label] for label in classes[0][:num_detections]]
boxes = boxes[0][:num_detections]
scores = scores[0][:num_detections]
traffic_lights = self.__convert_to_detected_tl(
boxes, scores, labels, msg.frame.camera_setup.height,
msg.frame.camera_setup.width)
self._logger.debug('@{}: {} detected traffic lights {}'.format(
msg.timestamp, self._name, traffic_lights))
if (self._flags.visualize_detected_traffic_lights
or self._flags.log_traffic_light_detector_output):
msg.frame.annotate_with_bounding_boxes(msg.timestamp,
traffic_lights,
self._bbox_colors)
if self._flags.visualize_detected_traffic_lights:
msg.frame.visualize(self._name)
if self._flags.log_traffic_light_detector_output:
msg.frame.save(msg.timestamp.coordinates[0],
self._flags.data_path,
'tl-detector-{}'.format(self._name))
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, msg.timestamp,
runtime))
traffic_lights_stream.send(
ObstaclesMessage(traffic_lights, msg.timestamp, runtime))
def on_watermark(self, timestamp, control_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
start_time = time.time()
can_bus_msg = self._can_bus_msgs.popleft()
waypoint_msg = self._waypoint_msgs.popleft()
tl_msg = self._traffic_lights_msgs.popleft()
obstacles_msg = self._obstacles_msgs.popleft()
point_cloud_msg = self._point_clouds.popleft()
vehicle_transform = can_bus_msg.data.transform
# Vehicle sped in m/s
vehicle_speed = can_bus_msg.data.forward_speed
wp_angle = waypoint_msg.wp_angle
wp_vector = waypoint_msg.wp_vector
wp_angle_speed = waypoint_msg.wp_angle_speed
target_speed = waypoint_msg.target_speed
transformed_camera_setup = copy.deepcopy(self._camera_setup)
transformed_camera_setup.set_transform(
vehicle_transform * transformed_camera_setup.transform)
traffic_lights = self.__transform_tl_output(
tl_msg, point_cloud_msg.point_cloud, transformed_camera_setup)
(people, vehicles) = self.__transform_detector_output(
obstacles_msg, point_cloud_msg.point_cloud,
transformed_camera_setup)
self._logger.debug('@{}: speed {} and location {}'.format(
timestamp, vehicle_speed, vehicle_transform))
self._logger.debug('@{}: people {}'.format(timestamp, people))
self._logger.debug('@{}: vehicles {}'.format(timestamp, vehicles))
speed_factor, _ = self.__stop_for_agents(vehicle_transform.location,
wp_angle, wp_vector, vehicles,
people, traffic_lights,
timestamp)
control_msg = self.get_control_message(wp_angle, wp_angle_speed,
speed_factor, vehicle_speed,
target_speed, timestamp)
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, timestamp,
runtime))
control_stream.send(control_msg)
def on_frame_msg(self, msg):
""" Invoked when a FrameMessage is received on the camera stream."""
self._logger.debug('@{}: {} received frame'.format(
msg.timestamp, self._name))
start_time = time.time()
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
camera_frame = msg.frame
# Store frames so that they can be re-processed once we receive the
# next update from the detector.
self._to_process.append((msg.timestamp, camera_frame))
# Track if we have a tracker ready to accept new frames.
if self._ready_to_update:
self.__track_bboxes_on_frame(camera_frame, msg.timestamp, False)
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, msg.timestamp,
runtime))
def on_ground_obstacles(self, msg, map_stream):
# Ignore the first several seconds of the simulation because the car is
# not moving at the beginning.
assert len(msg.timestamp.coordinates) == 1
game_time = msg.timestamp.coordinates[0]
bboxes = []
# Select the person bounding boxes.
for obstacle in msg.obstacles:
if obstacle.label == 'person':
bboxes.append(obstacle.bounding_box)
# Remove the buffered bboxes that are too old.
while (len(self._ground_bboxes) > 0
and game_time - self._ground_bboxes[0][0] >
self._flags.decay_max_latency):
self._ground_bboxes.popleft()
sim_time = msg.timestamp.coordinates[0]
for (old_game_time, old_bboxes) in self._ground_bboxes:
# Ideally, we would like to take multiple precision values at
# different recalls and average them, but we can't vary model
# confidence, so we just return the actual precision.
if (len(bboxes) > 0 or len(old_bboxes) > 0):
latency = game_time - old_game_time
precisions = []
for iou in self._iou_thresholds:
(precision,
_) = get_precision_recall_at_iou(bboxes, old_bboxes, iou)
precisions.append(precision)
self._logger.info("Precision {}".format(precisions))
avg_precision = float(sum(precisions)) / len(precisions)
self._logger.info(
"The latency is {} and the average precision is {}".format(
latency, avg_precision))
self._csv_logger.info('{},{},{},{},{:.4f}'.format(
time_epoch_ms(), sim_time, self.config.name, latency,
avg_precision))
map_stream.send(
erdos.Message(msg.timestamp, (latency, avg_precision)))
# Buffer the new bounding boxes.
self._ground_bboxes.append((game_time, bboxes))
def fuse(self):
# Return if we don't have car position, distances or obstacles.
start_time = time.time()
if min(
map(len,
[self._car_positions, self._distances, self._obstacles
])) == 0:
return
self.__discard_old_data()
obstacle_positions = self.__calc_obstacle_positions(
self._obstacles[0][1], self._distances[0][1],
self._car_positions[0][1][0],
np.arccos(self._car_positions[0][1][1][0]))
timestamp = self._obstacles[0][0]
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},{}'.format(time_epoch_ms(), self._name,
runtime))
output_msg = ObstaclePositionsSpeedsMessage(obstacle_positions,
timestamp)
self._fused_stream.send(output_msg)
def compute_depth(self, timestamp, depth_estimation_stream):
self._logger.debug('@{}: {} received watermark'.format(
timestamp, self._name))
start_time = time.time()
imgL = self._left_imgs.pop(timestamp)
imgR = self._right_imgs.pop(timestamp)
cudnn.benchmark = False
self._model.eval()
imgL = imgL.float().cuda().unsqueeze(0)
imgR = imgR.float().cuda().unsqueeze(0)
with torch.no_grad():
outputs = self._model(imgL, imgR)
output = torch.squeeze(outputs[2], 1)
output = output.squeeze().cpu().numpy()
# Process the output (disparity) to depth, model-dependent
depth = preprocess.disp2depth(output)
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, timestamp,
runtime))
if self._flags.visualize_depth_est:
cv2.imshow(self._name, output)
cv2.waitKey(1)
camera_setup = CameraSetup("depth_estimation",
"estimation.anynet",
depth.shape[1],
depth.shape[0],
self._transform,
fov=self._fov)
depth_estimation_stream.send(
DepthFrameMessage(DepthFrame(depth, camera_setup), timestamp))
def on_watermark(self, timestamp, obstacles_error_stream):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
assert len(timestamp.coordinates) == 1
op_start_time = time.time()
game_time = timestamp.coordinates[0]
if not self._last_notification:
self._last_notification = game_time
return
else:
self._sim_interval = (game_time - self._last_notification)
self._last_notification = game_time
sim_time = timestamp.coordinates[0]
while len(self._detector_start_end_times) > 0:
print("INSIDE EVAL WATERMARK", timestamp)
(end_time, start_time) = self._detector_start_end_times[0]
if end_time <= game_time:
heapq.heappop(self._detector_start_end_times)
ego_transform, ground_obstacles = self.__get_ground_obstacles_at(end_time)
#go_labels = []
#for go in ground_obstacles:
# go_labels.append(go.label)
obstacles = self.__get_obstacles_at(start_time)
#do_labels = []
#for do in obstacles:
# do_labels.append(do.label)
#print(timestamp, go_labels, do_labels)
if (len(obstacles) > 0 or len(ground_obstacles) > 0):
errs = pylot.perception.detection.utils.get_errors(
ground_obstacles, obstacles, ego_transform)
# Get runtime in ms
runtime = (time.time() - op_start_time) * 1000
# self._csv_logger.info('{},{},{},{},{:.4f}'.format(
# time_epoch_ms(), sim_time, self.config.name, 'runtime',
# runtime))
self._logger.info('errors calculated')
matchobs = []
for i in range(len(errs)):
ground_ob = errs[i][0]
det_ob = errs[i][1]
err_val = errs[i][2]
det_id = "NONE"
if (det_ob is not None):
det_ob.vis_error = err_val
det_id = det_ob.id
ego_point = ego_transform.location.as_numpy_array().reshape(1, 3)
ego_loc = ego_transform.inverse_transform_points(ego_point).reshape(3,)
ob_actual_point = ground_ob.transform.location.as_numpy_array().reshape(1, 3)
ob_actual_loc = ego_transform.inverse_transform_points(ob_actual_point).reshape(3,)
relative_dist = ob_actual_loc - ego_loc
#print([ground_ob.id, ego_loc, "===", ob_actual_loc, "====", relative_dist])
print([ground_ob.id, det_id, err_val])
self._csv_logger.info('{},{},{},{},{},{:.4f},{:.4f},{:.4f},{:.4f}'.format(
time_epoch_ms(), sim_time, self.config.name, ground_ob.id, ground_ob.label,
relative_dist[0], relative_dist[1], relative_dist[2],
err_val))
self._logger.debug('Computing accuracy for {} {}'.format(
end_time, start_time))
obstacles_error_stream.send(ObstaclesMessage(timestamp, obstacles))
obstacles_error_stream.send(erdos.WatermarkMessage(timestamp))
else:
# The remaining entries require newer ground obstacles.
break
self.__garbage_collect_obstacles()
def on_msg_camera_stream(self, msg, obstacles_stream):
""" Invoked when the operator receives a message on the data stream."""
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self._name))
start_time = time.time()
# The models expect BGR images.
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
# Expand dimensions since the model expects images to have
# shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(msg.frame.frame, axis=0)
(boxes, scores, classes, num_detections) = self._tf_session.run(
[
self._detection_boxes, self._detection_scores,
self._detection_classes, self._num_detections
],
feed_dict={self._image_tensor: image_np_expanded})
num_detections = int(num_detections[0])
res_classes = classes[0][:num_detections]
res_boxes = boxes[0][:num_detections]
res_scores = scores[0][:num_detections]
obstacles = []
for i in range(0, num_detections):
if (res_classes[i] in self._coco_labels and res_scores[i] >=
self._flags.obstacle_detection_min_score_threshold):
obstacles.append(
DetectedObstacle(
BoundingBox2D(
int(res_boxes[i][1] *
msg.frame.camera_setup.width),
int(res_boxes[i][3] *
msg.frame.camera_setup.width),
int(res_boxes[i][0] *
msg.frame.camera_setup.height),
int(res_boxes[i][2] *
msg.frame.camera_setup.height)), res_scores[i],
self._coco_labels[res_classes[i]]))
else:
self._logger.warning('Filtering unknown class: {}'.format(
res_classes[i]))
self._logger.debug('@{}: {} obstacles: {}'.format(
msg.timestamp, self._name, obstacles))
if (self._flags.visualize_detected_obstacles
or self._flags.log_detector_output):
msg.frame.annotate_with_bounding_boxes(msg.timestamp, obstacles,
self._bbox_colors)
if self._flags.visualize_detected_obstacles:
msg.frame.visualize(self._name)
if self._flags.log_detector_output:
msg.frame.save(msg.timestamp.coordinates[0],
self._flags.data_path,
'detector-{}'.format(self._name))
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, msg.timestamp,
runtime))
# Send out obstacles.
obstacles_stream.send(
ObstaclesMessage(obstacles, msg.timestamp, runtime))
def on_waypoints_update(self, msg):
""" Invoked upon receipt of a waypoints message from the pipeline.
This method retrieves the pose message for the timestamp, calculates
the runtime of the pipeline, logs it and saves the waypoints for the
future.
Args:
msg (:py:class:`pylot.planning.messages.WaypointsMessage`): The
waypoints message received for the given timestamp.
"""
waypoint_recv_time = time.time()
self._logger.debug("@{}: received waypoints update.".format(
msg.timestamp))
# Retrieve the game time.
game_time = msg.timestamp.coordinates[0]
# Ensure that a single invocation of the pipeline is happening.
assert self._last_localization_update == game_time, \
"Concurrent Execution of the pipeline."
watermark = erdos.WatermarkMessage(msg.timestamp)
if self._waypoint_num < 10:
self._logger.debug(
"@{}: received waypoint num {}. "
"Skipping because the simulator might not be in sync.".format(
msg.timestamp, self._waypoint_num))
self._first_waypoint = False
self._waypoint_num += 1
# Send a message on the notify stream to ask the simulator to send
# a new sensor stream.
self._pipeline_finish_notify_stream.send(watermark)
return
# Retrieve the pose message for this timestamp.
(pose_msg, pose_recv_time) = self._pose_map[game_time]
# Calculate and log the processing time for this waypoints message.
processing_time = int((waypoint_recv_time - pose_recv_time) * 1000)
self._csv_logger.info('{},{},{},{:.4f}'.format(time_epoch_ms(),
game_time,
'end-to-end-runtime',
processing_time))
# Apply the waypoints at the timestamp + processing time.
applicable_time = game_time + processing_time
if (self._last_highest_applicable_time is None
or self._last_highest_applicable_time < applicable_time):
self._last_highest_applicable_time = applicable_time
self._waypoints.append((applicable_time, msg))
self._logger.debug(
"@{}: waypoints will be applicable at {}".format(
msg.timestamp, applicable_time))
else:
# The last waypoint applicable time was higher, we should purge
# the ones higher than this one and add this entry.
self._logger.debug(
"@{}: Popping the last applicable time: {}".format(
msg.timestamp, self._waypoints[-1][0]))
assert (
self._waypoints.pop()[0] == self._last_highest_applicable_time)
while self._waypoints[-1][0] >= applicable_time:
self._logger.debug(
"@{}: Popping the last applicable time: {}".format(
msg.timestamp, self._waypoints[-1][0]))
self._waypoints.pop()
self._last_highest_applicable_time = applicable_time
self._waypoints.append((applicable_time, msg))
self._logger.debug("@{}: the waypoints were adjusted "
"and will be applicable at {}".format(
msg.timestamp, applicable_time))
# Delete the pose from the map.
self._pose_map.pop(game_time, None)
# Send a message on the notify stream to ask the simulator to send a
# new sensor stream.
self._pipeline_finish_notify_stream.send(watermark)
def on_watermark(self, timestamp, control_stream):
""" The callback function invoked upon receipt of a WatermarkMessage.
The function uses the latest location of the vehicle and drives to the
next waypoint, while doing either a stop or a swerve upon the
detection of a person.
Args:
timestamp: Timestamp for which the WatermarkMessage was received.
control_stream: Output stream on which the callback can write to.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
pose_msg = self._pose_msgs.popleft()
ground_obstacles_msg = self._ground_obstacles_msgs.popleft()
obstacle_msg = self._obstacle_msgs.popleft()
self._logger.debug(
"The vehicle is travelling at a speed of {} m/s.".format(
pose_msg.data.forward_speed))
ego_transform = pose_msg.data.transform
ego_location = ego_transform.location
ego_wp = self._map.get_waypoint(ego_location.as_carla_location())
people_obstacles = [
obstacle for obstacle in ground_obstacles_msg.obstacles
if obstacle.label == 'person'
]
sim_time = timestamp.coordinates[0]
# Heuristic to tell us how far away do we detect the person.
for person in people_obstacles:
person_wp = self._map.get_waypoint(
person.transform.location.as_carla_location(),
project_to_road=False)
if person_wp and person_wp.road_id == ego_wp.road_id:
for obstacle in obstacle_msg.obstacles:
if obstacle.label == 'person':
self._csv_logger.info(
"{},{},{},{:.4f},detected a {} {:.4f} m away".
format(time_epoch_ms(), sim_time, self.config.name,
self.SPEED, obstacle.label,
person.distance(ego_transform)))
self._csv_logger.info(
"{},{},{},{:.4f},vehicle speed {:.4f} m/s".format(
time_epoch_ms(), sim_time, self.config.name,
self.SPEED, pose_msg.data.forward_speed))
# Figure out the location of the ego vehicle and compute the next
# waypoint.
if self._goal_reached or ego_location.distance(
self._goal) <= self.GOAL_DISTANCE:
self._logger.info(
"The distance was {} and we reached the goal.".format(
ego_location.distance(self._goal)))
control_stream.send(
ControlMessage(0.0, 0.0, 1.0, True, False, timestamp))
self._goal_reached = True
else:
person_detected = False
for person in people_obstacles:
person_wp = self._map.get_waypoint(
person.transform.location.as_carla_location(),
project_to_road=False)
if person_wp and ego_location.distance(
person.transform.location) <= self.DETECTION_DISTANCE:
person_detected = True
break
if person_detected and self._flags.avoidance_behavior == 'stop':
control_stream.send(
ControlMessage(0.0, 0.0, 1.0, True, False, timestamp))
return
# Get the waypoint that is SAMPLING_DISTANCE away.
sample_distance = self.SAMPLING_DISTANCE if \
ego_transform.forward_vector.x > 0 else \
-1 * self.SAMPLING_DISTANCE
steer_loc = ego_location + pylot.utils.Location(
x=sample_distance, y=0, z=0)
wp_steer = self._map.get_waypoint(steer_loc.as_carla_location())
in_swerve = False
fwd_vector = wp_steer.transform.get_forward_vector()
waypoint_fwd = [fwd_vector.x, fwd_vector.y, fwd_vector.z]
if person_detected:
# If a pedestrian was detected, make sure we're driving on the
# wrong direction.
if np.dot(ego_transform.forward_vector.as_numpy_array(),
waypoint_fwd) > 0:
# We're not driving in the wrong direction, get left
# lane waypoint.
if wp_steer.get_left_lane():
wp_steer = wp_steer.get_left_lane()
in_swerve = True
else:
# We're driving in the right direction, continue driving.
pass
else:
# The person was not detected, come back from the swerve.
if np.dot(ego_transform.forward_vector.as_numpy_array(),
waypoint_fwd) < 0:
# We're driving in the wrong direction, get the left lane
# waypoint.
if wp_steer.get_left_lane():
wp_steer = wp_steer.get_left_lane()
in_swerve = True
else:
# We're driving in the right direction, continue driving.
pass
# self._world.debug.draw_point(wp_steer.transform.location,
# size=0.2,
# life_time=30000.0)
wp_steer_vector, _, wp_steer_angle = \
ego_transform.get_vector_magnitude_angle(
pylot.utils.Location.from_carla_location(
wp_steer.transform.location))
current_speed = max(0, pose_msg.data.forward_speed)
steer = pylot.control.utils.radians_to_steer(
wp_steer_angle, self._flags.steer_gain)
# target_speed = self.SPEED if not in_swerve else self.SPEED / 5.0
target_speed = self.SPEED
throttle, brake = pylot.control.utils.compute_throttle_and_brake(
self._pid, current_speed, target_speed, self._flags)
control_stream.send(
ControlMessage(steer, throttle, brake, False, False,
timestamp))
def on_watermark(self, timestamp):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
assert len(timestamp.coordinates) == 1
op_start_time = time.time()
game_time = timestamp.coordinates[0]
if not self._last_notification:
self._last_notification = game_time
return
else:
self._sim_interval = (game_time - self._last_notification)
self._last_notification = game_time
sim_time = timestamp.coordinates[0]
while len(self._tracker_start_end_times) > 0:
(end_time, start_time) = self._tracker_start_end_times[0]
# We can compute tracker metrics if the endtime is not greater than
# the ground time.
if end_time <= game_time:
# This is the closest ground bounding box to the end time.
heapq.heappop(self._tracker_start_end_times)
ground_obstacles = self.__get_ground_obstacles_at(end_time)
# Get tracker output obstacles.
tracker_obstacles = self.__get_tracked_obstacles_at(start_time)
if (len(tracker_obstacles) > 0 or len(ground_obstacles) > 0):
metrics_summary_df = self.get_tracker_metrics(
tracker_obstacles, ground_obstacles)
# Get runtime in ms
runtime = (time.time() - op_start_time) * 1000
self._csv_logger.info('{},{},{},{},{}'.format(
time_epoch_ms(), sim_time, self.config.name, 'runtime',
runtime))
# Write metrics to csv log file
for metric_name in self._flags.tracking_metrics:
if metric_name in metrics_summary_df.columns:
if (metric_name == 'mostly_tracked'
or metric_name == 'mostly_lost'
or metric_name == 'partially_tracked'):
ratio = metrics_summary_df[metric_name].values[
0] / metrics_summary_df[
'num_unique_objects'].values[0]
self._csv_logger.info(
"{},{},{},{},{:.4f}".format(
time_epoch_ms(), sim_time,
self.config.name,
'ratio_' + metric_name, ratio))
elif metric_name == 'motp':
# See https://github.com/cheind/py-motmetrics/issues/92 # noqa: E501
motp = (1 - metrics_summary_df[metric_name].
values[0]) * 100
self._csv_logger.info(
'{},{},{},{},{:.4f}'.format(
time_epoch_ms(), sim_time,
self.config.name, metric_name, motp))
elif (metric_name == 'idf1'
or metric_name == 'mota'):
metric_val = metrics_summary_df[
metric_name].values[0] * 100
self._csv_logger.info(
'{},{},{},{},{:.4f}'.format(
time_epoch_ms(), sim_time,
self.config.name, metric_name,
metric_val))
else:
self._csv_logger.info(
'{},{},{},{},{:.4f}'.format(
time_epoch_ms(), sim_time,
self.config.name, metric_name,
metrics_summary_df[metric_name].
values[0]))
else:
raise ValueError(
'Unexpected tracking metric: {}'.format(
metric_name))
self._logger.debug('Computing accuracy for {} {}'.format(
end_time, start_time))
else:
# The remaining entries require newer ground obstacles.
break
self.__garbage_collect_obstacles()
def on_waypoints_update(self, msg):
""" Invoked upon receipt of a waypoints message from the pipeline.
This method retrieves the pose message for the timestamp, calculates
the runtime of the pipeline, logs it and saves the waypoints for the
future.
Args:
msg (:py:class:`pylot.planning.messages.WaypointsMessage`): The
waypoints message received for the given timestamp.
"""
waypoint_recv_time = time.time()
self._logger.debug("@{}: received waypoints update.".format(
msg.timestamp))
if self._first_waypoint:
self._logger.debug(
"@{}: received first waypoint. "
"Skipping because the simulator might not be in sync.".format(
msg.timestamp))
self._first_waypoint = False
return
# Retrieve the game time.
game_time = msg.timestamp.coordinates[0]
# Retrieve the pose message for this timestamp.
(pose_msg, pose_recv_time) = self._pose_map[game_time]
# Calculate and log the processing time for this waypoints message.
processing_time = int((waypoint_recv_time - pose_recv_time) * 1000)
self._csv_logger.info('{},{},{},{:.4f}'.format(time_epoch_ms(),
game_time,
'end-to-end-runtime',
processing_time))
# Apply the waypoints at the timestamp + processing time.
applicable_time = game_time + processing_time
if (self._last_highest_applicable_time is None
or self._last_highest_applicable_time < applicable_time):
self._last_highest_applicable_time = applicable_time
self._waypoints.append((applicable_time, msg))
self._logger.debug(
"@{}: waypoints will be applicable at {}".format(
msg.timestamp, applicable_time))
else:
# We add the applicable time to the time between localization
# readings, and put these waypoints at that location.
sensor_frequency = self._flags.carla_fps
if self._flags.carla_localization_frequency != -1:
sensor_frequency = self._flags.carla_localization_frequency
applicable_time = self._last_highest_applicable_time + int(
1000 / sensor_frequency)
self._last_highest_applicable_time = applicable_time
self._waypoints.append((applicable_time, msg))
self._logger.debug(
"@{}: the waypoints were adjusted by the localization "
"frequency and will be applicable at {}".format(
msg.timestamp, applicable_time))
# Delete the pose from the map.
del self._pose_map[game_time]
def _calculate_metrics(self, timestamp, ground_trajectories, predictions):
""" Calculates and logs MSD (mean squared distance), ADE (average
displacement error), and FDE (final displacement error).
Args:
ground_trajectories: A dict of perfect past trajectories.
predictions: A list of obstacle predictions.
"""
# Vehicle metrics.
vehicle_cnt = 0
vehicle_msd = 0.0
vehicle_ade = 0.0
vehicle_fde = 0.0
# Person metrics.
person_cnt = 0
person_msd = 0.0
person_ade = 0.0
person_fde = 0.0
for obstacle in predictions:
# We remove altitude from the accuracy calculation because the
# prediction operators do not currently predict altitude.
predicted_trajectory = [
Vector2D(transform.location.x, transform.location.y)
for transform in obstacle.trajectory
]
ground_trajectory = [
Vector2D(transform.location.x, transform.location.y)
for transform in ground_trajectories[obstacle.id].trajectory
]
if obstacle.label in VEHICLE_LABELS:
vehicle_cnt += 1
elif obstacle.label == 'person':
person_cnt += 1
else:
raise ValueError('Unexpected obstacle label {}'.format(
obstacle.label))
l2_distance = 0.0
l1_distance = 0.0
for idx in range(1, len(predicted_trajectory) + 1):
# Calculate MSD
l2_distance += predicted_trajectory[-idx].l2_distance(
ground_trajectory[-idx])
# Calculate ADE
l1_distance += predicted_trajectory[-idx].l1_distance(
ground_trajectory[-idx])
l2_distance /= len(predicted_trajectory)
l1_distance /= len(predicted_trajectory)
fde = predicted_trajectory[-1].l1_distance(ground_trajectory[-1])
if obstacle.label in VEHICLE_LABELS:
vehicle_msd += l2_distance
vehicle_ade += l1_distance
vehicle_fde += fde
elif obstacle.label == 'person':
person_msd += l2_distance
person_ade += l1_distance
person_fde += fde
else:
raise ValueError('Unexpected obstacle label {}'.format(
obstacle.label))
# Log metrics.
sim_time = timestamp.coordinates[0]
actor_cnt = person_cnt + vehicle_cnt
if actor_cnt > 0:
msd = (person_msd + vehicle_msd) / actor_cnt
ade = (person_ade + vehicle_ade) / actor_cnt
fde = (person_fde + vehicle_fde) / actor_cnt
self._csv_logger.info('{},{},prediction,MSD,{:.4f}'.format(
time_epoch_ms(), sim_time, msd))
self._csv_logger.info('{},{},prediction,ADE,{:.4f}'.format(
time_epoch_ms(), sim_time, ade))
self._csv_logger.info('{},{},prediction,FDE,{:.4f}'.format(
time_epoch_ms(), sim_time, fde))
if person_cnt > 0:
person_msd /= person_cnt
person_ade /= person_cnt
person_fde /= person_cnt
self._logger.info('Person MSD is: {:.4f}'.format(person_msd))
self._logger.info('Person ADE is: {:.4f}'.format(person_ade))
self._logger.info('Person FDE is: {:.4f}'.format(person_fde))
self._csv_logger.info('{},{},prediction,person-MSD,{:.4f}'.format(
time_epoch_ms(), sim_time, person_msd))
self._csv_logger.info('{},{},prediction,person-ADE,{:.4f}'.format(
time_epoch_ms(), sim_time, person_ade))
self._csv_logger.info('{},{},prediction,person-FDE,{:.4f}'.format(
time_epoch_ms(), sim_time, person_fde))
if vehicle_cnt > 0:
vehicle_msd /= vehicle_cnt
vehicle_ade /= vehicle_cnt
vehicle_fde /= vehicle_cnt
self._logger.info('Vehicle MSD is: {:.4f}'.format(vehicle_msd))
self._logger.info('Vehicle ADE is: {:.4f}'.format(vehicle_ade))
self._logger.info('Vehicle FDE is: {:.4f}'.format(vehicle_fde))
self._csv_logger.info('{},{},prediction,vehicle-MSD,{:.4f}'.format(
time_epoch_ms(), sim_time, vehicle_msd))
self._csv_logger.info('{},{},prediction,vehicle-ADE,{:.4f}'.format(
time_epoch_ms(), sim_time, vehicle_ade))
self._csv_logger.info('{},{},prediction,vehicle-FDE,{:.4f}'.format(
time_epoch_ms(), sim_time, vehicle_fde))
def calculate_metrics(self, ground_trajectories, predictions):
""" Calculates and logs MSD (mean squared distance), ADE (average
displacement error), and FDE (final displacement error).
Args:
ground_trajectories: A dict of perfect past trajectories.
predictions: A list of obstacle predictions.
"""
# Vehicle metrics.
vehicle_cnt = 0
vehicle_msd = 0.0
vehicle_ade = 0.0
vehicle_fde = 0.0
# Person metrics.
person_cnt = 0
person_msd = 0.0
person_ade = 0.0
person_fde = 0.0
for obstacle in predictions:
predicted_trajectory = obstacle.trajectory
ground_trajectory = ground_trajectories[obstacle.id].trajectory
if obstacle.label == 'vehicle':
vehicle_cnt += 1
elif obstacle.label == 'person':
person_cnt += 1
else:
raise ValueError('Unexpected obstacle label {}'.format(
obstacle.label))
l2_distance = 0.0
l1_distance = 0.0
for idx in range(1, len(predicted_trajectory) + 1):
# Calculate MSD
l2_distance += predicted_trajectory[-idx].location.distance(
ground_trajectory[-idx].location)
# Calculate ADE
l1_distance += predicted_trajectory[-idx].location.l1_distance(
ground_trajectory[-idx].location)
l2_distance /= len(predicted_trajectory)
l1_distance /= len(predicted_trajectory)
fde = predicted_trajectory[-1].location.l1_distance(
ground_trajectory[-1].location)
if obstacle.label == 'vehicle':
vehicle_msd += l2_distance
vehicle_ade += l1_distance
vehicle_fde += fde
elif obstacle.label == 'person':
person_msd += l2_distance
person_ade += l1_distance
person_fde += fde
else:
raise ValueError('Unexpected obstacle label {}'.format(
obstacle.label))
vehicle_msd /= vehicle_cnt
vehicle_ade /= vehicle_cnt
vehicle_fde /= vehicle_cnt
person_msd /= person_cnt
person_ade /= person_cnt
person_fde /= person_cnt
# Log metrics.
self._logger.info('Vehicle MSD is: {}'.format(vehicle_msd))
self._logger.info('Vehicle ADE is: {}'.format(vehicle_ade))
self._logger.info('Vehicle FDE is: {}'.format(vehicle_fde))
self._logger.info('Person MSD is: {}'.format(person_msd))
self._logger.info('Person ADE is: {}'.format(person_ade))
self._logger.info('Person FDE is: {}'.format(person_fde))
self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self._name,
'vehicle-MSD', vehicle_msd))
self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self._name,
'vehicle-ADE', vehicle_ade))
self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self._name,
'vehicle-FDE', vehicle_fde))
self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self._name,
'person-MSD', person_msd))
self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self._name,
'person-ADE', person_ade))
self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self._name,
'person-FDE', person_fde))
