def run(args): import evo.common_ape_rpe as common from evo.tools import file_interface, log from evo.tools.settings import SETTINGS log.configure_logging(args.verbose, args.silent, args.debug) if args.debug: from pprint import pformat parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)}) logger.debug("main_parser config:\n{}".format(parser_str)) logger.debug(SEP) traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args) pose_relation = common.get_pose_relation(args) result = ape( traj_ref=traj_ref, traj_est=traj_est, pose_relation=pose_relation, align=args.align, correct_scale=args.correct_scale, ref_name=ref_name, est_name=est_name, ) if args.plot or args.save_plot or args.serialize_plot: common.plot( args, result, result.trajectories[ref_name], result.trajectories[est_name]) if args.save_results: logger.debug(SEP) if not SETTINGS.save_traj_in_zip: del result.trajectories[ref_name] del result.trajectories[est_name] file_interface.save_res_file( args.save_results, result, confirm_overwrite=not args.no_warnings)
def run(args): import evo.common_ape_rpe as common from evo.tools import file_interface, log from evo.tools.settings import SETTINGS log.configure_logging(args.verbose, args.silent, args.debug) if args.debug: from pprint import pformat parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)}) logger.debug("main_parser config:\n{}".format(parser_str)) logger.debug(SEP) traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args) pose_relation = common.get_pose_relation(args) result = ape( traj_ref=traj_ref, traj_est=traj_est, pose_relation=pose_relation, align=args.align, correct_scale=args.correct_scale, ref_name=ref_name, est_name=est_name, ) if args.plot or args.save_plot or args.serialize_plot: common.plot(args, result, result.trajectories[ref_name], result.trajectories[est_name]) if args.save_results: logger.debug(SEP) if not SETTINGS.save_traj_in_zip: del result.trajectories[ref_name] del result.trajectories[est_name] file_interface.save_res_file(args.save_results, result, confirm_overwrite=not args.no_warnings)
#!/usr/bin/env python from __future__ import print_function import argparse import copy from evo.tools import log log.configure_logging() import numpy as np import os from tqdm import tqdm import openpyxl from openpyxl import Workbook from openpyxl.styles import Border, Side, PatternFill, Font, GradientFill, Alignment from openpyxl.utils.dataframe import dataframe_to_rows from openpyxl.utils import get_column_letter import pandas as pd from evo.tools import plot from evo.tools.plot import PlotMode from evo.tools.settings import SETTINGS from evo.core.metrics import PoseRelation, Unit from evo.core import metrics from evo.core import sync from evo.core import trajectory # temporarily override some package settings SETTINGS.plot_figsize = [6, 6] SETTINGS.plot_split = True SETTINGS.plot_usetex = False
def run(args): from evo.core import metrics from evo.tools import file_interface, log log.configure_logging(args.verbose, args.silent, 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) pose_relation = None if args.pose_relation == "full": pose_relation = metrics.PoseRelation.full_transformation elif args.pose_relation == "rot_part": pose_relation = metrics.PoseRelation.rotation_part elif args.pose_relation == "trans_part": pose_relation = metrics.PoseRelation.translation_part elif args.pose_relation == "angle_deg": pose_relation = metrics.PoseRelation.rotation_angle_deg elif args.pose_relation == "angle_rad": pose_relation = metrics.PoseRelation.rotation_angle_rad traj_ref, traj_est, stamps_est = None, None, None ref_name, est_name = "", "" plot_mode = None # no plot imports unless really needed (slow) if args.subcommand == "tum": traj_ref, traj_est = file_interface.load_assoc_tum_trajectories( args.ref_file, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.ref_file, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "kitti": traj_ref = file_interface.read_kitti_poses_file(args.ref_file) traj_est = file_interface.read_kitti_poses_file(args.est_file) ref_name, est_name = args.ref_file, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "euroc": args.align = True logger.info( "forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)" ) logger.debug(SEP) traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories( args.state_gt_csv, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.state_gt_csv, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "bag": import rosbag logger.debug("opening bag file " + args.bag) bag = rosbag.Bag(args.bag, 'r') try: traj_ref, traj_est = file_interface.load_assoc_bag_trajectories( bag, args.ref_topic, args.est_topic, args.t_max_diff, args.t_offset, ) finally: bag.close() ref_name, est_name = args.ref_topic, args.est_topic if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xy if not args.plot_mode else PlotMode[ args.plot_mode] main_ape( traj_ref=traj_ref, traj_est=traj_est, pose_relation=pose_relation, align=args.align, correct_scale=args.correct_scale, ref_name=ref_name, est_name=est_name, show_plot=args.plot, save_plot=args.save_plot, plot_mode=plot_mode, save_results=args.save_results, no_warnings=args.no_warnings, serialize_plot=args.serialize_plot, plot_colormap_max=args.plot_colormap_max, plot_colormap_min=args.plot_colormap_min, plot_colormap_max_percentile=args.plot_colormap_max_percentile, )
def run(args): import sys import pandas as pd from evo.tools import file_interface, 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) 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 = 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: 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] 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] logger.debug(SEP) logger.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() logger.debug(SEP) logger.debug("Aggregated dataframe:\n{}".format( df.to_string(line_width=80))) # show a statistics overview logger.debug(SEP) logger.info("\n{}\n\n{}\n".format( first_title, df.loc["stats"].T.to_string(line_width=80))) 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 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) # 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()) # 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: 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 = 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()
def run(args: argparse.Namespace) -> None: print('using this main_ape') log.configure_logging(args.verbose, args.silent, args.debug, local_logfile=args.logfile) if args.debug: from pprint import pformat parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)}) logger.debug("main_parser config:\n{}".format(parser_str)) logger.debug(SEP) traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args) traj_ref_full = None if args.plot_full_ref: import copy traj_ref_full = copy.deepcopy(traj_ref) if args.flip_xy: print("flip xy", args.flip_xy) flip_rotation = transformations.rotation_matrix(math.pi / 2, [0, 0, 1]) print('transformation matrix,', flip_rotation) traj_est.transform_rotation_only(flip_rotation) if isinstance(traj_ref, PoseTrajectory3D) and isinstance( traj_est, PoseTrajectory3D): logger.debug(SEP) if args.t_start or args.t_end: if args.t_start: logger.info("Using time range start: {}s".format(args.t_start)) if args.t_end: logger.info("Using time range end: {}s".format(args.t_end)) traj_ref.reduce_to_time_range(args.t_start, args.t_end) logger.debug("Synchronizing trajectories...") traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, args.t_max_diff, args.t_offset, first_name=ref_name, snd_name=est_name) pose_relation = common.get_pose_relation(args) print('align_odom: ', args.align_odom) result = ape( traj_ref=traj_ref, traj_est=traj_est, pose_relation=pose_relation, align=args.align, correct_scale=args.correct_scale, n_to_align=args.n_to_align, align_origin=args.align_origin, align_odom=args.align_odom, ref_name=ref_name, est_name=est_name, ) if args.plot or args.save_plot or args.serialize_plot: common.plot_result(args, result, traj_ref, result.trajectories[est_name], traj_ref_full=traj_ref_full) if args.save_results: logger.debug(SEP) if not SETTINGS.save_traj_in_zip: del result.trajectories[ref_name] del result.trajectories[est_name] file_interface.save_res_file(args.save_results, result, confirm_overwrite=not args.no_warnings)
def run(args: argparse.Namespace) -> None: log.configure_logging(args.verbose, args.silent, args.debug, local_logfile=args.logfile) if args.debug: from pprint import pformat parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)}) logger.debug("main_parser config:\n{}".format(parser_str)) logger.debug(SEP) traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args) pose_relation = common.get_pose_relation(args) delta_unit = common.get_delta_unit(args) traj_ref_full = None if args.plot_full_ref: import copy traj_ref_full = copy.deepcopy(traj_ref) if isinstance(traj_ref, PoseTrajectory3D) and isinstance( traj_est, PoseTrajectory3D): logger.debug("Synchronizing trajectories...") traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, args.t_max_diff, args.t_offset, first_name=ref_name, snd_name=est_name) result = rpe( traj_ref=traj_ref, traj_est=traj_est, pose_relation=pose_relation, delta=args.delta, delta_unit=delta_unit, rel_delta_tol=args.delta_tol, all_pairs=args.all_pairs, align=args.align, correct_scale=args.correct_scale, n_to_align=args.n_to_align, align_origin=args.align_origin, ref_name=ref_name, est_name=est_name, ) if args.plot or args.save_plot or args.serialize_plot: common.plot_result(args, result, traj_ref, result.trajectories[est_name], traj_ref_full=traj_ref_full) if args.save_results: logger.debug(SEP) if not SETTINGS.save_traj_in_zip: del result.trajectories[ref_name] del result.trajectories[est_name] file_interface.save_res_file(args.save_results, result, confirm_overwrite=not args.no_warnings)
def main(): import argparse import argcomplete basic_desc = "experimental tool for opening a serialized PlotCollection" lic = "(c) evo authors" 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 log, plot, user log.configure_logging(verbose=True) if not args.title: title = os.path.basename(args.in_file) else: title = args.title if not args.no_warnings: logger.warning( "This tool is experimental and not guranteed to work.\nOnly works " "if the same plot settings are used as for serialization.\n" "If not, try: evo_config show/set \n" + SEP) plot_collection = plot.PlotCollection(title, deserialize=args.in_file) logger.debug("Deserialized PlotCollection: " + str(plot_collection)) plot_collection.show() if args.serialize_plot: logger.debug(SEP) plot_collection.serialize(args.serialize_plot, confirm_overwrite=not args.no_warnings) if args.save_plot: logger.debug(SEP) plot_collection.export(args.save_plot, confirm_overwrite=not args.no_warnings) if args.to_html: import mpld3 logger.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: logger.debug(out) f.write(html) if not args.no_warnings: logger.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 main(): import argcomplete basic_desc = "crappy configuration tool" lic = "(c) [email protected]" shared_parser = argparse.ArgumentParser(add_help=False) shared_parser.add_argument("--no_color", help="don't color output", action="store_true") main_parser = argparse.ArgumentParser(description="%s %s" % (basic_desc, lic)) sub_parsers = main_parser.add_subparsers(dest="subcommand") sub_parsers.required = True show_parser = sub_parsers.add_parser("show", description="show configuration - %s" % lic, parents=[shared_parser]) show_parser.add_argument("config", help="optional config file to display (default: package settings)", nargs='?') show_parser.add_argument("--brief", help="show only the .json data", action="store_true") set_parser = sub_parsers.add_parser("set", description=SET_HELP, parents=[shared_parser], formatter_class=argparse.RawTextHelpFormatter) set_parser.add_argument("params", choices=list(DEFAULT_SETTINGS_DICT.keys()), nargs=argparse.REMAINDER, help="parameters to set") set_parser.add_argument("-c", "--config", help="optional config file (default: package settings)", default=None) set_parser.add_argument("-m", "--merge", help="other config file to merge in (priority)", default=None) set_parser.add_argument("--soft", help="do a soft-merge (no overwriting)", action="store_true") gen_parser = sub_parsers.add_parser("generate", description=GENERATE_HELP, parents=[shared_parser], formatter_class=argparse.RawTextHelpFormatter) gen_parser.add_argument("-o", "--out", help="path for config file to generate") reset_parser = sub_parsers.add_parser("reset", description="reset package settings - %s" % lic, parents=[shared_parser]) reset_parser.add_argument("-y", help="acknowledge automatically", action="store_true") argcomplete.autocomplete(main_parser) if len(sys.argv) > 1 and sys.argv[1] == "set": args, other_args = main_parser.parse_known_args() other_args = [arg for arg in sys.argv[2:] if not arg.startswith('-')] else: args, other_args = main_parser.parse_known_args() log.configure_logging() colorama.init() config = settings.DEFAULT_PATH if hasattr(args, "config"): if args.config: config = args.config if args.subcommand == "show": if not args.brief and not args.config: style = Style.BRIGHT if not args.no_color else Style.NORMAL doc_str = "\n".join("{0}{1}{2}:\n{3}\n".format(style, k, Style.RESET_ALL, v[1]) for k, v in sorted(DEFAULT_SETTINGS_DICT_DOC.items())) logger.info(doc_str) logger.info("{0}\n{1}\n{0}".format(SEP, config)) show(config, colored=not args.no_color) if config == settings.DEFAULT_PATH and not args.brief: logger.info(SEP + "\nSee text above for parameter descriptions.") elif args.subcommand == "set": if not os.access(config, os.W_OK): logger.error("No permission to modify " + config) sys.exit() if other_args or args.merge: logger.info("{0}\nOld configuration:\n{0}".format(SEP)) show(config, colored=not args.no_color) try: set_cfg(config, other_args) except ConfigError as e: logger.error(e) sys.exit(1) if args.merge: merge_json_union(config, args.merge, args.soft) logger.info(SEP + "\nNew configuration:\n" + SEP) show(config, colored=not args.no_color) else: logger.error("No configuration parameters given (see --help).") elif args.subcommand == "generate": if other_args: logger.info("{0}\nParsed by argparse:\n{1}\n" "{0}\nWARNING:\n" "Make sure you use the 'long-style' -- options (e.g. --plot) if possible\n" "and no combined short '-' flags, (e.g. -vp)\n{0}".format(SEP, other_args)) data = generate(other_args) log_info_dict_json(data, colored=not args.no_color) if args.out and user.check_and_confirm_overwrite(args.out): with open(args.out, 'w') as out: out.write(json.dumps(data, indent=4, sort_keys=True)) elif not args.out: logger.warning("\n(-o | --out) not specified - saving nothing") else: logger.error("No command line arguments given (see --help)") elif args.subcommand == "reset": if not os.access(config, os.W_OK): logger.error("No permission to modify" + config) sys.exit() if args.y or user.confirm("Reset the package settings to the default settings? (y/n)"): settings.reset() logger.info("{0}\nPackage settings after reset:\n{0}".format(SEP)) show(settings.DEFAULT_PATH, colored=not args.no_color)
def run(args): import sys from evo.core import metrics from evo.tools import file_interface, log # manually check bins and tols arguments to allow them to be in config files if not args.bins or not args.tols: logger.error("The following arguments are required: -b/--bins, -t/--tols") sys.exit(1) log.configure_logging(args.verbose, args.silent, 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) pose_relation = None if args.pose_relation == "trans_part": pose_relation = metrics.PoseRelation.translation_part elif args.pose_relation == "angle_deg": pose_relation = metrics.PoseRelation.rotation_angle_deg elif args.pose_relation == "angle_rad": pose_relation = metrics.PoseRelation.rotation_angle_rad traj_ref, traj_est, stamps_est = None, None, None ref_name, est_name = "", "" if args.subcommand == "tum": traj_ref, traj_est = file_interface.load_assoc_tum_trajectories( args.ref_file, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.ref_file, args.est_file elif args.subcommand == "kitti": traj_ref = file_interface.read_kitti_poses_file(args.ref_file) traj_est = file_interface.read_kitti_poses_file(args.est_file) ref_name, est_name = args.ref_file, args.est_file elif args.subcommand == "euroc": args.align = True logger.info("Forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)") logger.debug(SEP) traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories( args.state_gt_csv, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.state_gt_csv, args.est_file elif args.subcommand == "bag": import rosbag logger.debug("Opening bag file {} ...".format(args.bag)) bag = rosbag.Bag(args.bag, 'r') try: traj_ref, traj_est = file_interface.load_assoc_bag_trajectories( bag, args.ref_topic, args.est_topic, args.t_max_diff, args.t_offset, ) finally: bag.close() ref_name, est_name = args.ref_topic, args.est_topic main_rpe_for_each(traj_ref, traj_est, pose_relation, args.mode, args.bins, args.tols, args.align, args.correct_scale, ref_name, est_name, args.plot, args.save_plot, args.save_results, args.no_warnings, serialize_plot=args.serialize_plot)
You should have received a copy of the GNU General Public License along with evo. If not, see <http://www.gnu.org/licenses/>. """ import logging import sys import evo.core.lie_algebra as lie from evo.core import trajectory from evo.tools import plot, file_interface, log import numpy as np import matplotlib.pyplot as plt logger = logging.getLogger("evo") log.configure_logging(verbose=True) traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt") traj_est = file_interface.read_kitti_poses_file( "../test/data/KITTI_00_ORB.txt") # add artificial Sim(3) transformation traj_est.transform(lie.se3(np.eye(3), [0, 0, 0])) traj_est.scale(0.5) logger.info("\nUmeyama alignment without scaling") traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref) logger.info("\nUmeyama alignment with scaling") traj_est_aligned_scaled = trajectory.align_trajectory(traj_est, traj_ref,
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 main(): import argcomplete basic_desc = "crappy configuration tool" lic = "(c) [email protected]" shared_parser = argparse.ArgumentParser(add_help=False) shared_parser.add_argument("--no_color", help="don't color output", action="store_true") main_parser = argparse.ArgumentParser(description="%s %s" % (basic_desc, lic)) sub_parsers = main_parser.add_subparsers(dest="subcommand") sub_parsers.required = True show_parser = sub_parsers.add_parser( "show", description="show configuration - %s" % lic, parents=[shared_parser]) show_parser.add_argument( "config", help="optional config file to display (default: package settings)", nargs='?') show_parser.add_argument("--brief", help="show only the .json data", action="store_true") set_parser = sub_parsers.add_parser( "set", description=SET_HELP, parents=[shared_parser], formatter_class=argparse.RawTextHelpFormatter) set_parser.add_argument("params", choices=list(DEFAULT_SETTINGS_DICT.keys()), nargs=argparse.REMAINDER, help="parameters to set") set_parser.add_argument( "-c", "--config", help="optional config file (default: package settings)", default=None) set_parser.add_argument("-m", "--merge", help="other config file to merge in (priority)", default=None) set_parser.add_argument("--soft", help="do a soft-merge (no overwriting)", action="store_true") gen_parser = sub_parsers.add_parser( "generate", description=GENERATE_HELP, parents=[shared_parser], formatter_class=argparse.RawTextHelpFormatter) gen_parser.add_argument("-o", "--out", help="path for config file to generate") reset_parser = sub_parsers.add_parser( "reset", description="reset package settings - %s" % lic, parents=[shared_parser]) reset_parser.add_argument("-y", help="acknowledge automatically", action="store_true") argcomplete.autocomplete(main_parser) if len(sys.argv) > 1 and sys.argv[1] == "set": args, other_args = main_parser.parse_known_args() other_args = [arg for arg in sys.argv[2:] if not arg.startswith('-')] else: args, other_args = main_parser.parse_known_args() log.configure_logging() colorama.init() config = settings.DEFAULT_PATH if hasattr(args, "config"): if args.config: config = args.config if args.subcommand == "show": if not args.brief and not args.config: style = Style.BRIGHT if not args.no_color else Style.NORMAL doc_str = "\n".join( "{0}{1}{2}:\n{3}\n".format(style, k, Style.RESET_ALL, v[1]) for k, v in sorted(DEFAULT_SETTINGS_DICT_DOC.items())) logger.info(doc_str) logger.info("{0}\n{1}\n{0}".format(SEP, config)) show(config, colored=not args.no_color) if config == settings.DEFAULT_PATH and not args.brief: logger.info(SEP + "\nSee text above for parameter descriptions.") elif args.subcommand == "set": if not os.access(config, os.W_OK): logger.error("No permission to modify " + config) sys.exit() if other_args or args.merge: logger.info("{0}\nOld configuration:\n{0}".format(SEP)) show(config, colored=not args.no_color) try: set_cfg(config, other_args) except ConfigError as e: logger.error(e) sys.exit(1) if args.merge: merge_json_union(config, args.merge, args.soft) logger.info(SEP + "\nNew configuration:\n" + SEP) show(config, colored=not args.no_color) else: logger.error("No configuration parameters given (see --help).") elif args.subcommand == "generate": if other_args: logger.info( "{0}\nParsed by argparse:\n{1}\n" "{0}\nWARNING:\nMake sure you use the 'long-style' -- options " "(e.g. --plot) if possible\nand no combined short '-' flags, " "(e.g. -vp)\n{0}".format(SEP, other_args)) data = generate(other_args) log_info_dict_json(data, colored=not args.no_color) if args.out and user.check_and_confirm_overwrite(args.out): with open(args.out, 'w') as out: out.write(json.dumps(data, indent=4, sort_keys=True)) elif not args.out: logger.warning("\n(-o | --out) not specified - saving nothing") else: logger.error("No command line arguments given (see --help)") elif args.subcommand == "reset": if not os.access(config, os.W_OK): logger.error("No permission to modify" + config) sys.exit() if args.y or user.confirm( "Reset the package settings to the default settings? (y/n)"): settings.reset() logger.info("{0}\nPackage settings after reset:\n{0}".format(SEP)) show(settings.DEFAULT_PATH, colored=not args.no_color)
def main(): import sys import argparse import argcomplete main_parser = argparse.ArgumentParser() shared_parser = argparse.ArgumentParser(add_help=False) sub_parsers = main_parser.add_subparsers(dest="subcommand") sub_parsers.required = True pkg_parser = sub_parsers.add_parser( "pkg", description="show infos of the package", parents=[shared_parser]) pkg_parser.add_argument("--info", help="show the package description", action="store_true") pkg_parser.add_argument("--version", help="print the package version", action="store_true") pkg_parser.add_argument("--pyversion", help="print the Python version", action="store_true") pkg_parser.add_argument("--license", help="print the package license", action="store_true") pkg_parser.add_argument("--location", help="print the package path", action="store_true") pkg_parser.add_argument("--logfile", help="print the logfile path", action="store_true") pkg_parser.add_argument("--open_log", help="open the package logfile", action="store_true") pkg_parser.add_argument("--clear_log", help="clear package logfile", action="store_true") cat_parser = sub_parsers.add_parser( "cat_log", description="pipe stdin to evo logfile" " or print logfile to stdout (if no stdin)", parents=[shared_parser]) cat_parser.add_argument("-l", "--loglevel", help="loglevel of the message", default="info", choices=["error", "warning", "info", "debug"]) cat_parser.add_argument("-m", "--message", help="explicit message instead of pipe") cat_parser.add_argument("-s", "--source", help="source name to use for the log message") cat_parser.add_argument("--clear_log", help="clear logfile before exiting", action="store_true") argcomplete.autocomplete(main_parser) if len(sys.argv[1:]) == 0: sys.argv.extend(["pkg", "--info"]) # cheap trick because YOLO args = main_parser.parse_args() line_end = "\n" if sys.stdout.isatty() else "" if args.subcommand == "pkg": if not len(sys.argv) > 2: pkg_parser.print_help() sys.exit(1) if args.license: print( open(os.path.join(settings.PACKAGE_BASE_PATH, "LICENSE")).read()) if args.info: main_parser.print_usage() print(DESC) if args.version: here = os.path.dirname(os.path.abspath(__file__)) version_path = os.path.join(here, "version") print(" ".join(open(version_path).read().splitlines()), end=line_end) if args.pyversion: import platform as pf print(pf.python_version(), end=line_end) if args.location: print(settings.PACKAGE_BASE_PATH, end=line_end) if args.logfile or args.open_log: if not os.path.exists(settings.DEFAULT_LOGFILE_PATH): print("no logfile found - run: evo_config set logfile_enabled", end=line_end) sys.exit(1) print(settings.DEFAULT_LOGFILE_PATH, end=line_end) if args.open_log: import webbrowser webbrowser.open(settings.DEFAULT_LOGFILE_PATH) if args.clear_log: from evo.tools import user if user.confirm("clear logfile? (y/n)"): open(settings.DEFAULT_LOGFILE_PATH, mode='w') elif args.subcommand == "cat_log": if os.name == "nt": print("cat_log feature not available on Windows") sys.exit(1) if not args.message and sys.stdin.isatty(): if not os.path.exists(settings.DEFAULT_LOGFILE_PATH): print("no logfile found - run: evo_config set logfile_enabled", end=line_end) else: print(open(settings.DEFAULT_LOGFILE_PATH).read(), end="") elif not settings.SETTINGS.logfile_enabled: print("logfile disabled", end=line_end) sys.exit(1) else: import logging logger = logging.getLogger(__name__) from evo.tools import log file_fmt = log.DEFAULT_LONG_FMT if args.source: file_fmt = file_fmt.replace( "%(module)s.%(funcName)s():%(lineno)s", args.source) log.configure_logging(silent=True, file_fmt=file_fmt) if not args.message: msg = sys.stdin.read() else: msg = args.message getattr(logger, args.loglevel)(msg) if args.clear_log: open(settings.DEFAULT_LOGFILE_PATH, mode='w')
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 run(args): import evo.common_ape_rpe as common from evo.core import sync from evo.tools import file_interface, log from evo.tools.settings import SETTINGS log.configure_logging(args.verbose, args.silent, args.debug, local_logfile=args.logfile) if args.debug: from pprint import pformat parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)}) logger.debug("main_parser config:\n{}".format(parser_str)) logger.debug(SEP) if (args.plot or args.save_plot or args.serialize_plot) and args.all_pairs: raise NotImplementedError( "all_pairs mode cannot be used with plotting functions") traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args) pose_relation = common.get_pose_relation(args) delta_unit = common.get_delta_unit(args) traj_ref_full = None if args.plot_full_ref: import copy traj_ref_full = copy.deepcopy(traj_ref) if args.subcommand != "kitti": logger.debug("Synchronizing trajectories...") traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, args.t_max_diff, args.t_offset, first_name=ref_name, snd_name=est_name) result = rpe( traj_ref=traj_ref, traj_est=traj_est, pose_relation=pose_relation, delta=args.delta, delta_unit=delta_unit, rel_delta_tol=args.delta_tol, all_pairs=args.all_pairs, align=args.align, correct_scale=args.correct_scale, align_origin=args.align_origin, ref_name=ref_name, est_name=est_name, ) if args.plot or args.save_plot or args.serialize_plot: common.plot(args, result, traj_ref_full if args.plot_full_ref else traj_ref, result.trajectories[est_name]) if args.save_results: logger.debug(SEP) if not SETTINGS.save_traj_in_zip: del result.trajectories[ref_name] del result.trajectories[est_name] file_interface.save_res_file(args.save_results, result, 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()
You should have received a copy of the GNU General Public License along with evo. If not, see <http://www.gnu.org/licenses/>. """ import logging import sys import evo.core.lie_algebra as lie from evo.core import trajectory from evo.tools import plot, file_interface, log import numpy as np import matplotlib.pyplot as plt logger = logging.getLogger("evo") log.configure_logging(verbose=True) traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt") traj_est = file_interface.read_kitti_poses_file("../test/data/KITTI_00_ORB.txt") # add artificial Sim(3) transformation traj_est.transform(lie.se3(np.eye(3), [0, 0, 0])) traj_est.scale(0.5) logger.info("\nUmeyama alignment without scaling") traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref) logger.info("\nUmeyama alignment with scaling") traj_est_aligned_scaled = trajectory.align_trajectory( traj_est, traj_ref, correct_scale=True)