def associate_data(root_map: typing.Mapping,
*args: typing.Mapping) -> typing.List[typing.List]:
"""
Convert a number of maps key->value to a list of lists
[[key, map1[key], map2[key] map3[key] ...] ...]
The list will be sorted in key order
Returned inner lists will be in the same order as they are passed as arguments.
The first map passed is considered the reference point for the list of keys,
:param root_map: The first map to associate
:param args: Additional maps to associate to the first one
:return:
"""
if len(args) <= 0:
# Nothing to associate, flatten the root map and return
return sorted([k, v] for k, v in root_map.items())
root_keys = set(root_map.keys())
all_same = True
# First, check if all the maps have the same list of keys
for other_map in args:
if set(other_map.keys()) != root_keys:
all_same = False
break
if all_same:
# All the maps have the same set of keys, just flatten them
return sorted([key, root_map[key]] +
[other_map[key] for other_map in args]
for key in root_keys)
else:
# We need to associate the maps, the timestamps are a little out
rekeyed_maps = []
for other_map in args:
matches = ass.associate(root_map,
other_map,
offset=0,
max_difference=3)
rekeyed_map = {
root_key: other_map[other_key]
for root_key, other_key in matches
}
root_keys &= set(rekeyed_map.keys())
rekeyed_maps.append(rekeyed_map)
return sorted([key, root_map[key]] +
[rekeyed_map[key] for rekeyed_map in rekeyed_maps]
for key in root_keys)
def _plot_error_vs_motion(self,
db_client: arvet.database.client.DatabaseClient):
import matplotlib.pyplot as pyplot
save_path = os.path.join('figures',
type(self).__name__, 'errors vs motions')
os.makedirs(save_path, exist_ok=True)
logging.getLogger(__name__).info(
"Plotting error vs time and saving to {0} ...".format(save_path))
for dataset_name, dataset_id in self.datasets.items():
for system_name, system_id in self.systems.items():
logging.getLogger(__name__).info(
" .... distributions for {0} on {1}".format(
system_name, dataset_name))
times = []
motion_distances = []
rotation_angles = []
motion_errors = []
mean_motion_errors = []
rotation_errors = []
mean_rotation_errors = []
motion_noise = []
rotation_noise = []
trial_result_list = self.get_trial_results(
system_id, dataset_id)
ground_truth_motions = None
gt_scale = None
computed_motion_sequences = []
timestamps = []
if len(trial_result_list) <= 0:
continue
# Collect all the trial results
for trial_result_id in trial_result_list:
trial_result = dh.load_object(db_client,
db_client.trials_collection,
trial_result_id)
if trial_result is not None and trial_result.success:
if ground_truth_motions is None:
ground_truth_motions = trial_result.get_ground_truth_camera_poses(
)
gt_scale = old_th.find_trajectory_scale(
ground_truth_motions)
ground_truth_motions = old_th.trajectory_to_motion_sequence(
ground_truth_motions)
traj = trial_result.get_computed_camera_poses()
# Normalize monocular trajectories
if 'mono' in system_name.lower():
if gt_scale is not None:
traj = old_th.rescale_trajectory(
traj, gt_scale)
else:
logging.getLogger(__name__).warning(
"Cannot rescale trajectory, missing ground truth"
)
computed_motion_sequences.append(
old_th.trajectory_to_motion_sequence(traj))
timestamps.append({
k: v
for k, v in ass.associate(ground_truth_motions,
traj,
max_difference=0.1,
offset=0)
})
# Now that we have all the trajectories, we can measure consistency
for time in sorted(ground_truth_motions.keys()):
if sum(1 for idx in range(len(timestamps))
if time in timestamps[idx]) <= 1:
# Skip locations/results which appear in only one trajectory
continue
# Find the mean estimated motion for this time
computed_motions = [
computed_motion_sequences[idx][timestamps[idx]
[time]].location
for idx in range(len(computed_motion_sequences))
if time in timestamps[idx] and timestamps[idx][time] in
computed_motion_sequences[idx]
]
computed_rotations = [
computed_motion_sequences[idx][
timestamps[idx][time]].rotation_quat(True)
for idx in range(len(computed_motion_sequences))
if time in timestamps[idx] and timestamps[idx][time] in
computed_motion_sequences[idx]
]
assert len(computed_motions) == len(computed_rotations)
if len(computed_motions) > 0:
mean_computed_motion = np.mean(computed_motions,
axis=0)
mean_computed_rotation = data_helpers.quat_mean(
computed_rotations)
times += [time for _ in range(len(computed_motions))]
motion_distances += [
np.linalg.norm(ground_truth_motions[time].location)
for _ in range(len(computed_motions))
]
rotation_angles += [
data_helpers.quat_angle(
ground_truth_motions[time].rotation_quat(True))
for _ in range(len(computed_rotations))
]
motion_errors += [
np.linalg.norm(computed_motion -
ground_truth_motions[time].location)
for computed_motion in computed_motions
]
motion_noise += [
np.linalg.norm(computed_motion -
mean_computed_motion)
for computed_motion in computed_motions
]
mean_motion_errors += [
np.linalg.norm(mean_computed_motion -
ground_truth_motions[time].location)
for _ in range(len(computed_motions))
]
rotation_errors += [
data_helpers.quat_diff(
computed_rotation,
ground_truth_motions[time].rotation_quat(True))
for computed_rotation in computed_rotations
]
rotation_noise += [
data_helpers.quat_diff(computed_rotation,
mean_computed_rotation)
for computed_rotation in computed_rotations
]
mean_rotation_errors += [
data_helpers.quat_diff(
mean_computed_rotation,
ground_truth_motions[time].rotation_quat(True))
for _ in range(len(computed_rotations))
]
# Plot every combination of motion vs error as a plot and a heatmap
figure, axes = pyplot.subplots(12,
2,
squeeze=False,
figsize=(14, 66),
dpi=80)
fig_title = '{0} on {1} errors vs motions.png'.format(
system_name, dataset_name)
figure.suptitle(fig_title)
plot_idx = 0
for x_title_name, x_axis_name, x_data in [
('motion', 'distance moved (m)',
np.array(motion_distances)),
('rotation', 'angle rotated (rad)',
np.array(rotation_angles))
]:
for y_title_name, y_axis_name, y_data in [
('motion error', 'motion error (m)',
np.array(motion_errors)),
('motion noise', 'motion noise (m)',
np.array(motion_noise)),
('mean motion error', 'mean motion error (m)',
np.array(mean_motion_errors)),
('rotation error', 'rotation error (rad)',
np.array(rotation_errors)),
('rotation noise', 'rotation noise (rad)',
np.array(rotation_noise)),
('mean rotation error', 'mean rotation error (rad)',
np.array(mean_rotation_errors)),
]:
x_limits = data_helpers.compute_window(
x_data, std_deviations=4)
y_limits = data_helpers.compute_window(
y_data, std_deviations=4)
x_outliers = data_helpers.compute_outliers(
x_data, x_limits)
y_outliers = data_helpers.compute_outliers(
y_data, y_limits)
ax = axes[plot_idx][0]
ax.set_title("{0} vs {1}".format(
y_title_name, x_title_name))
ax.set_xlim(x_limits)
ax.set_ylim(y_limits)
ax.set_xlabel(x_axis_name +
" ({0} outliers)".format(x_outliers))
ax.set_ylabel(y_axis_name +
" ({0} outliers)".format(y_outliers))
ax.plot(x_data,
y_data,
c='blue',
alpha=0.5,
marker='.',
markersize=2,
linestyle='None')
ax = axes[plot_idx][1]
ax.set_title("{0} vs {1} histogram".format(
y_title_name, x_title_name))
ax.set_xlabel(x_axis_name +
" ({0} outliers)".format(x_outliers))
ax.set_ylabel(y_axis_name +
" ({0} outliers)".format(y_outliers))
heatmap, xedges, yedges = np.histogram2d(
x_data,
y_data,
bins=300,
range=[x_limits, y_limits])
ax.imshow(heatmap.T,
extent=[
xedges[0], xedges[-1], yedges[0],
yedges[-1]
],
origin='lower',
aspect='auto',
cmap=pyplot.get_cmap('inferno_r'))
plot_idx += 1
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.98, right=0.99)
figure.savefig(os.path.join(save_path, fig_title + '.png'))
pyplot.close(figure)
def _create_error_vs_same_direction_motion_plot(
self, db_client: arvet.database.client.DatabaseClient):
import matplotlib.pyplot as pyplot
save_path = os.path.join(
type(self).get_output_folder(), 'error vs motion heatmaps')
os.makedirs(save_path, exist_ok=True)
logging.getLogger(__name__).info(
"Plotting error vs motion and saving to {0} ...".format(save_path))
for dataset_name, dataset_id in self.datasets.items():
for system_name, system_id in self.systems.items():
logging.getLogger(__name__).info(
" .... error vs motion heatmaps for {0} on {1}".format(
system_name, dataset_name))
# Collect errors from all the trial resuls
forward_motion = []
sideways_motion = []
vertical_motion = []
total_motion = []
forward_error = []
sideways_error = []
vertical_error = []
total_error = []
trial_result_list = self.get_trial_results(
system_id, dataset_id)
for trial_result_id in trial_result_list:
trial_result = dh.load_object(db_client,
db_client.trials_collection,
trial_result_id)
if trial_result is not None and trial_result.success:
ground_truth_motions = trial_result.get_ground_truth_motions(
)
computed_motions = trial_result.get_computed_camera_motions(
)
matches = ass.associate(ground_truth_motions,
computed_motions,
offset=0,
max_difference=0.1)
for match in matches:
gt_motion = ground_truth_motions[match[0]].location
error = gt_motion - computed_motions[
match[1]].location
forward_motion.append(gt_motion[0])
sideways_motion.append(gt_motion[1])
vertical_motion.append(gt_motion[2])
total_motion.append(np.linalg.norm(gt_motion))
forward_error.append(error[0])
sideways_error.append(error[1])
vertical_error.append(error[2])
total_error.append(np.linalg.norm(error))
if len(total_error) <= 0:
# Make sure we have some data to plot
continue
# Plot error vs motion in that direction
title = "{0} on {1} error vs motion".format(
system_name, dataset_name)
figure, axes = pyplot.subplots(1, 4, figsize=(40, 10), dpi=80)
figure.suptitle(title)
ax = axes[0]
ax.set_title('forward')
ax.set_xlabel('motion (m)')
ax.set_ylabel('error (m)')
heatmap, xedges, yedges = np.histogram2d(forward_motion,
forward_error,
bins=300)
ax.imshow(
heatmap.T,
extent=[xedges[0], xedges[-1], yedges[0], yedges[-1]],
origin='lower',
aspect='auto',
cmap=pyplot.get_cmap('magma_r'))
ax = axes[1]
ax.set_title('sideways')
ax.set_xlabel('motion (m)')
ax.set_ylabel('error (m)')
heatmap, xedges, yedges = np.histogram2d(sideways_motion,
sideways_error,
bins=300)
ax.imshow(
heatmap.T,
extent=[xedges[0], xedges[-1], yedges[0], yedges[-1]],
origin='lower',
aspect='auto',
cmap=pyplot.get_cmap('magma_r'))
ax = axes[2]
ax.set_title('vertical')
ax.set_xlabel('motion (m)')
ax.set_ylabel('error (m)')
heatmap, xedges, yedges = np.histogram2d(vertical_motion,
vertical_error,
bins=300)
ax.imshow(
heatmap.T,
extent=[xedges[0], xedges[-1], yedges[0], yedges[-1]],
origin='lower',
aspect='auto',
cmap=pyplot.get_cmap('magma_r'))
ax = axes[3]
ax.set_title('total')
ax.set_xlabel('motion (m)')
ax.set_ylabel('error (m)')
heatmap, xedges, yedges = np.histogram2d(total_motion,
total_error,
bins=300)
ax.imshow(
heatmap.T,
extent=[xedges[0], xedges[-1], yedges[0], yedges[-1]],
origin='lower',
aspect='auto',
cmap=pyplot.get_cmap('magma_r'))
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.90, right=0.99)
figure.savefig(os.path.join(save_path, title + '.png'))
pyplot.close(figure)
def _create_error_distribution_plots(
self, db_client: arvet.database.client.DatabaseClient):
import matplotlib.pyplot as pyplot
noise_save_path = os.path.join(
type(self).get_output_folder(), 'error distribution')
os.makedirs(noise_save_path, exist_ok=True)
logging.getLogger(__name__).info(
"Plotting error distributions to {0} ...".format(noise_save_path))
for system_name, system_id in self.systems.items():
logging.getLogger(__name__).info(
" .... plotting for {0}".format(system_name))
for dataset_name, dataset_id in self.datasets.items():
all_computed_motions = []
ground_truth_motions = None
trial_result_list = self.get_trial_results(
system_id, dataset_id)
if len(trial_result_list) <= 0:
continue
# Collect all the computed motions
for trial_result_id in trial_result_list:
trial_result = dh.load_object(db_client,
db_client.trials_collection,
trial_result_id)
if trial_result is not None and trial_result.success:
if ground_truth_motions is None:
ground_truth_motions = trial_result.get_ground_truth_motions(
)
all_computed_motions.append(
trial_result.get_computed_camera_motions())
# Collect statistics on the error for each frame motion
times = []
trans_error = []
rot_error = []
for computed_motions in all_computed_motions:
matches = ass.associate(ground_truth_motions,
computed_motions,
offset=0,
max_difference=0.1)
for match in matches:
times.append(match[0])
trans_error.append(
computed_motions[match[1]].location -
ground_truth_motions[match[0]].location)
rot_error.append(
tf.quat_diff(
computed_motions[match[1]].rotation_quat(True),
ground_truth_motions[match[0]].rotation_quat(
True)))
trans_error = np.array(trans_error)
error_magnitudes = np.linalg.norm(trans_error, axis=1)
# Plot translational noise histograms
title = "{0} on {1} translational error distribution".format(
system_name, dataset_name)
figure, axes = pyplot.subplots(1, 2, figsize=(20, 8), dpi=80)
figure.suptitle(title)
ax = axes[0]
ax.set_title('per-axis error distribution')
ax.set_xlabel('noise (m)')
ax.set_ylabel('density')
for data, colour in [(trans_error[:, 0], 'red'),
(trans_error[:, 1], 'green'),
(trans_error[:, 2], 'blue')]:
ax.hist(data,
density=True,
bins=300,
alpha=0.3,
color=colour)
ax = axes[1]
ax.set_title('total error distribution')
ax.set_xlabel('noise (m)')
ax.set_ylabel('density')
ax.hist(error_magnitudes, density=True, bins=300, color='blue')
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.90, right=0.99)
figure.savefig(os.path.join(noise_save_path, title + '.png'))
pyplot.close(figure)
# Plot rotational noise histograms
title = "{0} on {1} error distribution".format(
system_name, dataset_name)
figure, axes = pyplot.subplots(1, 1, figsize=(16, 8), dpi=80)
figure.suptitle(title)
axes.set_xlabel('noise (rad)')
axes.set_ylabel('density')
axes.hist(rot_error, density=True, bins=300, color='blue')
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.95, right=0.99)
figure.savefig(os.path.join(noise_save_path, title + '.png'))
pyplot.close(figure)
# Plot noise vs time
title = "{0} on {1} noise vs time".format(
system_name, dataset_name)
figure, axes = pyplot.subplots(2, 1, figsize=(18, 10), dpi=80)
figure.suptitle(title)
ax = axes[0]
ax.set_xlabel('time (s)')
ax.set_ylabel('noise distance (m)')
ax.plot(times,
error_magnitudes,
c='blue',
alpha=0.5,
marker='.',
markersize=2,
linestyle='None')
ax = axes[1]
ax.set_xlabel('time (s)')
ax.set_ylabel('noise angle (rad)')
ax.plot(times,
rot_error,
c='blue',
alpha=0.5,
marker='.',
markersize=2,
linestyle='None')
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.95, right=0.99)
figure.savefig(os.path.join(noise_save_path, title + '.png'))
pyplot.close(figure)
def _create_absolute_error_plot(
self, db_client: arvet.database.client.DatabaseClient):
import matplotlib.pyplot as pyplot
noise_save_path = os.path.join(
type(self).get_output_folder(), 'absolute error distribution')
os.makedirs(noise_save_path, exist_ok=True)
logging.getLogger(__name__).info(
"Plotting absolute error distributions to {0} ...".format(
noise_save_path))
for system_name, system_id in self.systems.items():
logging.getLogger(__name__).info(
" .... plotting for {0}".format(system_name))
for dataset_name, dataset_id in self.datasets.items():
trial_result_list = self.get_trial_results(
system_id, dataset_id)
if len(trial_result_list) <= 0:
continue
# Collect error measurements for all trials
times = []
x_error = []
y_error = []
z_error = []
error_magnitude = []
error_angle = []
for trial_result_id in trial_result_list:
trial_result = dh.load_object(db_client,
db_client.trials_collection,
trial_result_id)
if trial_result is not None and trial_result.success:
ground_truth_trajectory = trial_result.get_ground_truth_camera_poses(
)
computed_trajectory = trial_result.get_computed_camera_poses(
)
matches = ass.associate(ground_truth_trajectory,
computed_trajectory,
offset=0,
max_difference=0.1)
for match in matches:
error = computed_trajectory[
match[1]].location - ground_truth_trajectory[
match[0]].location
times.append(match[0])
x_error.append(error[0])
y_error.append(error[1])
z_error.append(error[2])
error_magnitude.append(np.linalg.norm(error))
error_angle.append(
tf.quat_diff(
computed_trajectory[
match[1]].rotation_quat(True),
ground_truth_trajectory[
match[0]].rotation_quat(True)))
# Plot error vs motion in that direction
title = "{0} on {1} noise distribution".format(
system_name, dataset_name)
figure, axes = pyplot.subplots(1, 5, figsize=(40, 8), dpi=80)
figure.suptitle(title)
ax = axes[0]
ax.set_title('x absolute error distribution')
ax.set_xlabel('error (m)')
ax.set_ylabel('Probability')
ax.hist(x_error, density=True, bins=300, color='blue')
ax = axes[1]
ax.set_title('y absolute error distribution')
ax.set_xlabel('error (m)')
ax.set_ylabel('Probability')
ax.hist(y_error, density=True, bins=300, color='blue')
ax = axes[2]
ax.set_title('z absolute error distribution')
ax.set_xlabel('error (m)')
ax.set_ylabel('Probability')
ax.hist(z_error, density=True, bins=300, color='blue')
ax = axes[3]
ax.set_title('total absolute error distribution')
ax.set_xlabel('error (m)')
ax.set_ylabel('Probability')
ax.hist(error_magnitude, density=True, bins=300, color='blue')
ax = axes[4]
ax.set_title('angle error distribution')
ax.set_xlabel('angle error (rad)')
ax.set_ylabel('Probability')
ax.hist(error_angle, density=True, bins=300, color='blue')
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.90, right=0.99)
figure.savefig(os.path.join(noise_save_path, title + '.png'))
pyplot.close(figure)
# Plot noise vs time
title = "{0} on {1} error vs time".format(
system_name, dataset_name)
figure, axes = pyplot.subplots(2, 1, figsize=(18, 10), dpi=80)
figure.suptitle(title)
ax = axes[0]
ax.set_xlabel('time (s)')
ax.set_ylabel('error magnitude (m)')
ax.plot(times,
error_magnitude,
c='blue',
alpha=0.5,
marker='.',
markersize=2,
linestyle='None')
ax = axes[1]
ax.set_xlabel('time (s)')
ax.set_ylabel('error angle (rad)')
ax.plot(times,
error_angle,
c='blue',
alpha=0.5,
marker='.',
markersize=2,
linestyle='None')
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.95, right=0.99)
figure.savefig(os.path.join(noise_save_path, title + '.png'))
pyplot.close(figure)