def plot(args, result, traj_ref, traj_est):
from evo.tools import plot
from evo.tools.settings import SETTINGS
import matplotlib.pyplot as plt
import numpy as np
logger.debug(SEP)
logger.debug("Plotting results... ")
plot_mode = plot.PlotMode(args.plot_mode)
# Plot the raw metric values.
fig1 = plt.figure(figsize=SETTINGS.plot_figsize)
if "seconds_from_start" in result.np_arrays:
seconds_from_start = result.np_arrays["seconds_from_start"]
else:
seconds_from_start = None
plot.error_array(
fig1, result.np_arrays["error_array"], x_array=seconds_from_start,
statistics={
s: result.stats[s]
for s in SETTINGS.plot_statistics if s not in ("min", "max")
}, name=result.info["label"], title=result.info["title"],
xlabel="$t$ (s)" if seconds_from_start else "index")
# Plot the values color-mapped onto the trajectory.
fig2 = plt.figure(figsize=SETTINGS.plot_figsize)
ax = plot.prepare_axis(fig2, plot_mode)
plot.traj(ax, plot_mode, traj_ref, style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color, label='reference',
alpha=SETTINGS.plot_reference_alpha)
if args.plot_colormap_min is None:
args.plot_colormap_min = result.stats["min"]
if args.plot_colormap_max is None:
args.plot_colormap_max = result.stats["max"]
if args.plot_colormap_max_percentile is not None:
args.plot_colormap_max = np.percentile(
result.np_arrays["error_array"], args.plot_colormap_max_percentile)
plot.traj_colormap(ax, traj_est, result.np_arrays["error_array"],
plot_mode, min_map=args.plot_colormap_min,
max_map=args.plot_colormap_max,
title="Error mapped onto trajectory")
fig2.axes.append(ax)
plot_collection = plot.PlotCollection(result.info["title"])
plot_collection.add_figure("raw", fig1)
plot_collection.add_figure("map", fig2)
if args.plot:
plot_collection.show()
if args.save_plot:
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
def main():
import argparse
import argcomplete
basic_desc = "experimental tool for opening a serialized PlotCollection (pickle format)"
lic = "(c) [email protected]"
main_parser = argparse.ArgumentParser(description="%s %s" % (basic_desc, lic))
main_parser.add_argument("in_file", help="path to a serialized plot_collection")
main_parser.add_argument("-t", "--title", help="custom title (default: file name)")
main_parser.add_argument("--save_plot", help="path to save plot", default=None)
main_parser.add_argument("--serialize_plot", help="path to re-serialize PlotCollection",
default=None)
main_parser.add_argument("--to_html", help="convert to html (requires mpld3 library)",
action="store_true")
main_parser.add_argument("--no_warnings", help="no warnings requiring user confirmation",
action="store_true")
argcomplete.autocomplete(main_parser)
args = main_parser.parse_args()
from evo.tools import plot, settings, user
settings.configure_logging(verbose=True)
if not args.title:
title = os.path.basename(args.in_file)
else:
title = args.title
if not args.no_warnings:
logging.warning("Please note that this tool is experimental and not guranteed to work.\n"
"Only works if the same matplotlib backend is used as for serialization.\n"
"If not, try: evo_config set plot_backend <backend_name>\n" + SEP)
plot_collection = plot.PlotCollection(title, deserialize=args.in_file)
logging.debug("deserialized PlotCollection: " + str(plot_collection))
plot_collection.show()
if args.serialize_plot:
logging.debug(SEP)
plot_collection.serialize(args.serialize_plot, confirm_overwrite=not args.no_warnings)
if args.save_plot:
logging.debug(SEP)
plot_collection.export(args.save_plot, confirm_overwrite=not args.no_warnings)
if args.to_html:
import mpld3
logging.debug(SEP + "\nhtml export\n")
for name, fig in plot_collection.figures.items():
html = mpld3.fig_to_html(fig)
out = name + ".html"
with open(out, 'w') as f:
logging.debug(out)
f.write(html)
if not args.no_warnings:
logging.debug(SEP)
if user.confirm("save changes & overwrite original file "
+ args.in_file + "? (y/n)"):
plot_collection.serialize(args.in_file, confirm_overwrite=False)
def show_debug_figure(image_data, label, t_inputs):
from evo.tools import plot
fig_obs = make_observations_figure(image_data, label, t_inputs)
# create temporary pose file which holds 1) identity and 2) relative pose from label
tmp_file_name = '.tmp_label.txt'
with open(tmp_file_name, 'w') as f:
f.write(
mat2string(np.eye(4, dtype=np.float64)) +
mat2string(euler2mat(label)))
fig_traj, fig_xyz, fig_rpy = make_evo_traj_figures(tmp_file_name)
# remove temporary pose file
os.remove(tmp_file_name)
# add figures to evo::plot_collection instance
plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
plot_collection.add_figure("observations", fig_obs)
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
# show all plots in tabbed window
show_tabbed_plots(plot_collection)
def main_ape(traj_ref,
traj_est,
pose_relation,
align=True,
correct_scale=False,
ref_name="",
est_name="",
show_plot=False,
save_plot=None,
plot_mode=None,
save_results=None,
no_warnings=False,
serialize_plot=None):
from evo.algorithms import metrics
from evo.algorithms import trajectory
from evo.tools import file_interface
from evo.tools.settings import SETTINGS
only_scale = correct_scale and not align
if align or correct_scale:
logging.debug(SEP)
if only_scale:
logging.debug("correcting scale...")
else:
logging.debug("aligning using Umeyama's method..." + (
" (with scale correction)" if correct_scale else ""))
traj_est = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale, only_scale)
logging.debug(SEP)
# calculate APE
data = (traj_ref, traj_est)
ape_metric = metrics.APE(pose_relation)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
title = str(ape_metric)
if align and not correct_scale:
title += "\n(with SE(3) Umeyama alignment)"
elif align and correct_scale:
title += "\n(with Sim(3) Umeyama alignment)"
elif only_scale:
title += "\n(scale corrected)"
else:
title += "\n(not aligned)"
logging.debug(SEP)
res_str = ""
for name, val in sorted(ape_statistics.items()):
res_str += "{:>10}".format(name) + "\t" + "{0:.6f}".format(val) + "\n"
logging.info("\nstatistics of " + title + ":\n\n" + res_str)
if show_plot or save_plot or save_results or serialize_plot:
if isinstance(traj_est, trajectory.PoseTrajectory3D):
seconds_from_start = [
t - traj_est.timestamps[0] for t in traj_est.timestamps
]
else:
seconds_from_start = None
if show_plot or save_plot or serialize_plot:
from evo.tools import plot
import matplotlib.pyplot as plt
logging.debug(SEP)
logging.debug("plotting results... ")
fig1 = plt.figure(figsize=(SETTINGS.plot_figsize[0],
SETTINGS.plot_figsize[1]))
# metric values
plot.error_array(
fig1,
ape_metric.error,
x_array=seconds_from_start,
statistics=ape_statistics,
name="APE" + (" (" + ape_metric.unit.value + ")")
if ape_metric.unit else "",
title=title,
xlabel="$t$ (s)" if seconds_from_start else "index")
# info text
if SETTINGS.plot_info_text and est_name and ref_name:
ax = fig1.gca()
ax.text(0,
-0.12,
"estimate: " + est_name + "\nreference: " + ref_name,
transform=ax.transAxes,
fontsize=8,
color="gray")
# trajectory colormapped
fig2 = plt.figure(figsize=(SETTINGS.plot_figsize[0],
SETTINGS.plot_figsize[1]))
plot_mode = plot_mode if plot_mode is not None else plot.PlotMode.xyz
ax = plot.prepare_axis(fig2, plot_mode)
plot.traj(ax,
plot_mode,
traj_ref,
'--',
'gray',
'reference',
alpha=0.0 if SETTINGS.plot_hideref else 1.0)
plot.traj_colormap(ax,
traj_est,
ape_metric.error,
plot_mode,
min_map=ape_statistics["min"],
max_map=ape_statistics["max"],
title="APE mapped onto trajectory")
fig2.axes.append(ax)
plot_collection = plot.PlotCollection(title)
plot_collection.add_figure("raw", fig1)
plot_collection.add_figure("map", fig2)
if show_plot:
plot_collection.show()
if save_plot:
plot_collection.export(save_plot,
confirm_overwrite=not no_warnings)
if serialize_plot:
logging.debug(SEP)
plot_collection.serialize(serialize_plot,
confirm_overwrite=not no_warnings)
if save_results:
logging.debug(SEP)
file_interface.save_res_file(save_results,
ape_metric,
ape_statistics,
title,
ref_name,
est_name,
seconds_from_start,
traj_ref,
traj_est,
confirm_overwrite=not no_warnings)
return ape_statistics, ape_metric.error
def get_and_save_results_from_folder(folder_with_predicted_poses,category):
global args
global kitti_eval_tool
global folder_with_gt_poses
global output_folder
global t
global results
values_for_excel = []
columns_for_excel = []
type_of_statistics = 'mean'
for filename in sorted(os.listdir(folder_with_predicted_poses)):
if not(os.path.exists(os.path.join(folder_with_gt_poses, filename))):
print("file with gt poses doesn't exist for "+filename)
continue
if filename.find('.txt') == -1:
continue
seq_results = {}
seq_results['name_seq'] = filename[:filename.rfind('.')]
seq_results['category'] = category
folder_name = seq_results['category']
seq_results['metrics'] = {}
seq_results['lost'] = False
os.makedirs(os.path.join(output_folder, folder_name), exist_ok=True)
output_folder_seq = os.path.join(output_folder, folder_name, filename[:filename.rfind('.')])
os.makedirs(output_folder_seq, exist_ok=True)
if os.path.isfile(os.path.join(output_folder, folder_name,"results.txt")):
file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "a")
else:
file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "w")
file_results_txt.write("translation_error(%) rotation_error(deg/m) ATE(m) APE_translation_error_median(m) APE_rotation_error_median(deg) dst_to_trgt\n")
# -------------------------------------Getting results---------------------------------------------------
if args.gt_format == 'kitti':
traj_ref = file_interface.read_kitti_poses_file(os.path.join(folder_with_gt_poses, filename))
if args.gt_format == 'tum':
traj_ref = file_interface.read_tum_trajectory_file(os.path.join(folder_with_gt_poses, filename))
seq_results["length_of_ref_traj"] = traj_ref.path_length
end_time_gt = traj_ref.get_infos()["t_end (s)"]
if args.gt_format == 'euroc':
traj_ref = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_gt_poses, filename))
if args.result_format == 'kitti':
traj_est = file_interface.read_kitti_poses_file(os.path.join(folder_with_predicted_poses, filename))
if args.result_format == 'tum':
traj_est = file_interface.read_tum_trajectory_file(os.path.join(folder_with_predicted_poses, filename))
seq_results["length_of_estimated_traj"] = traj_est.path_length
if args.result_format == 'euroc':
traj_est = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_predicted_poses, filename))
if args.result_format == 'tum' and args.gt_format == 'tum':
seq_results["num_gt_poses"] = traj_ref.num_poses
seq_results["num_predicted_poses"] = traj_est.num_poses
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, args.max_diff)
end_time_est = traj_est.get_infos()["t_end (s)"]
if (abs(end_time_est - end_time_gt) > 0.2) or (traj_est.get_infos()["t_start (s)"] > 0.2):
print('LOST in track '+filename[:filename.rfind('.')])
seq_results['lost'] = True
results.append(seq_results)
t.update(1)
continue
if args.alignment != None:
traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=args.alignment.find("scale") != -1, correct_only_scale=args.alignment=="scale")
trajectory.align_trajectory_origin(traj_est, traj_ref)
data = (traj_ref, traj_est)
ape_metric_translation = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric_rotation = metrics.APE(metrics.PoseRelation.rotation_angle_deg)
ape_metric_translation.process_data(data)
ape_metric_rotation.process_data(data)
ape_translation_statistics = ape_metric_translation.get_all_statistics()
ape_rotation_statistics = ape_metric_rotation.get_all_statistics()
ape_translation_statistics_plot = copy.deepcopy(ape_translation_statistics)
ape_rotation_statistics_plot = copy.deepcopy(ape_rotation_statistics)
ape_translation_statistics_plot.pop('sse')
ape_translation_statistics_plot.pop('std')
ape_translation_statistics_plot.pop('min')
ape_translation_statistics_plot.pop('max')
ape_rotation_statistics_plot.pop('sse')
ape_rotation_statistics_plot.pop('std')
ape_rotation_statistics_plot.pop('min')
ape_rotation_statistics_plot.pop('max')
kitti_trans_err, kitti_rot_err, ate = kitti_eval_tool.eval(traj_ref.poses_se3,
traj_est.poses_se3,
alignment=None)
#---------------------------------adding results to variable seq_results for excel -----------------------------
seq_results['metrics']['dist_to_trgt'] = traj_est.get_infos()['pos_end (m)'] - traj_ref.get_infos()['pos_end (m)']
seq_results['metrics']['dist_to_trgt'] = np.sum(np.array(seq_results['metrics']['dist_to_trgt'])**2)**0.5
seq_results['metrics']["Kitti trans err (%)"] = kitti_trans_err
seq_results['metrics']["Kitti rot err (deg/m)"] = kitti_rot_err
seq_results['metrics']["ATE (m)"] = ate
seq_results['metrics']["APE(trans err) median (m)"] = ape_translation_statistics["median"]
seq_results['metrics']["APE(rot err) median (deg)"] = ape_rotation_statistics["median"]
#--------------------------------------------------------------------------------------------------------
#-------------------------------------------------------------------------------------------------------
# --------------------------------printing results into console----------------------------------------------
print('Results for "'+filename+'":')
print('Kitti average translational error (%): {:.7f}'.format(kitti_trans_err))
print('Kitti average rotational error (deg/m): {:.7f}'.format(kitti_rot_err))
print('ATE (m): {:.7f}'.format(ate))
print('APE(translation error) median (m): {:.7f}'.format(ape_translation_statistics["median"]))
print('APE(rotation error) median (deg): {:.7f}'.format(ape_rotation_statistics["median"]))
print('distance to target on the last frame: {:.7f}'.format(seq_results['metrics']['dist_to_trgt']))
#------------------------------------------------------------------------------------------------------------
#---------------------------------Saving results into overall results text file------------------------------
file_results_txt.write('{:<24} '.format(filename[:filename.rfind('.')]))
file_results_txt.write('{:>7.4f} '.format(kitti_trans_err))
file_results_txt.write('{:>7.4f} '.format(kitti_rot_err))
file_results_txt.write('{:>7.4f} '.format(ate))
file_results_txt.write('{:>7.4f} '.format(ape_translation_statistics["median"]))
file_results_txt.write('{:>7.4f} '.format(ape_rotation_statistics["median"]))
file_results_txt.write('{:>7.4f}\n'.format(seq_results['metrics']['dist_to_trgt']))
#------------------------------------------------------------------------------------------------------------
# --------------------------------Saving metrics to text file for one track----------------------------------
txt_filename = filename[:filename.rfind('.')]+"_metrics.txt"
with open(os.path.join(output_folder_seq, txt_filename), "w") as txt_file:
txt_file.write('Kitti average translational error (%): {:.7f}\n'.format(kitti_trans_err))
txt_file.write('Kitti average rotational error (deg/m): {:.7f}\n'.format(kitti_rot_err))
txt_file.write('ATE (m): {:.7f}\n'.format(ate))
txt_file.write('APE(translation error) median (m): {:.7f}\n'.format(ape_translation_statistics["median"]))
txt_file.write('APE(rotation error) median (deg): {:.7f}\n'.format(ape_rotation_statistics["median"]))
txt_file.write('Distance to target on the last frame: {:.7f}\n'.format(seq_results['metrics']['dist_to_trgt']))
#---------------------------------------------------------------------------------------------------------
# ---------------------------------Saving values of errors for each frame to text file------------------------
# ------------------------------------------for translation errors----------------------------------------
txt_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.txt"
output_folder_seq_translation = os.path.join(output_folder_seq,"translation")
output_folder_seq_rotation = os.path.join(output_folder_seq,"rotation")
os.makedirs(output_folder_seq_translation, exist_ok=True)
os.makedirs(output_folder_seq_rotation, exist_ok=True)
with open(os.path.join(output_folder_seq_translation, txt_filename), "w") as txt_file:
for error in ape_metric_translation.error:
txt_file.write('{:.10f}\n'.format(error))
# -----------------------------------------for rotation degree errors--------------------------------------
txt_filename = filename[:filename.rfind('.')]+"_APE(rotation_deg)_errors.txt"
with open(os.path.join(output_folder_seq_rotation, txt_filename), "w") as txt_file:
for error in ape_metric_rotation.error:
txt_file.write('{:.10f}\n'.format(error))
#----------------------------------------------------------------------------------------------------------
# ---------------------------------------Saving plot of errors of each frame------------------------------
# ------------------------------------------for translation errors----------------------------------------
plot_collection = plot.PlotCollection("Example")
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1, ape_metric_translation.error,
name="APE", title=str(ape_metric_translation), xlabel="Index of frame", ylabel='Error')
plot_collection.add_figure("raw", fig_1)
plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.png"
plt.savefig(os.path.join(output_folder_seq_translation, plot_filename))
plt.close(fig_1)
# -----------------------------------------for rotation degree errors--------------------------------------
plot_collection = plot.PlotCollection("Example")
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1, ape_metric_rotation.error,
name="APE", title=str(ape_metric_rotation), xlabel="Index of frame", ylabel='Error')
plot_collection.add_figure("raw", fig_1)
plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_errors.png"
plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename))
plt.close(fig_1)
#-----------------------------------------------------------------------------------------------------------
# -----------------------------------------Saving trajectory plot-------------------------------------------
# ------------------------------------------for translation errors----------------------------------------
fig_2 = plt.figure(figsize=(8, 8))
ax = plot.prepare_axis(fig_2, plot_mode)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap( ax, traj_est, ape_metric_translation.error, plot_mode,
min_map=ape_translation_statistics["min"],
max_map=ape_translation_statistics["max"], title="APE translation mapped onto trajectory")
plot_collection.add_figure("traj (error)", fig_2)
plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_map.png"
plt.savefig(os.path.join(output_folder_seq_translation,plot_filename))
plt.close(fig_2)
# -----------------------------------------for rotation degree errors--------------------------------------
fig_2 = plt.figure(figsize=(8, 8))
ax = plot.prepare_axis(fig_2, plot_mode)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap( ax, traj_est, ape_metric_rotation.error, plot_mode,
min_map=ape_rotation_statistics["min"],
max_map=ape_rotation_statistics["max"], title="APE rotation mapped onto trajectory")
plot_collection.add_figure("traj (error)", fig_2)
plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_map.png"
plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename))
plt.close(fig_2)
#-----------------------------------------------------------------------------------------------------------
print()
active_worksheet = wb['sheet1']
thin = Side(border_style="thin", color="000000")
thick = Side(border_style="thick", color="000000")
medium = Side(border_style="medium", color="000000")
font_header = Font(name='Arial',
size=10,
bold=True,
italic=False,
vertAlign=None,
underline='none',
strike=False,
color='FF000000')
font_values = Font(name='Arial',
size=10,
bold=False,
italic=False,
vertAlign=None,
underline='none',
strike=False,
color='FF000000')
active_worksheet.row_dimensions[2].height = 35
file_results_txt.close()
results.append(seq_results)
t.update(1)
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False)
print("calculating APE")
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
print("mean:", ape_statistics["mean"])
print("loading plot modules")
from evo.tools import plot
import matplotlib.pyplot as plt
print("plotting")
plot_collection = plot.PlotCollection("Example")
# metric values
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1,
ape_metric.error,
statistics=ape_statistics,
name="APE",
title=str(ape_metric))
plot_collection.add_figure("raw", fig_1)
# trajectory colormapped with error
fig_2 = plt.figure(figsize=(8, 8))
plot_mode = plot.PlotMode.xy
ax = plot.prepare_axis(fig_2, plot_mode)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap(ax,
def run(args):
import sys
import logging
import pandas as pd
from evo.tools import file_interface, user, settings, pandas_bridge
from evo.tools.settings import SETTINGS
pd.options.display.width = 80
pd.options.display.max_colwidth = 20
settings.configure_logging(args.verbose, args.silent, args.debug)
if args.debug:
import pprint
arg_dict = {arg: getattr(args, arg) for arg in vars(args)}
logging.debug("main_parser config:\n{}\n".format(
pprint.pformat(arg_dict)))
df = pd.DataFrame()
for result_file in args.result_files:
result = file_interface.load_res_file(result_file)
name = result_file if args.use_filenames else None
df = pd.concat([df, pandas_bridge.result_to_df(result, name)],
axis="columns")
keys = df.columns.values.tolist()
if SETTINGS.plot_usetex:
keys = [key.replace("_", "\\_") for key in keys]
df.columns = keys
duplicates = [x for x in keys if keys.count(x) > 1]
if duplicates:
logging.error("Values of 'est_name' must be unique - duplicates: {}\n"
"Try using the --use_filenames option to use filenames "
"for labeling instead.".format(", ".join(duplicates)))
sys.exit(1)
# derive a common index type if possible - preferably timestamps
common_index = None
time_indices = ["timestamps", "seconds_from_start", "sec_from_start"]
if args.use_rel_time:
del time_indices[0]
for idx in time_indices:
if idx not in df.loc["np_arrays"].index:
continue
if df.loc["np_arrays", idx].isnull().values.any():
continue
else:
common_index = idx
break
# build error_df (raw values) according to common_index
if common_index is None:
# use a non-timestamp index
error_df = pd.DataFrame(df.loc["np_arrays", "error_array"].tolist(),
index=keys).T
else:
error_df = pd.DataFrame()
for key in keys:
new_error_df = pd.DataFrame(
{key: df.loc["np_arrays", "error_array"][key]},
index=df.loc["np_arrays", common_index][key])
duplicates = new_error_df.index.duplicated(keep="first")
if any(duplicates):
logging.warning(
"duplicate indices in error array of {} - "
"keeping only first occurrence of duplicates".format(key))
new_error_df = new_error_df[~duplicates]
error_df = pd.concat([error_df, new_error_df], axis=1)
# check titles
first_title = df.loc["info", "title"][0]
first_file = args.result_files[0]
if not args.no_warnings:
checks = df.loc["info", "title"] != first_title
for i, differs in enumerate(checks):
if not differs:
continue
else:
mismatching_title = df.loc["info", "title"][i]
mismatching_file = args.result_files[i]
logging.debug(SEP)
logging.warning(
CONFLICT_TEMPLATE.format(first_file, first_title,
mismatching_title,
mismatching_file))
if not user.confirm(
"Go on anyway? - enter 'y' or any other key to exit"):
sys.exit()
if logging.getLogger().isEnabledFor(logging.DEBUG):
logging.debug(SEP)
logging.debug("Aggregated dataframe:\n{}".format(
df.to_string(line_width=80)))
# show a statistics overview
logging.debug(SEP)
logging.info("\n{}\n\n{}\n".format(
first_title, df.loc["stats"].T.to_string(line_width=80)))
if args.save_table:
logging.debug(SEP)
if args.no_warnings or user.check_and_confirm_overwrite(
args.save_table):
if SETTINGS.table_export_data.lower() == "error_array":
data = error_df
elif SETTINGS.table_export_data.lower() in ("info", "stats"):
data = df.loc[SETTINGS.table_export_data.lower()]
else:
raise ValueError(
"unsupported export data specifier: {}".format(
SETTINGS.table_export_data))
if SETTINGS.table_export_transpose:
data = data.T
if SETTINGS.table_export_format == "excel":
writer = pd.ExcelWriter(args.save_table)
data.to_excel(writer)
writer.save()
writer.close()
else:
getattr(data,
"to_" + SETTINGS.table_export_format)(args.save_table)
logging.debug("{} table saved to: {}".format(
SETTINGS.table_export_format, args.save_table))
if args.plot or args.save_plot or args.serialize_plot:
# check if data has NaN "holes" due to different indices
inconsistent = error_df.isnull().values.any()
if inconsistent and common_index != "timestamps" and not args.no_warnings:
logging.debug(SEP)
logging.warning("Data lengths/indices are not consistent, "
"raw value plot might not be correctly aligned")
from evo.tools import plot
import matplotlib.pyplot as plt
import seaborn as sns
import math
# use default plot settings
figsize = (SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])
use_cmap = SETTINGS.plot_multi_cmap.lower() != "none"
colormap = SETTINGS.plot_multi_cmap if use_cmap else None
linestyles = ["-o" for x in args.result_files
] if args.plot_markers else None
# labels according to first dataset
title = first_title
if "xlabel" in df.loc["info"].index and not df.loc[
"info", "xlabel"].isnull().values.any():
index_label = df.loc["info", "xlabel"][0]
else:
index_label = "$t$ (s)" if common_index else "index"
metric_label = df.loc["info", "label"][0]
plot_collection = plot.PlotCollection(title)
# raw value plot
fig_raw = plt.figure(figsize=figsize)
# handle NaNs from concat() above
error_df.interpolate(method="index").plot(ax=fig_raw.gca(),
colormap=colormap,
style=linestyles,
title=first_title)
plt.xlabel(index_label)
plt.ylabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("raw", fig_raw)
# statistics plot
fig_stats = plt.figure(figsize=figsize)
exclude = df.loc["stats"].index.isin(["sse"]) # don't plot sse
df.loc["stats"][~exclude].plot(kind="barh",
ax=fig_stats.gca(),
colormap=colormap,
stacked=False)
plt.xlabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("stats", fig_stats)
# grid of distribution plots
raw_tidy = pd.melt(error_df,
value_vars=list(error_df.columns.values),
var_name="estimate",
value_name=metric_label)
col_wrap = 2 if len(args.result_files) <= 2 else math.ceil(
len(args.result_files) / 2.0)
dist_grid = sns.FacetGrid(raw_tidy, col="estimate", col_wrap=col_wrap)
dist_grid.map(sns.distplot, metric_label) # fits=stats.gamma
plot_collection.add_figure("histogram", dist_grid.fig)
# box plot
fig_box = plt.figure(figsize=figsize)
ax = sns.boxplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_box.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
plot_collection.add_figure("box_plot", fig_box)
# violin plot
fig_violin = plt.figure(figsize=figsize)
ax = sns.violinplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_violin.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
plot_collection.add_figure("violin_histogram", fig_violin)
if args.plot:
plot_collection.show()
if args.save_plot:
logging.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logging.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
trajectories[traj])
traj_est = trajectory.align_trajectory(traj_est,
traj_ref,
correct_scale=False)
print("calculating APE")
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
print("mean:", ape_statistics["mean"])
from evo.tools import plot
import matplotlib.pyplot as plt
plot_collection = plot.PlotCollection("Localization")
# # metric values
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1,
ape_metric.error,
statistics=ape_statistics,
name="APE",
title=str(ape_metric))
plot_collection.add_figure("raw", fig_1)
# plt.show()
# trajectory colormapped with error
fig_2 = plt.figure(figsize=(8, 8))
plot_mode = plot.PlotMode.xy
ax = plot.prepare_axis(fig_2, plot_mode)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap(ax,
def run_analysis(self,
traj_ref_path,
traj_vio_path,
traj_pgo_path,
segments,
dataset_name="",
discard_n_start_poses=0,
discard_n_end_poses=0):
""" Analyze data from a set of trajectory csv files.
Args:
traj_ref_path: string representing filepath of the reference (ground-truth) trajectory.
traj_vio_path: string representing filepath of the vio estimated trajectory.
traj_pgo_path: string representing filepath of the pgo estimated trajectory.
segments: list of segments for RPE calculation, defined in the experiments yaml file.
dataset_name: string representing the dataset's name
discard_n_start_poses: int representing number of poses to discard from start of analysis.
discard_n_end_poses: int representing the number of poses to discard from end of analysis.
"""
import copy
# Mind that traj_est_pgo might be None
traj_ref, traj_est_vio, traj_est_pgo = self.read_traj_files(
traj_ref_path, traj_vio_path, traj_pgo_path)
# We copy to distinguish from the pgo version that may be created
traj_ref_vio = copy.deepcopy(traj_ref)
# Register and align trajectories:
evt.print_purple("Registering and aligning trajectories")
traj_ref_vio, traj_est_vio = sync.associate_trajectories(
traj_ref_vio, traj_est_vio)
traj_est_vio = trajectory.align_trajectory(
traj_est_vio,
traj_ref_vio,
correct_scale=False,
discard_n_start_poses=int(discard_n_start_poses),
discard_n_end_poses=int(discard_n_end_poses))
# We do the same for the PGO trajectory if needed:
traj_ref_pgo = None
if traj_est_pgo is not None:
traj_ref_pgo = copy.deepcopy(traj_ref)
traj_ref_pgo, traj_est_pgo = sync.associate_trajectories(
traj_ref_pgo, traj_est_pgo)
traj_est_pgo = trajectory.align_trajectory(
traj_est_pgo,
traj_ref_pgo,
correct_scale=False,
discard_n_start_poses=int(discard_n_start_poses),
discard_n_end_poses=int(discard_n_end_poses))
# We need to pick the lowest num_poses before doing any computation:
num_of_poses = traj_est_vio.num_poses
if traj_est_pgo is not None:
num_of_poses = min(num_of_poses, traj_est_pgo.num_poses)
traj_est_pgo.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
traj_ref_pgo.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
traj_est_vio.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
traj_ref_vio.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
# Calculate all metrics:
(ape_metric_vio, rpe_metric_trans_vio, rpe_metric_rot_vio,
results_vio) = self.process_trajectory_data(traj_ref_vio,
traj_est_vio, segments,
True)
# We do the same for the pgo trajectory if needed:
ape_metric_pgo = None
rpe_metric_trans_pgo = None
rpe_metric_rot_pgo = None
results_pgo = None
if traj_est_pgo is not None:
(ape_metric_pgo, rpe_metric_trans_pgo, rpe_metric_rot_pgo,
results_pgo) = self.process_trajectory_data(
traj_ref_pgo, traj_est_pgo, segments, False)
# Generate plots for return:
plot_collection = None
if self.display_plots or self.save_plots:
evt.print_green("Plotting:")
log.info(dataset_name)
plot_collection = plot.PlotCollection("Example")
if traj_est_pgo is not None:
# APE Metric Plot:
plot_collection.add_figure(
"PGO_APE_translation",
plot_metric(ape_metric_pgo, "PGO + VIO APE Translation"))
# Trajectory Colormapped with ATE Plot:
plot_collection.add_figure(
"PGO_APE_translation_trajectory_error",
plot_traj_colormap_ape(
ape_metric_pgo, traj_ref_pgo, traj_est_vio,
traj_est_pgo, "PGO + VIO ATE Mapped Onto Trajectory"))
# RPE Translation Metric Plot:
plot_collection.add_figure(
"PGO_RPE_translation",
plot_metric(rpe_metric_trans_pgo,
"PGO + VIO RPE Translation"))
# Trajectory Colormapped with RTE Plot:
plot_collection.add_figure(
"PGO_RPE_translation_trajectory_error",
plot_traj_colormap_rpe(
rpe_metric_trans_pgo, traj_ref_pgo, traj_est_vio,
traj_est_pgo,
"PGO + VIO RPE Translation Error Mapped Onto Trajectory"
))
# RPE Rotation Metric Plot:
plot_collection.add_figure(
"PGO_RPE_Rotation",
plot_metric(rpe_metric_rot_pgo, "PGO + VIO RPE Rotation"))
# Trajectory Colormapped with RTE Plot:
plot_collection.add_figure(
"PGO_RPE_rotation_trajectory_error",
plot_traj_colormap_rpe(
rpe_metric_rot_pgo, traj_ref_pgo, traj_est_vio,
traj_est_pgo,
"PGO + VIO RPE Rotation Error Mapped Onto Trajectory"))
# Plot VIO results
plot_collection.add_figure(
"VIO_APE_translation",
plot_metric(ape_metric_vio, "VIO APE Translation"))
plot_collection.add_figure(
"VIO_APE_translation_trajectory_error",
plot_traj_colormap_ape(ape_metric_vio, traj_ref_vio,
traj_est_vio, None,
"VIO ATE Mapped Onto Trajectory"))
plot_collection.add_figure(
"VIO_RPE_translation",
plot_metric(rpe_metric_trans_vio, "VIO RPE Translation"))
plot_collection.add_figure(
"VIO_RPE_translation_trajectory_error",
plot_traj_colormap_rpe(
rpe_metric_trans_vio, traj_ref_vio, traj_est_vio, None,
"VIO RPE Translation Error Mapped Onto Trajectory"))
plot_collection.add_figure(
"VIO_RPE_Rotation",
plot_metric(rpe_metric_rot_vio, "VIO RPE Rotation"))
plot_collection.add_figure(
"VIO_RPE_rotation_trajectory_error",
plot_traj_colormap_rpe(
rpe_metric_rot_vio, traj_ref_vio, traj_est_vio, None,
"VIO RPE Rotation Error Mapped Onto Trajectory"))
return [plot_collection, results_vio, results_pgo]
def run(args):
import os
import sys
import logging
import pandas as pd
import numpy as np
from natsort import natsorted
from evo.tools import file_interface, user, settings
from evo.tools.settings import SETTINGS
settings.configure_logging(args.verbose, args.silent, args.debug)
if args.debug:
import pprint
logging.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
# store data in Pandas data frames for easier analysis
raw_df = pd.DataFrame()
stat_df = pd.DataFrame()
info_df = pd.DataFrame()
use_seconds = False
for result_file in args.result_files:
logging.debug(SEP)
result_obj = file_interface.load_res_file(result_file, True)
short_est_name = os.path.splitext(
os.path.basename(result_obj.info["est_name"]))[0]
error_array = result_obj.np_arrays["error_array"]
if "seconds_from_start" in result_obj.np_arrays:
seconds_from_start = result_obj.np_arrays["seconds_from_start"]
else:
seconds_from_start = None
if not args.no_warnings and (short_est_name in info_df.columns):
logging.warning("double entry detected: " + short_est_name)
if not user.confirm(
"ignore? enter 'y' to go on or any other key to quit"):
sys.exit()
if SETTINGS.plot_usetex:
short_est_name = short_est_name.replace("_", "\\_")
if args.use_abs_time:
if "traj_est" in result_obj.trajectories:
traj_est = result_obj.trajectories["traj_est"]
index = traj_est.timestamps
use_seconds = True
else:
msg = "no 'traj_est' trajectory found in " + result_file \
+ " but --use_abs_time requires the trajectory in the result file - " \
+ "to let the metrics app include them run: evo_config set save_traj_in_zip"
raise RuntimeError(msg)
elif seconds_from_start is not None:
index = seconds_from_start.tolist()
use_seconds = True
else:
index = np.arange(0, error_array.shape[0])
result_obj.info["traj. backup?"] = \
all(k in result_obj.trajectories for k in ("traj_ref", "traj_est"))
result_obj.info["res_file"] = result_file
new_raw_df = pd.DataFrame({short_est_name: error_array.tolist()},
index=index)
new_info_df = pd.DataFrame({short_est_name: result_obj.info})
new_stat_df = pd.DataFrame({short_est_name: result_obj.stats})
# natural sort num strings "10" "100" "20" -> "10" "20" "100"
new_stat_df = new_stat_df.reindex(index=natsorted(new_stat_df.index))
# column-wise concatenation
raw_df = pd.concat([raw_df, new_raw_df], axis=1)
info_df = pd.concat([info_df, new_info_df], axis=1)
stat_df = pd.concat([stat_df, new_stat_df], axis=1)
# if verbose: log infos of the current data
logging.debug(
"\n" + result_obj.pretty_str(title=True, stats=False, info=True))
logging.debug(SEP)
logging.info("\nstatistics overview:\n" +
stat_df.T.to_string(line_width=80) + "\n")
# check titles
first_title = info_df.ix["title", 0]
first_res_file = info_df.ix["res_file", 0]
if args.save_table or args.plot or args.save_plot:
for short_est_name, column in info_df.iteritems():
if column.ix["title"] != first_title and not args.no_warnings:
logging.info(SEP)
logging.warning(
"mismatching titles, you probably use data from different metrics"
)
logging.warning("conflict:\n" + "<" * 7 + " " +
first_res_file + "\n" + first_title + "\n" +
"=" * 7 + "\n" + column.ix["title"] + "\n" +
">" * 7 + " " + column.ix["res_file"])
logging.warning(
"only the first one will be used as the title!")
if not user.confirm(
"plot/save anyway? - enter 'y' or any other key to exit"
):
sys.exit()
if args.save_table:
logging.debug(SEP)
if args.no_warnings or user.check_and_confirm_overwrite(
args.save_table):
table_fmt = SETTINGS.table_export_format
if SETTINGS.table_export_transpose:
getattr(stat_df.T, "to_" + table_fmt)(args.save_table)
else:
getattr(stat_df, "to_" + table_fmt)(args.save_table)
logging.debug(table_fmt + " table saved to: " + args.save_table)
if args.plot or args.save_plot or args.serialize_plot:
# check if data has NaN "holes" due to different indices
inconsistent = raw_df.isnull().values.any()
if inconsistent and not args.no_warnings:
logging.debug(SEP)
logging.warning(
"data lengths/indices are not consistent, plotting probably makes no sense"
)
if not user.confirm(
"plot anyway? - enter 'y' or any other key to exit"):
sys.exit()
from evo.tools import plot
import matplotlib.pyplot as plt
import seaborn as sns
import math
from scipy import stats
# use default plot settings
figsize = (SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])
use_cmap = SETTINGS.plot_multi_cmap.lower() != "none"
colormap = SETTINGS.plot_multi_cmap if use_cmap else None
linestyles = ["-o" for x in args.result_files
] if args.plot_markers else None
# labels according to first dataset
title = first_title
if "xlabel" in info_df.ix[:, 0].index:
index_label = info_df.ix["xlabel", 0]
else:
index_label = "$t$ (s)" if use_seconds else "index"
metric_label = info_df.ix["label", 0]
plot_collection = plot.PlotCollection(title)
# raw value plot
fig_raw = plt.figure(figsize=figsize)
# handle NaNs from concat() above
raw_df.interpolate(method="index").plot(ax=fig_raw.gca(),
colormap=colormap,
style=linestyles,
title=first_title)
plt.xlabel(index_label)
plt.ylabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("raw", fig_raw)
# statistics plot
fig_stats = plt.figure(figsize=figsize)
exclude = stat_df.index.isin(["sse"]) # don't plot sse
stat_df[~exclude].plot(kind="barh",
ax=fig_stats.gca(),
colormap=colormap,
stacked=False)
plt.xlabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("stats", fig_stats)
# grid of distribution plots
raw_tidy = pd.melt(raw_df,
value_vars=list(raw_df.columns.values),
var_name="estimate",
value_name=metric_label)
col_wrap = 2 if len(args.result_files) <= 2 else math.ceil(
len(args.result_files) / 2.0)
dist_grid = sns.FacetGrid(raw_tidy, col="estimate", col_wrap=col_wrap)
dist_grid.map(sns.distplot, metric_label) # fits=stats.gamma
plot_collection.add_figure("histogram", dist_grid.fig)
# box plot
fig_box = plt.figure(figsize=figsize)
ax = sns.boxplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_box.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
plot_collection.add_figure("box_plot", fig_box)
# violin plot
fig_violin = plt.figure(figsize=figsize)
ax = sns.violinplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_violin.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
plot_collection.add_figure("violin_histogram", fig_violin)
if args.plot:
plot_collection.show()
if args.save_plot:
logging.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logging.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
def run(args):
import sys
import pandas as pd
from evo.tools import log, user, settings, pandas_bridge
from evo.tools.settings import SETTINGS
pd.options.display.width = 80
pd.options.display.max_colwidth = 20
log.configure_logging(args.verbose,
args.silent,
args.debug,
local_logfile=args.logfile)
if args.debug:
import pprint
arg_dict = {arg: getattr(args, arg) for arg in vars(args)}
logger.debug("main_parser config:\n{}\n".format(
pprint.pformat(arg_dict)))
df = load_results_as_dataframe(args.result_files, args.use_filenames,
args.merge)
keys = df.columns.values.tolist()
if SETTINGS.plot_usetex:
keys = [key.replace("_", "\\_") for key in keys]
df.columns = keys
duplicates = [x for x in keys if keys.count(x) > 1]
if duplicates:
logger.error("Values of 'est_name' must be unique - duplicates: {}\n"
"Try using the --use_filenames option to use filenames "
"for labeling instead.".format(", ".join(duplicates)))
sys.exit(1)
# derive a common index type if possible - preferably timestamps
common_index = None
time_indices = ["timestamps", "seconds_from_start", "sec_from_start"]
if args.use_rel_time:
del time_indices[0]
for idx in time_indices:
if idx not in df.loc["np_arrays"].index:
continue
if df.loc["np_arrays", idx].isnull().values.any():
continue
else:
common_index = idx
break
# build error_df (raw values) according to common_index
if common_index is None:
# use a non-timestamp index
error_df = pd.DataFrame(df.loc["np_arrays", "error_array"].tolist(),
index=keys).T
else:
error_df = pd.DataFrame()
for key in keys:
new_error_df = pd.DataFrame(
{key: df.loc["np_arrays", "error_array"][key]},
index=df.loc["np_arrays", common_index][key])
duplicates = new_error_df.index.duplicated(keep="first")
if any(duplicates):
logger.warning(
"duplicate indices in error array of {} - "
"keeping only first occurrence of duplicates".format(key))
new_error_df = new_error_df[~duplicates]
error_df = pd.concat([error_df, new_error_df], axis=1)
# check titles
first_title = df.loc["info", "title"][0] if not args.ignore_title else ""
first_file = args.result_files[0]
if not args.no_warnings and not args.ignore_title:
checks = df.loc["info", "title"] != first_title
for i, differs in enumerate(checks):
if not differs:
continue
else:
mismatching_title = df.loc["info", "title"][i]
mismatching_file = args.result_files[i]
logger.debug(SEP)
logger.warning(
CONFLICT_TEMPLATE.format(first_file, first_title,
mismatching_title,
mismatching_file))
if not user.confirm(
"You can use --ignore_title to just aggregate data.\n"
"Go on anyway? - enter 'y' or any other key to exit"):
sys.exit()
logger.debug(SEP)
logger.debug("Aggregated dataframe:\n{}".format(
df.to_string(line_width=80)))
# show a statistics overview
logger.debug(SEP)
if not args.ignore_title:
logger.info("\n" + first_title + "\n\n")
logger.info(df.loc["stats"].T.to_string(line_width=80) + "\n")
if args.save_table:
logger.debug(SEP)
if SETTINGS.table_export_data.lower() == "error_array":
data = error_df
elif SETTINGS.table_export_data.lower() in ("info", "stats"):
data = df.loc[SETTINGS.table_export_data.lower()]
else:
raise ValueError("unsupported export data specifier: {}".format(
SETTINGS.table_export_data))
pandas_bridge.save_df_as_table(data,
args.save_table,
confirm_overwrite=not args.no_warnings)
if args.plot or args.save_plot or args.serialize_plot:
# check if data has NaN "holes" due to different indices
inconsistent = error_df.isnull().values.any()
if inconsistent and common_index != "timestamps" and not args.no_warnings:
logger.debug(SEP)
logger.warning("Data lengths/indices are not consistent, "
"raw value plot might not be correctly aligned")
from evo.tools import plot
import matplotlib.pyplot as plt
import seaborn as sns
import math
# use default plot settings
figsize = (SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])
use_cmap = SETTINGS.plot_multi_cmap.lower() != "none"
colormap = SETTINGS.plot_multi_cmap if use_cmap else None
linestyles = ["-o" for x in args.result_files
] if args.plot_markers else None
# labels according to first dataset
if "xlabel" in df.loc["info"].index and not df.loc[
"info", "xlabel"].isnull().values.any():
index_label = df.loc["info", "xlabel"][0]
else:
index_label = "$t$ (s)" if common_index else "index"
metric_label = df.loc["info", "label"][0]
print
plot_collection = plot.PlotCollection(first_title)
# raw value plot
fig_raw = plt.figure(figsize=figsize)
# handle NaNs from concat() above
error_df.interpolate(method="index", limit_area="inside").plot(
ax=fig_raw.gca(),
colormap=colormap,
style=linestyles,
title=first_title,
alpha=SETTINGS.plot_trajectory_alpha)
plt.xlabel(index_label)
plt.ylabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("raw", fig_raw)
# statistics plot
if SETTINGS.plot_statistics:
fig_stats = plt.figure(figsize=figsize)
include = df.loc["stats"].index.isin(SETTINGS.plot_statistics)
if any(include):
df.loc["stats"][include].plot(kind="barh",
ax=fig_stats.gca(),
colormap=colormap,
stacked=False)
plt.xlabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("stats", fig_stats)
# grid of distribution plots
raw_tidy = pd.melt(error_df,
value_vars=list(error_df.columns.values),
var_name="estimate",
value_name=metric_label)
col_wrap = 2 if len(args.result_files) <= 2 else math.ceil(
len(args.result_files) / 2.0)
dist_grid = sns.FacetGrid(raw_tidy, col="estimate", col_wrap=col_wrap)
# TODO: see issue #98
import warnings
with warnings.catch_warnings():
warnings.simplefilter("ignore")
dist_grid.map(sns.distplot, metric_label) # fits=stats.gamma
plot_collection.add_figure("histogram", dist_grid.fig)
################### box plot ##############################
fig_box = plt.figure(figsize=figsize)
algorithm = str(raw_tidy["estimate"])
mono_pal = {
"tx2/vins-mono": "lightpink",
"tx2/alvio": "coral",
"tx2/rovio": "lightskyblue",
"../d1": "white",
"nx/vins-mono": "lightpink",
"nx/alvio": "coral",
"nx/rovio": "lightskyblue",
"../d2": "white",
"xavier/vins-mono": "lightpink",
"xavier/alvio": "coral",
"xavier/rovio": "lightskyblue"
}
tmp_pal = {"tx2/vins-fusion-gpu": "mediumpurple"}
stereo_pal = {
"tx2/orb2": "mediumseagreen",
"tx2/vins-fusion-gpu": "mediumpurple",
"tx2/msckf-vio": "khaki",
"tx2/kimera": "indianred",
"../d1": "white",
"nx/vins-fusion": "plum",
"nx/orb2": "mediumseagreen",
"nx/vins-fusion-imu": "hotpink",
"nx/vins-fusion-gpu": "mediumpurple",
"nx/msckf-vio": "khaki",
"nx/kimera": "indianred",
"../d2": "white",
"xavier/vins-fusion": "plum",
"xavier/orb2": "mediumseagreen",
"xavier/vins-fusion-imu": "hotpink",
"xavier/vins-fusion-gpu": "mediumpurple",
"xavier/msckf-vio": "khaki",
"xavier/kimera": "indianred"
}
# print(algorithm)
print("called")
ax = sns.boxplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_box.gca(),
palette=stereo_pal,
sym='') #color="blue")
ax.set_xticklabels(
labels=[item.get_text() for item in ax.get_xticklabels()],
rotation=30)
plot_collection.add_figure("box_plot", fig_box)
################### box plot ##############################
# violin plot
fig_violin = plt.figure(figsize=figsize)
ax = sns.violinplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_violin.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
plot_collection.add_figure("violin_histogram", fig_violin)
if args.plot:
plot_collection.show()
if args.save_plot:
logger.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
def main_rpe_for_each(traj_ref,
traj_est,
pose_relation,
mode,
bins,
rel_tols,
align=False,
correct_scale=False,
ref_name="",
est_name="",
show_plot=False,
save_plot=None,
save_results=None,
no_warnings=False,
serialize_plot=None):
from evo.algorithms import metrics
from evo.algorithms import filters
from evo.algorithms import trajectory
from evo.tools import file_interface
from evo.tools.settings import SETTINGS
if not bins or not rel_tols:
raise RuntimeError(
"bins and tolerances must have more than one element")
if len(bins) != len(rel_tols):
raise RuntimeError(
"bins and tolerances must have the same number of elements")
if mode in {"speed", "angular_speed"
} and traj_est is trajectory.PosePath3D:
raise RuntimeError("timestamps are required for mode: " + mode)
bin_unit = None
if mode == "speed":
bin_unit = metrics.VelUnit.meters_per_sec
elif mode == "path":
bin_unit = metrics.Unit.meters
elif mode == "angle":
bin_unit = metrics.Unit.degrees
elif mode == "angular_speed":
bin_unit = metrics.VelUnit.degrees_per_sec
rpe_unit = None
if pose_relation is metrics.PoseRelation.translation_part:
rpe_unit = metrics.Unit.meters
elif pose_relation is metrics.PoseRelation.rotation_angle_deg:
rpe_unit = metrics.Unit.degrees
elif pose_relation is metrics.PoseRelation.rotation_angle_rad:
rpe_unit = metrics.Unit.radians
correct_only_scale = correct_scale and not align
if align or correct_scale:
logging.debug(SEP)
if correct_only_scale:
logging.debug("correcting scale...")
else:
logging.debug("aligning using Umeyama's method..." + (
" (with scale correction)" if correct_scale else ""))
traj_est = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale,
correct_only_scale)
results = []
for bin, rel_tol, in zip(bins, rel_tols):
logging.debug(SEP)
logging.info("calculating RPE for each sub-sequence of " + str(bin) +
" (" + bin_unit.value + ")")
tol = bin * rel_tol
id_pairs = []
if mode == "path":
id_pairs = filters.filter_pairs_by_path(traj_ref.poses_se3,
bin,
tol,
all_pairs=True)
elif mode == "angle":
id_pairs = filters.filter_pairs_by_angle(traj_ref.poses_se3,
bin,
tol,
degrees=True)
elif mode == "speed":
id_pairs = filters.filter_pairs_by_speed(traj_ref.poses_se3,
traj_ref.timestamps, bin,
tol)
elif mode == "angular_speed":
id_pairs = filters.filter_pairs_by_angular_speed(
traj_ref.poses_se3, traj_ref.timestamps, bin, tol, True)
if len(id_pairs) == 0:
raise RuntimeError("bin " + str(bin) + " (" + str(bin_unit.value) +
") " +
"produced empty index list - try other values")
# calculate RPE with all IDs (delta 1 frames)
data = (traj_ref, traj_est)
# the delta here has nothing to do with the bin - 1f delta just to use all poses of the bin
rpe_metric = metrics.RPE(pose_relation,
delta=1,
delta_unit=metrics.Unit.frames,
all_pairs=True)
rpe_metric.process_data(data, id_pairs)
mean = rpe_metric.get_statistic(metrics.StatisticsType.mean)
results.append(mean)
if SETTINGS.plot_usetex:
mode.replace("_", "\_")
title = "mean RPE w.r.t. " + pose_relation.value + "\nfor different " + mode + " sub-sequences"
if align and not correct_scale:
title += "\n(with SE(3) Umeyama alignment)"
elif align and correct_scale:
title += "\n(with Sim(3) Umeyama alignment)"
elif correct_only_scale:
title += "\n(scale corrected)"
else:
title += "\n(not aligned)"
logging.debug(SEP)
logging.info("\n" + title + "\n")
res_str = ""
for bin, result in zip(bins, results):
res_str += "{:>10}".format(str(bin) + "(" + bin_unit.value + ")")
res_str += "\t" + "{0:.6f}".format(result) + "\n"
logging.info(res_str)
if show_plot or save_plot or serialize_plot:
from evo.tools import plot
import matplotlib.pyplot as plt
plot_collection = plot.PlotCollection(title)
fig = plt.figure(figsize=(SETTINGS.plot_figsize[0],
SETTINGS.plot_figsize[1]))
plot.error_array(fig,
results,
x_array=bins,
name="mean RPE" +
(" (" + rpe_unit.value + ")") if rpe_unit else "",
marker="o",
title=title,
xlabel=mode + " sub-sequences " + " (" +
bin_unit.value + ")")
# info text
if SETTINGS.plot_info_text and est_name and ref_name:
ax = fig.gca()
ax.text(0,
-0.12,
"estimate: " + est_name + "\nreference: " + ref_name,
transform=ax.transAxes,
fontsize=8,
color="gray")
plt.title(title)
plot_collection.add_figure("raw", fig)
if show_plot:
plot_collection.show()
if save_plot:
plot_collection.export(save_plot,
confirm_overwrite=not no_warnings)
if serialize_plot:
logging.debug(SEP)
plot_collection.serialize(serialize_plot,
confirm_overwrite=not no_warnings)
rpe_statistics = {bin: result for bin, result in zip(bins, results)}
if save_results:
logging.debug(SEP)
# utility class to trick save_res_file
class Metric:
unit = rpe_unit
error = results
file_interface.save_res_file(save_results,
Metric,
rpe_statistics,
title,
ref_name,
est_name,
bins,
traj_ref,
traj_est,
xlabel=mode + " sub-sequences " + " (" +
bin_unit.value + ")",
confirm_overwrite=not no_warnings)
return rpe_statistics, results
def main_rpe(traj_ref,
traj_est,
pose_relation,
delta,
delta_unit,
rel_delta_tol=0.1,
all_pairs=False,
align=False,
correct_scale=False,
ref_name="",
est_name="",
show_plot=False,
save_plot=None,
plot_mode=None,
save_results=None,
no_warnings=False,
support_loop=False,
serialize_plot=None):
from evo.core import metrics, result
from evo.core import trajectory
from evo.tools import file_interface
from evo.tools.settings import SETTINGS
if (show_plot or save_plot or serialize_plot) and all_pairs:
raise metrics.MetricsException(
"all_pairs mode cannot be used with plotting functions")
only_scale = correct_scale and not align
if align or correct_scale:
logging.debug(SEP)
if only_scale:
logging.debug("correcting scale...")
else:
logging.debug("aligning using Umeyama's method..." + (
" (with scale correction)" if correct_scale else ""))
traj_est = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale, only_scale)
logging.debug(SEP)
# calculate RPE
data = (traj_ref, traj_est)
rpe_metric = metrics.RPE(pose_relation, delta, delta_unit, rel_delta_tol,
all_pairs)
rpe_metric.process_data(data)
rpe_statistics = rpe_metric.get_all_statistics()
title = str(rpe_metric)
if align and not correct_scale:
title += "\n(with SE(3) Umeyama alignment)"
elif align and correct_scale:
title += "\n(with Sim(3) Umeyama alignment)"
elif only_scale:
title += "\n(scale corrected)"
else:
title += "\n(not aligned)"
rpe_result = result.from_metric(rpe_metric, title, ref_name, est_name)
logging.debug(SEP)
logging.info(rpe_result.pretty_str())
# restrict trajectories to delta ids
if support_loop:
# avoid overwriting if called repeatedly e.g. in Jupyter notebook
import copy
traj_ref = copy.deepcopy(traj_ref)
traj_est = copy.deepcopy(traj_est)
traj_ref.reduce_to_ids(rpe_metric.delta_ids)
traj_est.reduce_to_ids(rpe_metric.delta_ids)
if isinstance(traj_est, trajectory.PoseTrajectory3D) and not all_pairs:
seconds_from_start = [
t - traj_est.timestamps[0] for t in traj_est.timestamps
]
rpe_result.add_np_array("seconds_from_start", seconds_from_start)
else:
seconds_from_start = None
if show_plot or save_plot or serialize_plot and not all_pairs:
from evo.tools import plot
import matplotlib.pyplot as plt
logging.debug(SEP)
logging.debug("plotting results... ")
fig1 = plt.figure(figsize=(SETTINGS.plot_figsize[0],
SETTINGS.plot_figsize[1]))
# metric values
plot.error_array(
fig1,
rpe_metric.error,
x_array=seconds_from_start,
statistics=rpe_statistics,
name="RPE" +
(" (" + rpe_metric.unit.value + ")") if rpe_metric.unit else "",
title=title,
xlabel="$t$ (s)" if seconds_from_start else "index")
# info text
if SETTINGS.plot_info_text and est_name and ref_name:
ax = fig1.gca()
ax.text(0,
-0.12,
"estimate: " + est_name + "\nreference: " + ref_name,
transform=ax.transAxes,
fontsize=8,
color="gray")
# trajectory colormapped
fig2 = plt.figure(figsize=(SETTINGS.plot_figsize[0],
SETTINGS.plot_figsize[1]))
plot_mode = plot_mode if plot_mode is not None else plot.PlotMode.xyz
ax = plot.prepare_axis(fig2, plot_mode)
plot.traj(ax,
plot_mode,
traj_ref,
'--',
'gray',
'reference',
alpha=0 if SETTINGS.plot_hideref else 1)
plot.traj_colormap(ax,
traj_est,
rpe_metric.error,
plot_mode,
min_map=rpe_statistics["min"],
max_map=rpe_statistics["max"],
title="RPE mapped onto trajectory")
fig2.axes.append(ax)
plot_collection = plot.PlotCollection(title)
plot_collection.add_figure("raw", fig1)
plot_collection.add_figure("map", fig2)
if show_plot:
plot_collection.show()
if save_plot:
plot_collection.export(save_plot,
confirm_overwrite=not no_warnings)
if serialize_plot:
logging.debug(SEP)
plot_collection.serialize(serialize_plot,
confirm_overwrite=not no_warnings)
if save_results:
logging.debug(SEP)
if SETTINGS.save_traj_in_zip:
rpe_result.add_trajectory("traj_ref", traj_ref)
rpe_result.add_trajectory("traj_est", traj_est)
file_interface.save_res_file(save_results,
rpe_result,
confirm_overwrite=not no_warnings)
return rpe_result
def plot_multi(args, result, traj_ref_list, traj_est_list):
from evo.tools import plot
from evo.tools.settings import SETTINGS
import matplotlib.pyplot as plt
import numpy as np
logger.debug(SEP)
logger.debug("Plotting results... ")
plot_mode = plot.PlotMode(args.plot_mode)
figs = []
# Plot the raw metric values.
error_array_comb = np.array([])
for i in range(len(result)):
figs.append(plt.figure(figsize=SETTINGS.plot_figsize))
if "seconds_from_start" in result[i].np_arrays:
seconds_from_start = result[i].np_arrays["seconds_from_start"]
else:
seconds_from_start = None
plot.error_array(
figs[i], result[i].np_arrays["error_array"], x_array=seconds_from_start,
statistics={
s: result[i].stats[s]
for s in SETTINGS.plot_statistics if s not in ("min", "max")
}, name=result[i].info["label"], title=result[i].info["title"],
xlabel="$t$ (s)" if seconds_from_start else "index")
# Plot the values color-mapped onto the trajectory.
figs.append(plt.figure(figsize=SETTINGS.plot_figsize))
ax = plot.prepare_axis(figs[-1], plot_mode)
if args.ros_map_yaml:
plot.ros_map(ax, args.ros_map_yaml, plot_mode)
if args.plot_colormap_min is None:
args.plot_colormap_min = min([result[i].stats["min"] for i in range(len(result))])
if args.plot_colormap_max is None:
args.plot_colormap_max = max([result[i].stats["max"] for i in range(len(result))])
if args.plot_colormap_max_percentile is not None:
args.plot_colormap_max = np.percentile(
error_array_comb, args.plot_colormap_max_percentile)
traj_est_list_comb = np.array([])
for i in range(len(result)):
plot.traj(ax, plot_mode, traj_ref_list[i], style=SETTINGS.plot_reference_linestyle,
color=multirobot_reference_color[i], label='reference'+str(i),
alpha=0.8)
plot.draw_coordinate_axes(ax, traj_ref_list[i], plot_mode,
SETTINGS.plot_axis_marker_scale)
plot.draw_coordinate_axes(ax, traj_est_list[i], plot_mode,
SETTINGS.plot_axis_marker_scale)
figs[-1].axes.append(ax)
plot.traj_colormap_multi(ax, traj_est_list, [r.np_arrays["error_array"] for r in result],
plot_mode, min_map=args.plot_colormap_min,
max_map=args.plot_colormap_max,
title="Error mapped onto trajectory")
plot_collection = plot.PlotCollection("Multi-robot APE analysis")
for i in range(len(result)):
plot_collection.add_figure("raw" + str(i), figs[i])
plot_collection.add_figure("map", figs[-1])
if args.plot:
plot_collection.show()
if args.save_plot:
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
def run(args):
import sys
import numpy as np
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.core.trajectory import PoseTrajectory3D
from evo.tools import file_interface, log
log.configure_logging(verbose=args.verbose,
silent=args.silent,
debug=args.debug,
local_logfile=args.logfile)
if args.debug:
import pprint
logger.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logger.debug(SEP)
trajectories, ref_traj = load_trajectories(args)
if args.merge:
if args.subcommand == "kitti":
die("Can't merge KITTI files.")
if len(trajectories) == 0:
die("No trajectories to merge (excluding --ref).")
trajectories = {
"merged_trajectory": trajectory.merge(trajectories.values())
}
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories.items():
if type(traj) is trajectory.PosePath3D:
die("{} doesn't have timestamps - can't add time offset.".
format(name))
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.n_to_align != -1 and not (args.align or args.correct_scale):
die("--n_to_align is useless without --align or/and --correct_scale")
# TODO: this is fugly, but is a quick solution for remembering each synced
# reference when plotting pose correspondences later...
synced = (args.subcommand == "kitti" and ref_traj) or any(
(args.sync, args.align, args.correct_scale, args.align_origin))
synced_refs = {}
if synced:
from evo.core import sync
if not args.ref:
logger.debug(SEP)
die("Can't align or sync without a reference! (--ref) *grunt*")
for name, traj in trajectories.items():
if args.subcommand == "kitti":
ref_traj_tmp = ref_traj
else:
logger.debug(SEP)
ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
first_name="reference",
snd_name=name)
if args.align or args.correct_scale:
logger.debug(SEP)
logger.debug("Aligning {} to reference.".format(name))
trajectories[name].align(ref_traj_tmp,
correct_scale=args.correct_scale,
correct_only_scale=args.correct_scale
and not args.align,
n=args.n_to_align)
if args.align_origin:
logger.debug(SEP)
logger.debug("Aligning {}'s origin to reference.".format(name))
trajectories[name].align_origin(ref_traj_tmp)
if SETTINGS.plot_pose_correspondences:
synced_refs[name] = ref_traj_tmp
if args.transform_left or args.transform_right:
tf_type = "left" if args.transform_left else "right"
tf_path = args.transform_left \
if args.transform_left else args.transform_right
transform = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(transform):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
transform = lie.se3_inverse(transform)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, transform))
for traj in trajectories.values():
traj.transform(transform,
right_mul=args.transform_right,
propagate=args.propagate_transform)
for name, traj in trajectories.items():
print_traj_info(to_compact_name(name, args), traj, args.verbose,
args.full_check)
if args.ref:
print_traj_info(to_compact_name(args.ref, args), ref_traj,
args.verbose, args.full_check)
if args.plot or args.save_plot or args.serialize_plot:
import numpy as np
from evo.tools import plot
import matplotlib.pyplot as plt
import matplotlib.cm as cm
plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
fig_xyz, axarr_xyz = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_rpy, axarr_rpy = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
plot_mode = plot.PlotMode[args.plot_mode]
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
# for x-axis alignment starting from 0 with --plot_relative_time
start_time = None
if args.ref:
if isinstance(ref_traj, trajectory.PoseTrajectory3D) \
and args.plot_relative_time:
start_time = ref_traj.timestamps[0]
short_traj_name = to_compact_name(args.ref, args,
SETTINGS.plot_usetex)
plot.traj(ax_traj,
plot_mode,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
plot.draw_coordinate_axes(
ax_traj, ref_traj, plot_mode,
SETTINGS.plot_reference_axis_marker_scale)
plot.traj_xyz(axarr_xyz,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start_timestamp=start_time)
plot.traj_rpy(axarr_rpy,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start_timestamp=start_time)
if args.ros_map_yaml:
plot.ros_map(ax_traj, args.ros_map_yaml, plot_mode)
cmap_colors = None
if SETTINGS.plot_multi_cmap.lower() != "none":
cmap = getattr(cm, SETTINGS.plot_multi_cmap)
cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))
for name, traj in trajectories.items():
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
short_traj_name = to_compact_name(name, args, SETTINGS.plot_usetex)
plot.traj(ax_traj,
plot_mode,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha)
plot.draw_coordinate_axes(ax_traj, traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
if ref_traj and synced and SETTINGS.plot_pose_correspondences:
plot.draw_correspondence_edges(
ax_traj,
traj,
synced_refs[name],
plot_mode,
color=color,
style=SETTINGS.plot_pose_correspondences_linestyle,
alpha=SETTINGS.plot_trajectory_alpha)
plot.traj_xyz(axarr_xyz,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
plot.traj_rpy(axarr_rpy,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
if not SETTINGS.plot_usetex:
fig_rpy.text(0.,
0.005,
"euler_angle_sequence: {}".format(
SETTINGS.euler_angle_sequence),
fontsize=6)
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
if args.plot:
plot_collection.show()
if args.save_plot:
logger.info(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.info(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logger.info(SEP)
for name, traj in trajectories.items():
dest = to_filestem(name, args) + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = to_filestem(args.ref, args) + ".tum"
file_interface.write_tum_trajectory_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_kitti:
logger.info(SEP)
for name, traj in trajectories.items():
dest = to_filestem(name, args) + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = to_filestem(args.ref, args) + ".kitti"
file_interface.write_kitti_poses_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_bag or args.save_as_bag2:
from rosbags.rosbag1 import Writer as Rosbag1Writer
from rosbags.rosbag2 import Writer as Rosbag2Writer
writers = []
if args.save_as_bag:
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
writers.append(Rosbag1Writer(dest_bag_path))
if args.save_as_bag2:
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S"))
writers.append(Rosbag2Writer(dest_bag_path))
for writer in writers:
logger.info(SEP)
logger.info("Saving trajectories to " + str(writer.path) + "...")
try:
writer.open()
for name, traj in trajectories.items():
dest_topic = to_topic_name(name, args)
frame_id = traj.meta[
"frame_id"] if "frame_id" in traj.meta else ""
file_interface.write_bag_trajectory(
writer, traj, dest_topic, frame_id)
if args.ref:
dest_topic = to_topic_name(args.ref, args)
frame_id = ref_traj.meta[
"frame_id"] if "frame_id" in ref_traj.meta else ""
file_interface.write_bag_trajectory(
writer, ref_traj, dest_topic, frame_id)
finally:
writer.close()
if args.save_table:
from evo.tools import pandas_bridge
logger.debug(SEP)
df = pandas_bridge.trajectories_stats_to_df(trajectories)
pandas_bridge.save_df_as_table(df,
args.save_table,
confirm_overwrite=not args.no_warnings)
def run_analysis(traj_ref_path,
traj_est_path,
segments,
save_results,
display_plot,
save_plots,
save_folder,
confirm_overwrite=False,
dataset_name="",
discard_n_start_poses=0,
discard_n_end_poses=0):
""" Run analysis on given trajectories, saves plots on given path:
:param traj_ref_path: path to the reference (ground truth) trajectory.
:param traj_est_path: path to the estimated trajectory.
:param save_results: saves APE, and RPE per segment results.
:param save_plots: whether to save the plots.
:param save_folder: where to save the plots.
:param confirm_overwrite: whether to confirm overwriting plots or not.
:param dataset_name: optional param, to allow setting the same scale on different plots.
"""
# Load trajectories.
from evo.tools import file_interface
traj_ref = None
try:
traj_ref = file_interface.read_euroc_csv_trajectory(
traj_ref_path) # TODO make it non-euroc specific.
except file_interface.FileInterfaceException as e:
raise Exception(
"\033[91mMissing ground truth csv! \033[93m {}.".format(e))
traj_est = None
try:
traj_est = file_interface.read_swe_csv_trajectory(traj_est_path)
except file_interface.FileInterfaceException as e:
log.info(e)
raise Exception("\033[91mMissing vio output csv.\033[99m")
evt.print_purple("Registering trajectories")
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
evt.print_purple("Aligning trajectories")
traj_est = trajectory.align_trajectory(
traj_est,
traj_ref,
correct_scale=False,
discard_n_start_poses=int(discard_n_start_poses),
discard_n_end_poses=int(discard_n_end_poses))
num_of_poses = traj_est.num_poses
traj_est.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
traj_ref.reduce_to_ids(
range(int(discard_n_start_poses),
int(num_of_poses - discard_n_end_poses), 1))
results = dict()
evt.print_purple("Calculating APE translation part")
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_result = ape_metric.get_result()
results["absolute_errors"] = ape_result
log.info(ape_result.pretty_str(info=True))
# TODO(Toni): Save RPE computation results rather than the statistics
# you can compute statistics later...
evt.print_purple("Calculating RPE translation part for plotting")
rpe_metric_trans = metrics.RPE(metrics.PoseRelation.translation_part, 1.0,
metrics.Unit.frames, 0.0, False)
rpe_metric_trans.process_data(data)
rpe_stats_trans = rpe_metric_trans.get_all_statistics()
log.info("mean: %f" % rpe_stats_trans["mean"])
evt.print_purple("Calculating RPE rotation angle for plotting")
rpe_metric_rot = metrics.RPE(metrics.PoseRelation.rotation_angle_deg, 1.0,
metrics.Unit.frames, 1.0, False)
rpe_metric_rot.process_data(data)
rpe_stats_rot = rpe_metric_rot.get_all_statistics()
log.info("mean: %f" % rpe_stats_rot["mean"])
results["relative_errors"] = dict()
# Read segments file
for segment in segments:
results["relative_errors"][segment] = dict()
evt.print_purple("RPE analysis of segment: %d" % segment)
evt.print_lightpurple("Calculating RPE segment translation part")
rpe_segment_metric_trans = metrics.RPE(
metrics.PoseRelation.translation_part, float(segment),
metrics.Unit.meters, 0.01, True)
rpe_segment_metric_trans.process_data(data)
rpe_segment_stats_trans = rpe_segment_metric_trans.get_all_statistics()
results["relative_errors"][segment][
"rpe_trans"] = rpe_segment_stats_trans
# print(rpe_segment_stats_trans)
# print("mean:", rpe_segment_stats_trans["mean"])
evt.print_lightpurple("Calculating RPE segment rotation angle")
rpe_segment_metric_rot = metrics.RPE(
metrics.PoseRelation.rotation_angle_deg, float(segment),
metrics.Unit.meters, 0.01, True)
rpe_segment_metric_rot.process_data(data)
rpe_segment_stats_rot = rpe_segment_metric_rot.get_all_statistics()
results["relative_errors"][segment]["rpe_rot"] = rpe_segment_stats_rot
# print(rpe_segment_stats_rot)
# print("mean:", rpe_segment_stats_rot["mean"])
if save_results:
# Save results file
results_file = os.path.join(save_folder, 'results.yaml')
evt.print_green("Saving analysis results to: %s" % results_file)
with open(results_file, 'w') as outfile:
if confirm_overwrite:
if evt.user.check_and_confirm_overwrite(results_file):
outfile.write(yaml.dump(results, default_flow_style=False))
else:
log.info("Not overwritting results.")
else:
outfile.write(yaml.dump(results, default_flow_style=False))
# For each segment in segments file
# Calculate rpe with delta = segment in meters with all-pairs set to True
# Calculate max, min, rmse, mean, median etc
# Plot boxplot, or those cumulative figures you see in evo (like demographic plots)
if display_plot or save_plots:
evt.print_green("Plotting:")
log.info(dataset_name)
plot_collection = plot.PlotCollection("Example")
# metric values
fig_1 = plt.figure(figsize=(8, 8))
ymax = -1
if dataset_name is not "" and FIX_MAX_Y:
ymax = Y_MAX_APE_TRANS[dataset_name]
ape_statistics = ape_metric.get_all_statistics()
plot.error_array(
fig_1,
ape_metric.error,
statistics=ape_statistics,
name="APE translation",
title="" #str(ape_metric)
,
xlabel="Keyframe index [-]",
ylabel="APE translation [m]",
y_min=0.0,
y_max=ymax)
plot_collection.add_figure("APE_translation", fig_1)
# trajectory colormapped with error
fig_2 = plt.figure(figsize=(8, 8))
plot_mode = plot.PlotMode.xy
ax = plot.prepare_axis(fig_2, plot_mode)
plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
plot.traj_colormap(ax,
traj_est,
ape_metric.error,
plot_mode,
min_map=0.0,
max_map=math.ceil(ape_statistics['max'] * 10) / 10,
title="ATE mapped onto trajectory [m]")
plot_collection.add_figure("APE_translation_trajectory_error", fig_2)
# RPE
## Trans
### metric values
fig_3 = plt.figure(figsize=(8, 8))
if dataset_name is not "" and FIX_MAX_Y:
ymax = Y_MAX_RPE_TRANS[dataset_name]
plot.error_array(
fig_3,
rpe_metric_trans.error,
statistics=rpe_stats_trans,
name="RPE translation",
title="" #str(rpe_metric_trans)
,
xlabel="Keyframe index [-]",
ylabel="RPE translation [m]",
y_max=ymax)
plot_collection.add_figure("RPE_translation", fig_3)
### trajectory colormapped with error
fig_4 = plt.figure(figsize=(8, 8))
plot_mode = plot.PlotMode.xy
ax = plot.prepare_axis(fig_4, plot_mode)
traj_ref_trans = copy.deepcopy(traj_ref)
traj_ref_trans.reduce_to_ids(rpe_metric_trans.delta_ids)
traj_est_trans = copy.deepcopy(traj_est)
traj_est_trans.reduce_to_ids(rpe_metric_trans.delta_ids)
plot.traj(ax, plot_mode, traj_ref_trans, '--', 'gray', 'Reference')
plot.traj_colormap(
ax,
traj_est_trans,
rpe_metric_trans.error,
plot_mode,
min_map=0.0,
max_map=math.ceil(rpe_stats_trans['max'] * 10) / 10,
title="RPE translation error mapped onto trajectory [m]")
plot_collection.add_figure("RPE_translation_trajectory_error", fig_4)
## Rot
### metric values
fig_5 = plt.figure(figsize=(8, 8))
if dataset_name is not "" and FIX_MAX_Y:
ymax = Y_MAX_RPE_ROT[dataset_name]
plot.error_array(
fig_5,
rpe_metric_rot.error,
statistics=rpe_stats_rot,
name="RPE rotation error",
title="" #str(rpe_metric_rot)
,
xlabel="Keyframe index [-]",
ylabel="RPE rotation [deg]",
y_max=ymax)
plot_collection.add_figure("RPE_rotation", fig_5)
### trajectory colormapped with error
fig_6 = plt.figure(figsize=(8, 8))
plot_mode = plot.PlotMode.xy
ax = plot.prepare_axis(fig_6, plot_mode)
traj_ref_rot = copy.deepcopy(traj_ref)
traj_ref_rot.reduce_to_ids(rpe_metric_rot.delta_ids)
traj_est_rot = copy.deepcopy(traj_est)
traj_est_rot.reduce_to_ids(rpe_metric_rot.delta_ids)
plot.traj(ax, plot_mode, traj_ref_rot, '--', 'gray', 'Reference')
plot.traj_colormap(
ax,
traj_est_rot,
rpe_metric_rot.error,
plot_mode,
min_map=0.0,
max_map=math.ceil(rpe_stats_rot['max'] * 10) / 10,
title="RPE rotation error mapped onto trajectory [deg]")
plot_collection.add_figure("RPE_rotation_trajectory_error", fig_6)
if display_plot:
evt.print_green("Displaying plots.")
plot_collection.show()
if save_plots:
evt.print_green("Saving plots to: ")
log.info(save_folder)
# Config output format (pdf, eps, ...) using evo_config...
plot_collection.export(os.path.join(save_folder, "plots.eps"),
False)
plot_collection.export(os.path.join(save_folder, "plots.pdf"),
False)
def run(args):
import os
import sys
import logging
import evo.algorithms.lie_algebra as lie
from evo.algorithms import trajectory
from evo.tools import file_interface, settings
from evo.tools.settings import SETTINGS
settings.configure_logging(verbose=True,
silent=args.silent,
debug=args.debug)
if args.debug:
import pprint
logging.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logging.debug(SEP)
trajectories = []
ref_traj = None
if args.subcommand == "tum":
for traj_file in args.traj_files:
if traj_file != args.ref:
trajectories.append(
(traj_file,
file_interface.read_tum_trajectory_file(traj_file)))
if args.ref:
ref_traj = file_interface.read_tum_trajectory_file(args.ref)
elif args.subcommand == "kitti":
for pose_file in args.pose_files:
if pose_file != args.ref:
trajectories.append(
(pose_file,
file_interface.read_kitti_poses_file(pose_file)))
if args.ref:
ref_traj = file_interface.read_kitti_poses_file(args.ref)
elif args.subcommand == "euroc":
for csv_file in args.state_gt_csv:
if csv_file != args.ref:
trajectories.append(
(csv_file,
file_interface.read_euroc_csv_trajectory(csv_file)))
if args.ref:
ref_traj = file_interface.read_euroc_csv_trajectory(args.ref)
elif args.subcommand == "bag":
import rosbag
bag = rosbag.Bag(args.bag)
try:
if args.all_topics:
topic_info = bag.get_type_and_topic_info()
topics = sorted([
t for t in topic_info[1].keys()
if topic_info[1][t][0] == "geometry_msgs/PoseStamped"
and t != args.ref
])
if len(topics) == 0:
logging.error("no geometry_msgs/PoseStamped topics found!")
sys.exit(1)
else:
topics = args.topics
if not topics:
logging.warning(
"no topics used - specify topics or use the --all_topics flag"
)
sys.exit(1)
for topic in topics:
trajectories.append(
(topic, file_interface.read_bag_trajectory(bag, topic)))
if args.ref:
ref_traj = file_interface.read_bag_trajectory(bag, args.ref)
finally:
bag.close()
else:
raise RuntimeError("unsupported subcommand: " + args.subcommand)
if args.transform_left or args.transform_right:
tf_path = args.transform_left if args.transform_left else args.transform_right
t, xyz, quat = file_interface.load_transform_json(tf_path)
logging.debug(SEP)
logging.debug("applying transformation to the trajectories:\n" +
str(t))
if args.invert_transform:
t = lie.se3_inverse(t)
for name, traj in trajectories:
traj.transform(t, right_mul=args.transform_right)
if args.align or args.correct_scale:
if args.ref:
if args.subcommand == "kitti":
traj_tmp, ref_traj_tmp = trajectories, [
ref_traj for n, t in trajectories
]
else:
traj_tmp, ref_traj_tmp = [], []
from evo.algorithms import sync
for name, traj in trajectories:
logging.debug(SEP)
ref_assoc, traj_assoc = sync.associate_trajectories(
ref_traj, traj, first_name="ref", snd_name=name)
ref_traj_tmp.append(ref_assoc)
traj_tmp.append((name, traj_assoc))
trajectories = traj_tmp
correct_only_scale = args.correct_scale and not args.align
trajectories_new = []
for nt, ref_assoc in zip(trajectories, ref_traj_tmp):
logging.debug(SEP)
logging.debug("aligning " + nt[0] + " to " + args.ref + "...")
trajectories_new.append(
(nt[0],
trajectory.align_trajectory(nt[1], ref_assoc,
args.correct_scale,
correct_only_scale,
args.n_to_align)))
trajectories = trajectories_new
for name, traj in trajectories:
if args.t_offset and traj.timestamps.shape[0] != 0:
logging.debug(SEP)
logging.info("adding time offset to " + name + ": " +
str(args.t_offset) + " (s)")
traj.timestamps += args.t_offset
print_traj_info(name, traj, args.full_check)
if (args.align or args.correct_scale) and not args.ref:
logging.debug(SEP)
logging.warning("can't align without a reference! (--ref) *grunt*")
if args.ref:
print_traj_info(args.ref, ref_traj, args.full_check)
if args.plot or args.save_plot or args.serialize_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
import numpy as np
from evo.tools import plot
import matplotlib.pyplot as plt
import matplotlib.cm as cm
plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
fig_xyz, axarr_xyz = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
if (args.align or args.correct_scale) and not args.ref:
plt.xkcd(scale=2, randomness=4)
fig_traj.suptitle("what if --ref?")
fig_xyz.suptitle("what if --ref?")
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
if args.ref:
short_traj_name = os.path.splitext(os.path.basename(args.ref))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
ref_traj,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
plot.traj_xyz(axarr_xyz,
ref_traj,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
cmap_colors = None
if SETTINGS.plot_multi_cmap.lower() != "none":
cmap = getattr(cm, SETTINGS.plot_multi_cmap)
cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))
for name, traj in trajectories:
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
short_traj_name = os.path.splitext(os.path.basename(name))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj, plot_mode, traj, '-', color, short_traj_name)
if args.ref and isinstance(ref_traj, trajectory.PoseTrajectory3D):
start_time = ref_traj.timestamps[0]
else:
start_time = None
plot.traj_xyz(axarr_xyz,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time)
plt.tight_layout()
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
if args.plot:
plot_collection.show()
if args.save_plot:
logging.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logging.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logging.debug(SEP)
for name, traj in trajectories:
dest = os.path.splitext(os.path.basename(name))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_kitti:
logging.debug(SEP)
for name, traj in trajectories:
dest = os.path.splitext(os.path.basename(name))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_bag:
logging.debug(SEP)
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S.%f')) + ".bag"
logging.debug("saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories:
dest_topic = os.path.splitext(os.path.basename(name))[0]
file_interface.write_bag_trajectory(bag, traj, dest_topic)
if args.ref:
dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
file_interface.write_bag_trajectory(bag, ref_traj, dest_topic)
finally:
bag.close()
def run(args):
import sys
import pandas as pd
from evo.tools import log, user, settings
from evo.tools.settings import SETTINGS
pd.options.display.width = 80
pd.options.display.max_colwidth = 20
log.configure_logging(args.verbose,
args.silent,
args.debug,
local_logfile=args.logfile)
if args.debug:
import pprint
arg_dict = {arg: getattr(args, arg) for arg in vars(args)}
# logger.debug("main_parser config:\n{}\n".format(
# pprint.pformat(arg_dict)))
df = load_results_as_dataframe(args.result_files, args.use_filenames,
args.merge)
keys = df.columns.values.tolist()
if SETTINGS.plot_usetex:
keys = [key.replace("_", "\\_") for key in keys]
df.columns = keys
duplicates = [x for x in keys if keys.count(x) > 1]
if duplicates:
# logger.error("Values of 'est_name' must be unique - duplicates: {}\n"
# "Try using the --use_filenames option to use filenames "
# "for labeling instead.".format(", ".join(duplicates)))
sys.exit(1)
# derive a common index type if possible - preferably timestamps
common_index = None
time_indices = ["timestamps", "seconds_from_start", "sec_from_start"]
if args.use_rel_time:
del time_indices[0]
for idx in time_indices:
if idx not in df.loc["np_arrays"].index:
continue
if df.loc["np_arrays", idx].isnull().values.any():
continue
else:
common_index = idx
break
# build error_df (raw values) according to common_index
if common_index is None:
# use a non-timestamp index
error_df = pd.DataFrame(df.loc["np_arrays", "error_array"].tolist(),
index=keys).T
else:
error_df = pd.DataFrame()
for key in keys:
new_error_df = pd.DataFrame(
{key: df.loc["np_arrays", "error_array"][key]},
index=df.loc["np_arrays", common_index][key])
duplicates = new_error_df.index.duplicated(keep="first")
if any(duplicates):
# logger.warning(
# "duplicate indices in error array of {} - "
# "keeping only first occurrence of duplicates".format(key))
new_error_df = new_error_df[~duplicates]
error_df = pd.concat([error_df, new_error_df], axis=1)
# check titles
first_title = df.loc["info", "title"][0] if not args.ignore_title else ""
first_file = args.result_files[0]
if not args.no_warnings and not args.ignore_title:
checks = df.loc["info", "title"] != first_title
for i, differs in enumerate(checks):
if not differs:
continue
else:
mismatching_title = df.loc["info", "title"][i]
mismatching_file = args.result_files[i]
# logger.debug(SEP)
# logger.warning(
# CONFLICT_TEMPLATE.format(first_file, first_title,
# mismatching_title,
# mismatching_file))
if not user.confirm(
"You can use --ignore_title to just aggregate data.\n"
"Go on anyway? - enter 'y' or any other key to exit"):
sys.exit()
# logger.debug(SEP)
# logger.debug("Aggregated dataframe:\n{}".format(
# df.to_string(line_width=80)))
# show a statistics overview
# logger.debug(SEP)
# if not args.ignore_title:
# logger.info("\n" + first_title + "\n\n")
# logger.info(df.loc["stats"].T.to_string(line_width=80) + "\n")
if args.save_table:
# logger.debug(SEP)
if args.no_warnings or user.check_and_confirm_overwrite(
args.save_table):
if SETTINGS.table_export_data.lower() == "error_array":
data = error_df
elif SETTINGS.table_export_data.lower() in ("info", "stats"):
data = df.loc[SETTINGS.table_export_data.lower()]
else:
raise ValueError(
"unsupported export data specifier: {}".format(
SETTINGS.table_export_data))
if SETTINGS.table_export_transpose:
data = data.T
if SETTINGS.table_export_format == "excel":
writer = pd.ExcelWriter(args.save_table)
data.to_excel(writer)
writer.save()
writer.close()
else:
getattr(data,
"to_" + SETTINGS.table_export_format)(args.save_table)
# logger.debug("{} table saved to: {}".format(
# SETTINGS.table_export_format, args.save_table))
if args.plot or args.save_plot or args.serialize_plot:
# check if data has NaN "holes" due to different indices
inconsistent = error_df.isnull().values.any()
# if inconsistent and common_index != "timestamps" and not args.no_warnings:
# logger.debug(SEP)
# logger.warning("Data lengths/indices are not consistent, "
# "raw value plot might not be correctly aligned")
from evo.tools import plot
import matplotlib.pyplot as plt
import seaborn as sns
import math
# use default plot settings
figsize = (SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])
use_cmap = SETTINGS.plot_multi_cmap.lower() != "none"
colormap = SETTINGS.plot_multi_cmap if use_cmap else None
linestyles = ["-o" for x in args.result_files
] if args.plot_markers else None
# labels according to first dataset
if "xlabel" in df.loc["info"].index and not df.loc[
"info", "xlabel"].isnull().values.any():
index_label = df.loc["info", "xlabel"][0]
else:
index_label = "$t$ (s)" if common_index else "index"
metric_label = df.loc["info", "label"][0]
plot_collection = plot.PlotCollection(first_title)
# raw value plot
fig_raw = plt.figure(figsize=figsize)
# handle NaNs from concat() above
error_df.interpolate(method="index").plot(
ax=fig_raw.gca(),
colormap=colormap,
style=linestyles,
title=first_title,
alpha=SETTINGS.plot_trajectory_alpha)
plt.xlabel(index_label)
plt.ylabel(metric_label)
plt.legend(frameon=True)
plot_collection.add_figure("raw", fig_raw)
name = "test"
plt.savefig("/home/kostas/report/figures/appendix_stats/" + name +
"_raw.png",
dpi=300,
format='png',
bbox_inches='tight')
# statistics plot
if SETTINGS.plot_statistics:
fig_stats = plt.figure(figsize=figsize)
include = df.loc["stats"].index.isin(SETTINGS.plot_statistics)
if any(include):
df.loc["stats"][include].plot(kind="barh",
ax=fig_stats.gca(),
colormap=colormap,
stacked=False)
plt.xlabel(metric_label)
plt.legend(frameon=True)
# df.loc["stats"][include].plot(kind="barh", ax=axarr[0,1],
# colormap=colormap, stacked=False)
plt.savefig("/home/kostas/report/figures/appendix_stats/" +
name + "_statistics.png",
dpi=300,
format='png',
bbox_inches='tight')
plot_collection.add_figure("stats", fig_stats)
# grid of distribution plots
raw_tidy = pd.melt(error_df,
value_vars=list(error_df.columns.values),
var_name="estimate",
value_name=metric_label)
col_wrap = 2 if len(args.result_files) <= 2 else math.ceil(
len(args.result_files) / 2.0)
dist_grid = sns.FacetGrid(raw_tidy, col="estimate", col_wrap=col_wrap)
# TODO: see issue #98
import warnings
with warnings.catch_warnings():
warnings.simplefilter("ignore")
dist_grid.map(sns.distplot, metric_label) # fits=stats.gamma
plt.savefig("/home/kostas/report/figures/appendix_stats/" + name +
"_distributions.png",
dpi=300,
format='png',
bbox_inches='tight')
plot_collection.add_figure("histogram", dist_grid.fig)
# box plot
fig_box = plt.figure(figsize=figsize)
ax = sns.boxplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_box.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
plt.savefig("/home/kostas/report/figures/appendix_stats/" + name +
"_boxes.png",
dpi=300,
format='png',
bbox_inches='tight')
plot_collection.add_figure("box_plot", fig_box)
# violin plot
fig_violin = plt.figure(figsize=figsize)
ax = sns.violinplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label],
ax=fig_violin.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
plot_collection.add_figure("violin_histogram", fig_violin)
if args.plot:
plot_collection.show()
if args.save_plot:
# logger.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
# logger.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.latex_plot:
# check if data has NaN "holes" due to different indices
inconsistent = error_df.isnull().values.any()
# if inconsistent and common_index != "timestamps" and not args.no_warnings:
# logger.debug(SEP)
# logger.warning("Data lengths/indices are not consistent, "
# "raw value plot might not be correctly aligned")
from evo.tools import plot
import matplotlib.pyplot as plt
import seaborn as sns
import math
# use default plot settings
# figsize = (SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])
use_cmap = SETTINGS.plot_multi_cmap.lower() != "none"
colormap = SETTINGS.plot_multi_cmap if use_cmap else None
linestyles = ["-o" for x in args.result_files
] if args.plot_markers else None
# labels according to first dataset
if "xlabel" in df.loc["info"].index and not df.loc[
"info", "xlabel"].isnull().values.any():
index_label = df.loc["info", "xlabel"][0]
else:
index_label = "$t$ (s)" if common_index else "index"
metric_label = df.loc["info", "label"][0]
plt.style.use('seaborn-whitegrid')
lfig, axes = plt.subplots(2, 2, figsize=(6.125, 4))
# plot_collection = plot.PlotCollection(first_title)
# raw value plot
# fig_raw = plt.figure(figsize=figsize)
# handle NaNs from concat() above
# error_df.interpolate(method="index").plot(
# ax=fig_raw.gca(), colormap=colormap, style=linestyles,
# title=first_title, alpha=SETTINGS.plot_trajectory_alpha)
error_df.interpolate(method="index").plot(
ax=axes[0, 0],
colormap=colormap,
style=linestyles,
title="Absolute Position Error",
alpha=SETTINGS.plot_trajectory_alpha,
legend=False)
plt.xlabel(index_label)
plt.ylabel(metric_label)
# plt.legend(frameon=True)
# plot_collection.add_figure("raw", fig_raw)
name = "test"
# plt.savefig("/home/kostas/report/figures/appendix_stats/"+name+"_raw.png",
# dpi = 300, format='png', bbox_inches='tight')
# statistics plot
if SETTINGS.plot_statistics:
# fig_stats = plt.figure(figsize=figsize)
include = df.loc["stats"].index.isin(SETTINGS.plot_statistics)
if any(include):
df.loc["stats"][include].plot(kind="barh",
ax=axes[0, 1],
colormap=colormap,
stacked=False,
legend=False)
plt.xlabel(metric_label)
# plt.legend(frameon=True)
# df.loc["stats"][include].plot(kind="barh", ax=axarr[0,1],
# colormap=colormap, stacked=False)
# plot_collection.add_figure("stats", fig_stats)
# grid of distribution plots
raw_tidy = pd.melt(error_df,
value_vars=list(error_df.columns.values),
var_name="estimate",
value_name=metric_label)
col_wrap = 2 if len(args.result_files) <= 2 else math.ceil(
len(args.result_files) / 2.0)
# axes[1,0] = sns.FacetGrid(raw_tidy, col="estimate", col_wrap=col_wrap)
# # TODO: see issue #98
# import warnings
# with warnings.catch_warnings():
# warnings.simplefilter("ignore")
# dist_grid.map(sns.distplot, metric_label) # fits=stats.gamma
# plot_collection.add_figure("histogram", dist_grid.fig)
# box plot
# fig_box = plt.figure(figsize=figsize)
# ax = sns.boxplot(x=raw_tidy["estimate"], y=raw_tidy[metric_label],
# ax=fig_box.gca())
axes[1, 1] = sns.boxplot(x=raw_tidy["estimate"],
y=raw_tidy[metric_label])
# plt.waitforbuttonpress()
plt.savefig("/home/kostas/results/test.png",
dpi=300,
format='png',
bbox_inches='tight')
# plt.savefig("/home/kostas/report/figures/appendix_stats/test.png",
# dpi = 300, format='png', bbox_inches='tight')
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
# plt.savefig("/home/kostas/report/figures/appendix_stats/"+name+"_boxes.png",
# dpi = 300, format='png', bbox_inches='tight')
# plot_collection.add_figure("box_plot", fig_box)
# violin plot
# fig_violin = plt.figure(figsize=figsize)
# ax = sns.violinplot(x=raw_tidy["estimate"], y=raw_tidy[metric_label],
# ax=fig_violin.gca())
# ax.set_xticklabels(labels=[item.get_text() for item in ax.get_xticklabels()], rotation=30)
# plot_collection.add_figure("violin_histogram", fig_violin)
if args.plot:
plot_collection.show()
if args.save_plot:
# logger.debug(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
# logger.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
def run(args):
import os
import sys
import numpy as np
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.core.trajectory import PoseTrajectory3D
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(verbose=args.verbose,
silent=args.silent,
debug=args.debug)
if args.debug:
import pprint
logger.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logger.debug(SEP)
trajectories = []
ref_traj = None
if args.subcommand == "tum":
for traj_file in args.traj_files:
if traj_file != args.ref:
trajectories.append(
(traj_file,
file_interface.read_tum_trajectory_file(traj_file)))
if args.ref:
ref_traj = file_interface.read_tum_trajectory_file(args.ref)
elif args.subcommand == "kitti":
for pose_file in args.pose_files:
if pose_file != args.ref:
trajectories.append(
(pose_file,
file_interface.read_kitti_poses_file(pose_file)))
if args.ref:
ref_traj = file_interface.read_kitti_poses_file(args.ref)
elif args.subcommand == "euroc":
for csv_file in args.state_gt_csv:
if csv_file != args.ref:
trajectories.append(
(csv_file,
file_interface.read_euroc_csv_trajectory(csv_file)))
if args.ref:
ref_traj = file_interface.read_euroc_csv_trajectory(args.ref)
elif args.subcommand == "bag":
import rosbag
bag = rosbag.Bag(args.bag)
try:
if args.all_topics:
topic_info = bag.get_type_and_topic_info()
topics = sorted([
t for t in topic_info[1].keys()
if topic_info[1][t][0] == "geometry_msgs/PoseStamped"
and t != args.ref
])
if len(topics) == 0:
logger.error("No geometry_msgs/PoseStamped topics found!")
sys.exit(1)
else:
topics = args.topics
if not topics:
logger.warning(
"No topics used - specify topics or set --all_topics.")
sys.exit(1)
for topic in topics:
trajectories.append(
(topic, file_interface.read_bag_trajectory(bag, topic)))
if args.ref:
ref_traj = file_interface.read_bag_trajectory(bag, args.ref)
finally:
bag.close()
else:
raise RuntimeError("unsupported subcommand: " + args.subcommand)
if args.merge:
if args.subcommand == "kitti":
raise TypeError(
"can't merge KITTI files - but you can append them with 'cat'")
if len(trajectories) == 0:
raise RuntimeError("no trajectories to merge (excluding --ref)")
merged_stamps = trajectories[0][1].timestamps
merged_xyz = trajectories[0][1].positions_xyz
merged_quat = trajectories[0][1].orientations_quat_wxyz
for _, traj in trajectories[1:]:
merged_stamps = np.concatenate((merged_stamps, traj.timestamps))
merged_xyz = np.concatenate((merged_xyz, traj.positions_xyz))
merged_quat = np.concatenate(
(merged_quat, traj.orientations_quat_wxyz))
order = merged_stamps.argsort()
merged_stamps = merged_stamps[order]
merged_xyz = merged_xyz[order]
merged_quat = merged_quat[order]
trajectories = [("merged_trajectory",
PoseTrajectory3D(merged_xyz, merged_quat,
merged_stamps))]
if args.transform_left or args.transform_right:
tf_type = "left" if args.transform_left else "right"
tf_path = args.transform_left if args.transform_left else args.transform_right
t, xyz, quat = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(t):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
t = lie.se3_inverse(t)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, t))
for name, traj in trajectories:
traj.transform(t, right_mul=args.transform_right)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories:
if type(traj) is trajectory.PosePath3D:
logger.warning(
"{} doesn't have timestamps - can't add time offset.".
format(name))
else:
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.align or args.correct_scale:
if not args.ref:
logger.debug(SEP)
logger.warning("Can't align without a reference! (--ref) *grunt*")
else:
if args.subcommand == "kitti":
traj_tmp, ref_traj_tmp = trajectories, [
ref_traj for n, t in trajectories
]
else:
traj_tmp, ref_traj_tmp = [], []
from evo.core import sync
for name, traj in trajectories:
logger.debug(SEP)
ref_assoc, traj_assoc = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
first_name="ref",
snd_name=name)
ref_traj_tmp.append(ref_assoc)
traj_tmp.append((name, traj_assoc))
trajectories = traj_tmp
correct_only_scale = args.correct_scale and not args.align
trajectories_new = []
for nt, ref_assoc in zip(trajectories, ref_traj_tmp):
logger.debug(SEP)
logger.debug("Aligning " + nt[0] + " to " + args.ref + "...")
trajectories_new.append(
(nt[0],
trajectory.align_trajectory(nt[1], ref_assoc,
args.correct_scale,
correct_only_scale,
args.n_to_align)))
trajectories = trajectories_new
for name, traj in trajectories:
print_traj_info(name, traj, args.verbose, args.full_check)
if args.ref:
print_traj_info(args.ref, ref_traj, args.verbose, args.full_check)
if args.plot or args.save_plot or args.serialize_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
import numpy as np
from evo.tools import plot
import matplotlib.pyplot as plt
import matplotlib.cm as cm
plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
fig_xyz, axarr_xyz = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_rpy, axarr_rpy = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
if (args.align or args.correct_scale) and not args.ref:
plt.xkcd(scale=2, randomness=4)
fig_traj.suptitle("what if --ref?")
fig_xyz.suptitle("what if --ref?")
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
if args.ref:
short_traj_name = os.path.splitext(os.path.basename(args.ref))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
ref_traj,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
plot.traj_xyz(axarr_xyz,
ref_traj,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
plot.traj_rpy(axarr_rpy,
ref_traj,
'--',
'grey',
short_traj_name,
alpha=0 if SETTINGS.plot_hideref else 1)
cmap_colors = None
if SETTINGS.plot_multi_cmap.lower() != "none":
cmap = getattr(cm, SETTINGS.plot_multi_cmap)
cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))
for name, traj in trajectories:
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
short_traj_name = os.path.splitext(os.path.basename(name))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj, plot_mode, traj, '-', color, short_traj_name)
if args.ref and isinstance(ref_traj, trajectory.PoseTrajectory3D):
start_time = ref_traj.timestamps[0]
else:
start_time = None
plot.traj_xyz(axarr_xyz,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time)
plot.traj_rpy(axarr_rpy,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time)
plt.tight_layout()
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
if args.plot:
plot_collection.show()
if args.save_plot:
logger.info(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.info(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logger.info(SEP)
for name, traj in trajectories:
dest = os.path.splitext(os.path.basename(name))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_kitti:
logger.info(SEP)
for name, traj in trajectories:
dest = os.path.splitext(os.path.basename(name))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_bag:
logger.info(SEP)
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S.%f')) + ".bag"
logger.info("Saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories:
dest_topic = os.path.splitext(os.path.basename(name))[0]
frame_id = traj.meta[
"frame_id"] if "frame_id" in traj.meta else ""
file_interface.write_bag_trajectory(bag, traj, dest_topic,
frame_id)
if args.ref:
dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
frame_id = ref_traj.meta[
"frame_id"] if "frame_id" in ref_traj.meta else ""
file_interface.write_bag_trajectory(bag, ref_traj, dest_topic,
frame_id)
finally:
bag.close()
def plot_result(args: argparse.Namespace,
result: Result,
traj_ref: PosePath3D,
traj_est: PosePath3D,
traj_ref_full: typing.Optional[PosePath3D] = None) -> None:
from evo.tools import plot
from evo.tools.settings import SETTINGS
import matplotlib.pyplot as plt
import numpy as np
logger.debug(SEP)
logger.debug("Plotting results... ")
plot_mode = plot.PlotMode(args.plot_mode)
# Plot the raw metric values.
fig1 = plt.figure(figsize=SETTINGS.plot_figsize)
if "seconds_from_start" in result.np_arrays:
seconds_from_start = result.np_arrays["seconds_from_start"]
else:
seconds_from_start = None
plot.error_array(
fig1.gca(),
result.np_arrays["error_array"],
x_array=seconds_from_start,
statistics={
s: result.stats[s]
for s in SETTINGS.plot_statistics if s not in ("min", "max")
},
name=result.info["label"],
title=result.info["title"],
xlabel="$t$ (s)" if seconds_from_start is not None else "index")
# Plot the values color-mapped onto the trajectory.
fig2 = plt.figure(figsize=SETTINGS.plot_figsize)
ax = plot.prepare_axis(fig2, plot_mode)
if args.ros_map_yaml:
plot.ros_map(ax, args.ros_map_yaml, plot_mode)
plot.traj(ax,
plot_mode,
traj_ref_full if traj_ref_full else traj_ref,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label='reference',
alpha=SETTINGS.plot_reference_alpha)
plot.draw_coordinate_axes(ax, traj_ref, plot_mode,
SETTINGS.plot_axis_marker_scale)
if args.plot_colormap_min is None:
args.plot_colormap_min = result.stats["min"]
if args.plot_colormap_max is None:
args.plot_colormap_max = result.stats["max"]
if args.plot_colormap_max_percentile is not None:
args.plot_colormap_max = np.percentile(
result.np_arrays["error_array"], args.plot_colormap_max_percentile)
plot.traj_colormap(ax,
traj_est,
result.np_arrays["error_array"],
plot_mode,
min_map=args.plot_colormap_min,
max_map=args.plot_colormap_max,
title="Error mapped onto trajectory")
plot.draw_coordinate_axes(ax, traj_est, plot_mode,
SETTINGS.plot_axis_marker_scale)
if SETTINGS.plot_pose_correspondences:
plot.draw_correspondence_edges(
ax,
traj_est,
traj_ref,
plot_mode,
style=SETTINGS.plot_pose_correspondences_linestyle,
color=SETTINGS.plot_reference_color,
alpha=SETTINGS.plot_reference_alpha)
fig2.axes.append(ax)
plot_collection = plot.PlotCollection(result.info["title"])
plot_collection.add_figure("raw", fig1)
plot_collection.add_figure("map", fig2)
if args.plot:
plot_collection.show()
if args.save_plot:
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.debug(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
def run(args):
import os
import sys
import numpy as np
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.core.trajectory import PoseTrajectory3D
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(verbose=args.verbose,
silent=args.silent,
debug=args.debug,
local_logfile=args.logfile)
if args.debug:
import pprint
logger.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logger.debug(SEP)
trajectories, ref_traj = load_trajectories(args)
if args.merge:
if args.subcommand == "kitti":
die("Can't merge KITTI files.")
if len(trajectories) == 0:
die("No trajectories to merge (excluding --ref).")
trajectories = {
"merged_trajectory": trajectory.merge(trajectories.values())
}
if args.transform_left or args.transform_right:
tf_type = "left" if args.transform_left else "right"
tf_path = args.transform_left \
if args.transform_left else args.transform_right
transform = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(transform):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
transform = lie.se3_inverse(transform)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, transform))
for traj in trajectories.values():
traj.transform(transform,
right_mul=args.transform_right,
propagate=args.propagate_transform)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories.items():
if type(traj) is trajectory.PosePath3D:
die("{} doesn't have timestamps - can't add time offset.".
format(name))
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.n_to_align != -1 and not (args.align or args.correct_scale):
die("--n_to_align is useless without --align or/and --correct_scale")
if args.sync or args.align or args.correct_scale or args.align_origin:
from evo.core import sync
if not args.ref:
logger.debug(SEP)
die("Can't align or sync without a reference! (--ref) *grunt*")
for name, traj in trajectories.items():
if args.subcommand == "kitti":
ref_traj_tmp = ref_traj
else:
logger.debug(SEP)
ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
first_name="reference",
snd_name=name)
if args.align or args.correct_scale:
logger.debug(SEP)
logger.debug("Aligning {} to reference.".format(name))
trajectories[name] = trajectory.align_trajectory(
trajectories[name],
ref_traj_tmp,
correct_scale=args.correct_scale,
correct_only_scale=args.correct_scale and not args.align,
n=args.n_to_align)
if args.align_origin:
logger.debug(SEP)
logger.debug("Aligning {}'s origin to reference.".format(name))
trajectories[name] = trajectory.align_trajectory_origin(
trajectories[name], ref_traj_tmp)
print_compact_name = not args.subcommand == "bag"
for name, traj in trajectories.items():
print_traj_info(name, traj, args.verbose, args.full_check,
print_compact_name)
if args.ref:
print_traj_info(args.ref, ref_traj, args.verbose, args.full_check,
print_compact_name)
if args.plot or args.save_plot or args.serialize_plot:
import numpy as np
from evo.tools import plot
import matplotlib.pyplot as plt
import matplotlib.cm as cm
plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
fig_xyz, axarr_xyz = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_rpy, axarr_rpy = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
plot_mode = plot.PlotMode[args.plot_mode]
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
if args.ref:
short_traj_name = os.path.splitext(os.path.basename(args.ref))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
plot.draw_coordinate_axes(ax_traj, ref_traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
plot.traj_xyz(axarr_xyz,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
plot.traj_rpy(axarr_rpy,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
if args.ros_map_yaml:
plot.ros_map(ax_traj, args.ros_map_yaml, plot_mode)
cmap_colors = None
if SETTINGS.plot_multi_cmap.lower() != "none":
cmap = getattr(cm, SETTINGS.plot_multi_cmap)
cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))
for name, traj in trajectories.items():
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
if print_compact_name:
short_traj_name = os.path.splitext(os.path.basename(name))[0]
else:
short_traj_name = name
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha)
plot.draw_coordinate_axes(ax_traj, traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
if args.ref and isinstance(ref_traj, trajectory.PoseTrajectory3D):
start_time = ref_traj.timestamps[0]
else:
start_time = None
plot.traj_xyz(axarr_xyz,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
plot.traj_rpy(axarr_rpy,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
if not SETTINGS.plot_usetex:
fig_rpy.text(0.,
0.005,
"euler_angle_sequence: {}".format(
SETTINGS.euler_angle_sequence),
fontsize=6)
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
if args.plot:
plot_collection.show()
if args.save_plot:
logger.info(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.info(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logger.info(SEP)
for name, traj in trajectories.items():
dest = os.path.splitext(os.path.basename(name))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_kitti:
logger.info(SEP)
for name, traj in trajectories.items():
dest = os.path.splitext(os.path.basename(name))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_bag:
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
logger.info(SEP)
logger.info("Saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories.items():
dest_topic = os.path.splitext(os.path.basename(name))[0]
frame_id = traj.meta[
"frame_id"] if "frame_id" in traj.meta else ""
file_interface.write_bag_trajectory(bag, traj, dest_topic,
frame_id)
if args.ref:
dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
frame_id = ref_traj.meta[
"frame_id"] if "frame_id" in ref_traj.meta else ""
file_interface.write_bag_trajectory(bag, ref_traj, dest_topic,
frame_id)
finally:
bag.close()
def run(args):
import os
import sys
import numpy as np
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.core.trajectory import PoseTrajectory3D
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(verbose=args.verbose,
silent=args.silent,
debug=args.debug)
if args.debug:
import pprint
logger.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logger.debug(SEP)
trajectories, ref_traj = load_trajectories(args)
if args.merge:
if args.subcommand == "kitti":
die("Can't merge KITTI files.")
if len(trajectories) == 0:
die("No trajectories to merge (excluding --ref).")
trajectories = {
"merged_trajectory": trajectory.merge(trajectories.values())
}
if args.transform_left or args.transform_right:
tf_type = "left" if args.transform_left else "right"
tf_path = args.transform_left \
if args.transform_left else args.transform_right
transform = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(transform):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
transform = lie.se3_inverse(transform)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, transform))
for traj in trajectories.values():
traj.transform(transform, right_mul=args.transform_right)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories.items():
if type(traj) is trajectory.PosePath3D:
die("{} doesn't have timestamps - can't add time offset.".
format(name))
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.sync or args.align or args.correct_scale:
from evo.core import sync
if not args.ref:
logger.debug(SEP)
die("Can't align or sync without a reference! (--ref) *grunt*")
for name, traj in trajectories.items():
if args.subcommand == "kitti":
ref_traj_tmp = ref_traj
else:
logger.debug(SEP)
ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
first_name="reference",
snd_name=name)
if args.align or args.correct_scale:
logger.debug(SEP)
logger.debug("Aligning {} to reference.".format(name))
trajectories[name] = trajectory.align_trajectory(
trajectories[name],
ref_traj_tmp,
correct_scale=args.correct_scale,
correct_only_scale=args.correct_scale and not args.align,
n=args.n_to_align)
for name, traj in trajectories.items():
print_traj_info(name, traj, args.verbose, args.full_check)
if args.ref:
print_traj_info(args.ref, ref_traj, args.verbose, args.full_check)
if args.plot or args.save_plot or args.serialize_plot:
from evo.tools.plot import PlotMode
plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[
args.plot_mode]
import numpy as np
from evo.tools import plot
import matplotlib.pyplot as plt
import matplotlib.cm as cm
plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
fig_xyz, axarr_xyz = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_rpy, axarr_rpy = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
pltstart = 0
pltend = traj.num_poses
if args.plotstart:
pltstart = args.plotstart
if args.plotend != -1:
pltend = args.plotend
if args.ref:
short_traj_name = os.path.splitext(os.path.basename(args.ref))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start=pltstart,
end=pltend)
plot.traj_xyz(axarr_xyz,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start=pltstart,
end=pltend)
plot.traj_rpy(axarr_rpy,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha,
start=pltstart,
end=pltend)
cmap_colors = None
if SETTINGS.plot_multi_cmap.lower() != "none":
cmap = getattr(cm, SETTINGS.plot_multi_cmap)
cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))
fig_3 = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
#plot_mode = plot.PlotMode.xz
ax = plot.prepare_axis(fig_3, plot_mode)
fig_3.axes.append(ax)
for name, traj in trajectories.items():
num = traj.positions_xyz.shape[0]
if pltstart >= num:
print(name, "plotstart > len!", num)
pltstart = 0
if pltend != -1 and (pltend > num or pltend < pltstart):
print(name, "plotend > len!", num)
pltend = traj.num_poses
pltstart = int(pltstart)
pltend = int(pltend)
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
short_traj_name = os.path.splitext(os.path.basename(name))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
traj,
'-',
color,
short_traj_name,
start=pltstart,
end=pltend)
if args.ref and isinstance(ref_traj, trajectory.PoseTrajectory3D):
start_time = ref_traj.timestamps[0]
else:
start_time = None
plot.traj_xyz(axarr_xyz,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time,
start=pltstart,
end=pltend)
plot.traj_rpy(axarr_rpy,
traj,
'-',
color,
short_traj_name,
start_timestamp=start_time,
start=pltstart,
end=pltend)
speeds = [
trajectory.calc_speed(traj.positions_xyz[i],
traj.positions_xyz[i + 1],
traj.timestamps[i],
traj.timestamps[i + 1])
for i in range(pltstart, pltend - 1)
]
speeds.append(0)
#plot.traj(ax, plot_mode, traj, '--', 'gray', 'reference')
plot.traj_colormap(ax,
traj,
speeds,
plot_mode,
min_map=min(speeds),
max_map=max(max(speeds), 10),
title="speed mapped onto trajectory",
start=pltstart,
end=pltend)
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
plot_collection.add_figure("traj (speed)", fig_3)
if args.plot:
plot_collection.show()
if args.save_plot:
logger.info(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.info(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logger.info(SEP)
for name, traj in trajectories.items():
dest = os.path.splitext(os.path.basename(name))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_kitti:
logger.info(SEP)
for name, traj in trajectories.items():
dest = os.path.splitext(os.path.basename(name))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_bag:
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
logger.info(SEP)
logger.info("Saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories.items():
dest_topic = os.path.splitext(os.path.basename(name))[0]
frame_id = traj.meta[
"frame_id"] if "frame_id" in traj.meta else ""
file_interface.write_bag_trajectory(bag, traj, dest_topic,
frame_id)
if args.ref:
dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
frame_id = ref_traj.meta[
"frame_id"] if "frame_id" in ref_traj.meta else ""
file_interface.write_bag_trajectory(bag, ref_traj, dest_topic,
frame_id)
finally:
bag.close()
