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()