def run_task(self, path_manager: arvet.config.path_manager.PathManager,
db_client: arvet.database.client.DatabaseClient):
import logging
import traceback
import arvet.util.database_helpers as dh
trainer = dh.load_object(db_client, db_client.trainer_collection,
self.trainer)
trainee = dh.load_object(db_client, db_client.trainee_collection,
self.trainee)
if trainer is None:
logging.getLogger(__name__).error(
"Could not deserialize trainer {0}".format(self.trainer))
self.mark_job_failed()
elif trainee is None:
logging.getLogger(__name__).error(
"Could not deserialize trainee {0}".format(self.trainee))
self.mark_job_failed()
elif not trainer.can_train_trainee(trainee):
logging.getLogger(__name__).error(
"Trainer {0} cannot train trainee {1}".format(
self.trainer, self.trainee))
self.mark_job_failed()
else:
logging.getLogger(__name__).info(
"Start training trainee {0} ({1}) with trainer {2} (3)".format(
self.trainee,
entity_registry.get_type_name(type(trainee)), self.trainer,
entity_registry.get_type_name(type(trainer))))
try:
system = trainer.train_vision_system(trainee)
except Exception as exception:
logging.getLogger(__name__).error(
"Error occurred while trainer {0} trains trainee {1}:\n{2}"
.format(self.trainer, self.trainee,
traceback.format_exc()))
self.mark_job_failed()
raise exception
if system is None:
logging.getLogger(__name__).error(
"Failed to train trainee {0} with trainer {1}".format(
self.trainer, self.trainee))
self.mark_job_failed()
else:
system_id = db_client.system_collection.insert(
system.serialize())
logging.getLogger(__name__).info(
"Successfully trained system {0}".format(system_id))
self.mark_job_complete(system_id)
def _create_axis_plot(self,
db_client: arvet.database.client.DatabaseClient):
save_path = os.path.join(
type(self).get_output_folder(), 'axis vs time')
os.makedirs(save_path, exist_ok=True)
logging.getLogger(__name__).info(
"Plotting axes vs time to {0} ...".format(save_path))
colours = [
'orange', 'cyan', 'gold', 'magenta', 'green', 'brown', 'purple',
'red', 'navy', 'darkkhaki', 'darkgreen', 'crimson'
]
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():
# Collect statistics on all the trajectories
trajectory_groups = []
added_ground_truth = False
trial_result_list = self.get_trial_results(
system_id, dataset_id)
if len(trial_result_list) <= 0:
continue
# Collect all the trial results
for trial_idx, trial_result_id in enumerate(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 not added_ground_truth:
trajectory_groups.append(('Ground Truth', [
th.zero_trajectory(
trial_result.get_ground_truth_camera_poses(
))
], {
'c': 'black'
}))
added_ground_truth = True
trajectory_groups.append(
('Repeat {0}'.format(trial_idx),
[trial_result.get_computed_camera_poses()], {
'c': colours[trial_idx % len(colours)]
}))
data_helpers.create_axis_plot(
title="{0} on {1}".format(system_name, dataset_name),
trajectory_groups=trajectory_groups,
save_path=save_path)
def export_trajectory_as_json(trial_results: typing.Mapping[str, bson.ObjectId], filename: str,
db_client: arvet.database.client.DatabaseClient) -> None:
"""
Export trajectories from trial results to json, so that the web gui can display them.
Allows for multiple trial results with different labels, final json format
is the label to the computed trajectory, plus a key 'ground_truth' containing the ground truth trajectory
:param trial_results: A map from label to
:param filename: The name of the file to save, without suffix {filename}.json
:param db_client: The database client, for loading trial results
:return:
"""
if len(trial_results) >= 1:
json_data = {}
added_ground_truth = False
# For each trial result
for label, trial_result_id in trial_results.items():
trial_result = dh.load_object(db_client, db_client.trials_collection, trial_result_id)
if trial_result is not None:
if trial_result.success:
if not added_ground_truth:
added_ground_truth = True
trajectory = trial_result.get_ground_truth_camera_poses()
if len(trajectory) > 0:
first_pose = trajectory[min(trajectory.keys())]
json_data['ground_truth'] = [
[time] + location_to_json(first_pose.find_relative(pose))
for time, pose in trajectory.items()
]
trajectory = trial_result.get_computed_camera_poses()
if len(trajectory) > 0:
first_pose = trajectory[min(trajectory.keys())]
json_data[label] = [[time] + location_to_json(first_pose.find_relative(pose))
for time, pose in trajectory.items()]
with open('{0}.json'.format(filename), 'w') as json_file:
json.dump(json_data, json_file)
def create_plot(self,
db_client: arvet.database.client.DatabaseClient,
system_name: str,
dataset_name: str,
reference_filenames: typing.List[str],
rescale: bool = False,
extra_filenames: typing.List[typing.Tuple[str,
typing.List[str],
dict]] = None):
if system_name not in self.systems:
logging.getLogger(__name__).warning(
"Missing system {0}".format(system_name))
return
if dataset_name not in self.datasets:
logging.getLogger(__name__).warning(
"Missing dataset {0}".format(dataset_name))
return
if extra_filenames is None:
extra_filenames = []
trial_result_list = self.get_trial_results(self.systems[system_name],
self.datasets[dataset_name])
reference_trajectories = [
load_ref_trajectory(filename) for filename in reference_filenames
if os.path.isfile(filename)
]
computed_trajectories = []
ground_truth_trajectories = []
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:
computed_trajectories.append(
th.zero_trajectory(
trial_result.get_computed_camera_poses()))
if len(ground_truth_trajectories) <= 0:
ground_truth_trajectories.append(
th.zero_trajectory(
trial_result.get_ground_truth_camera_poses()))
# Find the scale of the ground truth trajectory
gt_scale = 1
rescale = rescale and len(ground_truth_trajectories) >= 1
if rescale:
gt_scale = th.find_trajectory_scale(ground_truth_trajectories[0])
reference_trajectories = [
th.rescale_trajectory(traj, gt_scale)
for traj in reference_trajectories
]
computed_trajectories = [
th.rescale_trajectory(traj, gt_scale)
for traj in computed_trajectories
]
extra_trajectory_groups = []
for group_name, trajectory_files, style in extra_filenames:
trajectories = [
load_ref_trajectory(filename,
ref_timestamps=sorted(
reference_trajectories[0].keys()))
for filename in trajectory_files if os.path.isfile(filename)
]
if rescale:
trajectories = [
th.rescale_trajectory(traj, gt_scale)
for traj in trajectories
]
extra_trajectory_groups.append((group_name, trajectories, style))
# Build the graph
title = "Trajectory for {0} on {1}".format(system_name, dataset_name)
if rescale:
title += " (rescaled)"
data_helpers.create_axis_plot(
title, [('locally from example', reference_trajectories, {
'c': 'blue',
'linestyle': '-',
'marker': 'None'
}),
('through framework on HPC', computed_trajectories, {
'c': 'red',
'linestyle': '--',
'marker': 'None'
}),
('ground truth', ground_truth_trajectories, {
'c': 'black'
})] + extra_trajectory_groups,
os.path.join('figures',
type(self).__name__))
def _plot_trajectories(self,
db_client: arvet.database.client.DatabaseClient):
"""
Plot the ground-truth and computed trajectories for each system for each trajectory.
This is important for validation
:param db_client:
:return:
"""
import matplotlib.pyplot as pyplot
# noinspection PyUnresolvedReferences
from mpl_toolkits.mplot3d import Axes3D
logging.getLogger(__name__).info("Plotting trajectories...")
for dataset_name, dataset_id in self.datasets.items():
# Collect the trial results for this dataset
trial_results = {}
style = {}
for system_name, system_id in self.systems.items():
trial_result_list = self.get_trial_results(
system_id, dataset_id)
if len(trial_result_list) > 0:
label = "{0} on {1}".format(system_name, dataset_name)
trial_results[label] = trial_result_list[0]
style[
label] = '--' if dataset_name == 'reference dataset' else '-'
# Make sure we have at least one result to plot
if len(trial_results) > 1:
figure = pyplot.figure(figsize=(14, 10), dpi=80)
figure.suptitle(
"Computed trajectories for {0}".format(dataset_name))
ax = figure.add_subplot(111, projection='3d')
ax.set_xlabel('x-location')
ax.set_ylabel('y-location')
ax.set_zlabel('z-location')
ax.plot([0], [0], [0], 'ko', label='origin')
added_ground_truth = False
# For each trial result
for label, trial_result_id in trial_results.items():
trial_result = dh.load_object(db_client,
db_client.trials_collection,
trial_result_id)
if trial_result is not None:
if trial_result.success:
if not added_ground_truth:
lower, upper = data_helpers.plot_trajectory(
ax,
trial_result.get_ground_truth_camera_poses(
), 'ground truth trajectory')
mean = (upper + lower) / 2
lower = 1.2 * lower - mean
upper = 1.2 * upper - mean
ax.set_xlim(lower, upper)
ax.set_ylim(lower, upper)
ax.set_zlim(lower, upper)
added_ground_truth = True
data_helpers.plot_trajectory(
ax,
trial_result.get_computed_camera_poses(),
label=label,
style=style[label])
else:
print("Got failed trial: {0}".format(
trial_result.reason))
logging.getLogger(__name__).info(
"... plotted trajectories for {0}".format(dataset_name))
ax.legend()
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.95, right=0.99)
pyplot.show()
def _plot_relative_pose_error(self, db_client: arvet.database.client.DatabaseClient):
import matplotlib.pyplot as pyplot
logging.getLogger(__name__).info("Plotting relative pose error...")
# Map system ids and simulator ids to printable names
simulator_names = {v: k for k, v in self._simulators.items()}
systems = du.defaults({'LIBVISO 2': self._libviso_system}, self._orbslam_systems)
for trajectory_group in self._trajectory_groups.values():
# Collect all the image sources for this trajectory group
image_sources = {}
for simulator_id, dataset_id in trajectory_group.generated_datasets.items():
if simulator_id in simulator_names:
image_sources[simulator_names[simulator_id]] = dataset_id
else:
image_sources[simulator_id] = dataset_id
if len(image_sources) <= 1:
# Skip where we've only got one image source, it's not interesting.
continue
# Collect the results for each image source in this group
results = {}
for system_name, system_id in systems.items():
for dataset_name, dataset_id in image_sources.items():
trial_result_list = self.get_trial_results(system_id, dataset_id)
label = "{0} on {1}".format(system_name, dataset_name)
for idx in range(len(trial_result_list)):
result_id = self.get_benchmark_result(trial_result_list[idx], self._benchmark_rpe)
if result_id is not None:
if len(trial_result_list) > 1:
results[label + ' repeat {0}'.format(idx)] = result_id
else:
results[label] = result_id
if len(results) > 1:
figure = pyplot.figure(figsize=(14, 10), dpi=80)
figure.suptitle("Relative pose error for {0}".format(trajectory_group.name))
ax = figure.add_subplot(111)
ax.set_xlabel('time')
ax.set_ylabel('relative pose error')
# For each trial result
for label, result_id in results.items():
result = dh.load_object(db_client, db_client.results_collection, result_id)
if result is not None:
if result.success:
x = []
y = []
times = sorted(result.translational_error.keys())
for time in times:
error = result.translational_error[time]
if error < 100:
x.append(time - times[0])
y.append(error)
ax.plot(x, y, '-', label=label, alpha=0.7)
else:
print("Got failed result: {0}".format(result.reason))
logging.getLogger(__name__).info("... plotted rpe for {0}".format(trajectory_group.name))
ax.legend()
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.95, right=0.99)
pyplot.show()
def _plot_trajectories(self, db_client: arvet.database.client.DatabaseClient):
"""
Plot the ground-truth and computed trajectories for each system for each trajectory.
This is important for validation
:param db_client:
:return:
"""
import matplotlib.pyplot as pyplot
# noinspection PyUnresolvedReferences
from mpl_toolkits.mplot3d import Axes3D
logging.getLogger(__name__).info("Plotting trajectories...")
# Map system ids and simulator ids to printable names
simulator_names = {v: k for k, v in self._simulators.items()}
systems = du.defaults({'LIBVISO 2': self._libviso_system}, self._orbslam_systems)
for trajectory_group in self._trajectory_groups.values():
# Collect all the image sources for this trajectory group
image_sources = {}
for simulator_id, dataset_id in trajectory_group.generated_datasets.items():
if simulator_id in simulator_names:
image_sources[simulator_names[simulator_id]] = dataset_id
else:
image_sources[simulator_id] = dataset_id
# Collect the trial results for each image source in this group
trial_results = {}
for system_name, system_id in systems.items():
for dataset_name, dataset_id in image_sources.items():
trial_result_list = self.get_trial_results(system_id, dataset_id)
label = "{0} on {1}".format(system_name, dataset_name)
for idx in range(len(trial_result_list)):
if len(trial_result_list) > 1:
trial_results[label + ' repeat {0}'.format(idx)] = trial_result_list[idx]
else:
trial_results[label] = trial_result_list[idx]
# Make sure we have at least one result to plot
if len(trial_results) >= 1:
figure = pyplot.figure(figsize=(14, 10), dpi=80)
figure.suptitle("Computed trajectories for {0}".format(trajectory_group.name))
ax = figure.add_subplot(111, projection='3d')
ax.set_xlabel('x-location')
ax.set_ylabel('y-location')
ax.set_zlabel('z-location')
figure = pyplot.figure(figsize=(14, 10), dpi=80)
figure.suptitle("Computed orientation for {0}".format(trajectory_group.name))
oax = figure.add_subplot(111, projection='3d')
oax.set_xlabel('x-location')
oax.set_ylabel('y-location')
oax.set_zlabel('z-location')
added_ground_truth = False
lowest = -0.001
highest = 0.001
cmap = pyplot.get_cmap('Set1')
colour_index = 0
# For each trial result
for label, trial_result_id in trial_results.items():
trial_result = dh.load_object(db_client, db_client.trials_collection, trial_result_id)
if trial_result is not None:
if trial_result.success:
if not added_ground_truth:
trajectory = trial_result.get_ground_truth_camera_poses()
lower, upper = data_helpers.plot_trajectory(ax, trajectory, 'ground truth trajectory',
style='k--')
lowest = min(lowest, lower)
highest = max(highest, upper)
added_ground_truth = True
trajectory = trial_result.get_computed_camera_poses()
lower, upper = data_helpers.plot_trajectory(ax, trajectory, label=label, style='-')
plot_forward(oax, trajectory, label=label, colors=[cmap(colour_index / 9, alpha=0.5)])
lowest = min(lowest, lower)
highest = max(highest, upper)
colour_index += 1
else:
print("Got failed trial: {0}".format(trial_result.reason))
logging.getLogger(__name__).info("... plotted trajectories for {0}".format(trajectory_group.name))
ax.legend()
ax.set_xlim(lowest, highest)
ax.set_ylim(lowest, highest)
ax.set_zlim(lowest, highest)
oax.legend()
oax.set_xlim(lowest, highest)
oax.set_ylim(lowest, highest)
oax.set_zlim(lowest, highest)
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.95, right=0.99)
pyplot.show()
def schedule_generation(
self, simulators: typing.Mapping[str, bson.ObjectId],
quality_variations: typing.List[typing.Tuple[str, dict]],
task_manager: arvet.batch_analysis.task_manager.TaskManager,
db_client: arvet.database.client.DatabaseClient) -> bool:
"""
Do imports and dataset generation for this trajectory group.
Will create a controller, and then generate reduced quality synthetic datasets.
:param simulators: A Map of simulators, indexed by name
:param quality_variations: A list of names and quality variations
:param task_manager:
:param db_client:
:return: True if part of the group has changed, and it needs to be re-saved
"""
changed = False
# First, make a follow controller for the base dataset if we don't have one.
# This will be used to generate reduced-quality datasets following the same trajectory
# as the root dataset
if self.follow_controller_id is None:
self.follow_controller_id = follow_cont.create_follow_controller(
db_client,
self.reference_dataset,
sequence_type=sequence_type.ImageSequenceType.SEQUENTIAL)
changed = True
# Next, if we haven't already, compute baseline configuration from the reference dataset
if self.baseline_configuration is None or len(
self.baseline_configuration) == 0:
reference_dataset = dh.load_object(
db_client, db_client.image_source_collection,
self.reference_dataset)
if isinstance(reference_dataset,
arvet.core.image_collection.ImageCollection):
intrinsics = reference_dataset.get_camera_intrinsics()
self.baseline_configuration = {
# Simulation execution config
'stereo_offset': reference_dataset.get_stereo_baseline() \
if reference_dataset.is_stereo_available else 0,
'provide_rgb': True,
'provide_ground_truth_depth': False, # We don't care about this
'provide_labels': reference_dataset.is_labels_available,
'provide_world_normals': reference_dataset.is_normals_available,
# Depth settings
'provide_depth': reference_dataset.is_depth_available,
'depth_offset': reference_dataset.get_stereo_baseline() \
if reference_dataset.is_depth_available else 0,
'projector_offset': reference_dataset.get_stereo_baseline() \
if reference_dataset.is_depth_available else 0,
# Simulator camera settings, be similar to the reference dataset
'resolution': {'width': intrinsics.width, 'height': intrinsics.height},
'fov': max(intrinsics.horizontal_fov, intrinsics.vertical_fov),
'depth_of_field_enabled': False,
'focus_distance': None,
'aperture': 2.2,
# Quality settings - Maximum quality
'lit_mode': True,
'texture_mipmap_bias': 0,
'normal_maps_enabled': True,
'roughness_enabled': True,
'geometry_decimation': 0,
'depth_noise_quality': 1,
# Simulation server config
'host': 'localhost',
'port': 9000,
}
changed = True
# Then, for each simulator listed for this trajectory group
origin_counts = {}
for sim_name, origin in self.mappings:
# Count how many times each simulator is used, so we can assign a unique world name to each start point
if sim_name not in origin_counts:
origin_counts[sim_name] = 1
else:
origin_counts[sim_name] += 1
# Schedule generation of quality variations that don't exist yet
if sim_name in simulators:
# For every quality variation
for quality_name, config in quality_variations:
generate_dataset_task = task_manager.get_generate_dataset_task(
controller_id=self.follow_controller_id,
simulator_id=simulators[sim_name],
simulator_config=du.defaults(
{'origin': origin}, config,
self.baseline_configuration),
num_cpus=1,
num_gpus=0,
memory_requirements='3GB',
expected_duration='4:00:00')
if generate_dataset_task.is_finished:
world_name = "{0} {1}".format(sim_name,
origin_counts[sim_name])
if world_name not in self.generated_datasets:
self.generated_datasets[world_name] = {}
self.generated_datasets[world_name][
quality_name] = generate_dataset_task.result
changed = True
else:
task_manager.do_task(generate_dataset_task)
return changed
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 _compute_error_correlation(
self, db_client: arvet.database.client.DatabaseClient):
import matplotlib.pyplot as pyplot
import pandas as pd
save_path = os.path.join(
type(self).get_output_folder(), 'motion correlation')
os.makedirs(save_path, exist_ok=True)
logging.getLogger(__name__).info(
"Plotting correlation and saving to {0} ...".format(save_path))
for system_name, system_id in self.systems.items():
for dataset_name, dataset_id in self.datasets.items():
logging.getLogger(__name__).info(
" .... correlations for {0} on {1}".format(
system_name, dataset_name))
result_id = self.get_benchmark_result(
system_id, dataset_id, self.benchmarks['Estimate Error'])
benchmark_result = dh.load_object(db_client, db_client.results_collection, result_id) \
if result_id is not None else None
if benchmark_result is None:
logging.getLogger(__name__).info(
" .... no result available")
else:
dataframe = pd.DataFrame(
benchmark_result.observations,
columns=[
'x error', 'y error', 'z error',
'translational error length',
'translational error direction',
'rotational error', 'x noise', 'y noise',
'z noise', 'translational noise length',
'translational noise direction',
'rotational noise', 'tracking',
'number of features', 'number of matches',
'x motion', 'y motion', 'z motion',
'distance moved', 'angle rotated'
])
correlation = dataframe.corr()
print(correlation)
# Plot aggregate correlation to motion
title = "{0} on {1} statistics correlation".format(
system_name, dataset_name)
figure = pyplot.figure(figsize=(14, 10), dpi=80)
figure.suptitle(title)
ax = figure.add_subplot(111)
img = ax.matshow(
correlation,
aspect='auto',
cmap=pyplot.get_cmap('RdBu'),
norm=midpoint_normalize.MidpointNormalize(midpoint=0))
ax.set_xticks(range(len(correlation.columns)))
ax.set_xticklabels(correlation.columns,
rotation='vertical')
ax.set_yticks(range(len(correlation.columns)))
ax.set_yticklabels(correlation.columns)
figure.colorbar(img, ax=ax)
pyplot.tight_layout()
pyplot.subplots_adjust(top=0.75, right=0.99)
figure.savefig(os.path.join(save_path, title + '.png'))
pyplot.close(figure)
with open(os.path.join(save_path, title + '.txt'),
'w') as corr_file:
corr_file.write(str(correlation))
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)
