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.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") # 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 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) # 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 = 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, 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) 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 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: 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 = to_topic_name(name, args) 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 = 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(bag, ref_traj, dest_topic, frame_id) finally: bag.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(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)