def filter_pairs_by_speed(poses, timestamps, speed, tol): """ filters pairs in a list of SE(3) poses by the linear speed of the motion in between them :param poses: list of SE(3) poses :param timestamps: list of timestamps corresponding to the poses :param speed: in m/s :param tol: tolerance to accept or reject velocities, in m/s :return: list of index tuples of the filtered pairs """ positions = [pose[:3, 3] for pose in poses] speeds = [trajectory.calc_speed(positions[i], positions[i + 1], timestamps[i], timestamps[i + 1]) for i in range(len(positions) - 1)] id_pairs = [(i, i+1) for i, v in enumerate(speeds) if speed - tol <= v <= speed + tol] return id_pairs
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, traj_est, ape_metric.error, plot_mode, min_map=ape_statistics["min"], max_map=ape_statistics["max"], title="APE mapped onto trajectory") plot_collection.add_figure("traj (error)", fig_2) # trajectory colormapped with speed fig_3 = plt.figure(figsize=(8, 8)) plot_mode = plot.PlotMode.xy ax = plot.prepare_axis(fig_3, plot_mode) speeds = [trajectory.calc_speed(traj_est.positions_xyz[i], traj_est.positions_xyz[i + 1], traj_est.timestamps[i], traj_est.timestamps[i + 1]) for i in range(len(traj_est.positions_xyz) - 1)] speeds.append(0) plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference') plot.traj_colormap(ax, traj_est, speeds, plot_mode, min_map=min(speeds), max_map=max(speeds), title="speed mapped onto trajectory") fig_3.axes.append(ax) plot_collection.add_figure("traj (speed)", fig_3) plot_collection.show()
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") plot_collection.add_figure("traj (error)", fig_2) # trajectory colormapped with speed fig_3 = plt.figure(figsize=(8, 8)) plot_mode = plot.PlotMode.xy ax = plot.prepare_axis(fig_3, plot_mode) speeds = [ trajectory.calc_speed(traj_est.positions_xyz[i], traj_est.positions_xyz[i + 1], traj_est.timestamps[i], traj_est.timestamps[i + 1]) for i in range(len(traj_est.positions_xyz) - 1) ] speeds.append(0) plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference') plot.traj_colormap(ax, traj_est, speeds, plot_mode, min_map=min(speeds), max_map=max(speeds), title="speed mapped onto trajectory") fig_3.axes.append(ax) plot_collection.add_figure("traj (speed)", fig_3)
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()