def load_trajectories(args): import os from collections import OrderedDict from evo.tools import file_interface trajectories = OrderedDict() ref_traj = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file == args.ref: continue trajectories[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: continue trajectories[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: continue else: trajectories[ 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": if not (args.topics or args.all_topics): die("No topics used - specify topics or set --all_topics.") if not os.path.exists(args.bag): raise file_interface.FileInterfaceException( "File doesn't exist: {}".format(args.bag)) import rosbag bag = rosbag.Bag(args.bag) try: if args.all_topics: topics = file_interface.get_supported_topics(bag) if args.ref in topics: topics.remove(args.ref) if len(topics) == 0: die("No topics of supported types: {}".format(" ".join( file_interface.SUPPORTED_ROS_MSGS))) else: topics = args.topics for topic in topics: if topic == args.ref: continue trajectories[topic] = file_interface.read_bag_trajectory( bag, topic) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() return trajectories, ref_traj
def load_trajectories(args): from evo.core import sync from evo.tools import file_interface if args.subcommand == "tum": traj_ref = file_interface.read_tum_trajectory_file(args.ref_file) traj_est = file_interface.read_tum_trajectory_file(args.est_file) 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": traj_ref = file_interface.read_euroc_csv_trajectory(args.state_gt_csv) traj_est = file_interface.read_tum_trajectory_file(args.est_file) ref_name, est_name = args.state_gt_csv, args.est_file elif args.subcommand == "bag": import os logger.debug("Opening bag file " + args.bag) if not os.path.exists(args.bag): raise file_interface.FileInterfaceException( "File doesn't exist: {}".format(args.bag)) import rosbag bag = rosbag.Bag(args.bag, 'r') try: traj_ref = file_interface.read_bag_trajectory(bag, args.ref_topic) traj_est = file_interface.read_bag_trajectory(bag, args.est_topic) ref_name, est_name = args.ref_topic, args.est_topic finally: bag.close() else: raise KeyError("unknown sub-command: {}".format(args.subcommand)) return traj_ref, traj_est, ref_name, est_name
def load_trajectories(args): from evo.tools import file_interface trajectories = {} ref_traj = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file == args.ref: continue trajectories[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: continue trajectories[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: continue else: trajectories[ 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": if not (args.topics or args.all_topics): die("No topics used - specify topics or set --all_topics.") 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] in { "geometry_msgs/PoseStamped", "geometry_msgs/PoseWithCovarianceStamped", "nav_msgs/Odometry" } and t != args.ref ]) if len(topics) == 0: die("No geometry_msgs/PoseStamped, " "geometry_msgs/PoseWithCovarianceStamped or " "nav_msgs/Odometry topics found!") else: topics = args.topics for topic in topics: trajectories[topic] = file_interface.read_bag_trajectory( bag, topic) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() return trajectories, ref_traj
def load_trajectories(args): from evo.tools import file_interface trajectories = {} ref_traj = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file == args.ref: continue trajectories[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: continue trajectories[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: continue else: trajectories[ 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": if not (args.topics or args.all_topics): die("No topics used - specify topics or set --all_topics.") 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] in file_interface.SUPPORTED_ROS_MSGS and t != args.ref ]) if len(topics) == 0: die("No topics of supported types: {}".format(" ".join( file_interface.SUPPORTED_ROS_MSGS))) else: topics = args.topics for topic in topics: trajectories[topic] = file_interface.read_bag_trajectory( bag, topic) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() return trajectories, ref_traj
def load_trajectories(args): from evo.core import sync from evo.tools import file_interface if args.subcommand == "tum": traj_ref = file_interface.read_tum_trajectory_file(args.ref_file) traj_est = file_interface.read_tum_trajectory_file(args.est_file) 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 = file_interface.read_euroc_csv_trajectory(args.state_gt_csv) traj_est = file_interface.read_euroc_csv_trajectory(args.est_file) ref_name, est_name = args.state_gt_csv, args.est_file elif args.subcommand == "bag": import rosbag logger.debug("Opening bag file " + args.bag) bag = rosbag.Bag(args.bag, 'r') try: traj_ref = file_interface.read_bag_trajectory(bag, args.ref_topic) traj_est = file_interface.read_bag_trajectory(bag, args.est_topic) ref_name, est_name = args.ref_topic, args.est_topic finally: bag.close() else: raise KeyError("unknown sub-command: {}".format(args.subcommand)) 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) return traj_ref, traj_est, ref_name, est_name
def load_trajectories( args: argparse.Namespace ) -> typing.Tuple[PosePath3D, PosePath3D, str, str]: from evo.tools import file_interface traj_ref: typing.Union[PosePath3D, PoseTrajectory3D] traj_est: typing.Union[PosePath3D, PoseTrajectory3D] if args.subcommand == "tum": traj_ref = file_interface.read_tum_trajectory_file(args.ref_file) traj_est = file_interface.read_tum_trajectory_file(args.est_file) 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": traj_ref = file_interface.read_euroc_csv_trajectory(args.state_gt_csv) traj_est = file_interface.read_tum_trajectory_file(args.est_file) ref_name, est_name = args.state_gt_csv, args.est_file elif args.subcommand in ("bag", "bag2"): import os logger.debug("Opening bag file " + args.bag) if not os.path.exists(args.bag): raise file_interface.FileInterfaceException( "File doesn't exist: {}".format(args.bag)) if args.subcommand == "bag2": from rosbags.rosbag2 import Reader as Rosbag2Reader bag = Rosbag2Reader(args.bag) # type: ignore else: from rosbags.rosbag1 import Reader as Rosbag1Reader bag = Rosbag1Reader(args.bag) # type: ignore try: bag.open() traj_ref = file_interface.read_bag_trajectory(bag, args.ref_topic) traj_est = file_interface.read_bag_trajectory(bag, args.est_topic) ref_name, est_name = args.ref_topic, args.est_topic finally: bag.close() else: raise KeyError("unknown sub-command: {}".format(args.subcommand)) return traj_ref, traj_est, ref_name, est_name
def load_trajectories(args): from evo.core import sync from evo.tools import file_interface if args.subcommand == "tum": traj_ref = file_interface.read_tum_trajectory_file(args.ref_file) traj_est = file_interface.read_tum_trajectory_file(args.est_file) 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 = file_interface.read_euroc_csv_trajectory(args.state_gt_csv) traj_est = file_interface.read_tum_trajectory_file(args.est_file) ref_name, est_name = args.state_gt_csv, args.est_file elif args.subcommand == "bag": import rosbag logger.debug("Opening bag file " + args.bag) bag = rosbag.Bag(args.bag, 'r') try: traj_ref = file_interface.read_bag_trajectory(bag, args.ref_topic) traj_est = file_interface.read_bag_trajectory(bag, args.est_topic) ref_name, est_name = args.ref_topic, args.est_topic finally: bag.close() else: raise KeyError("unknown sub-command: {}".format(args.subcommand)) 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) return traj_ref, traj_est, ref_name, est_name
def test_write_read_integrity(self): import rosbag tmp_file = tempfile.NamedTemporaryFile(delete=False) bag_out = rosbag.Bag(tmp_file.name, 'w') traj_out = helpers.fake_trajectory(1000, 0.1) self.assertTrue(traj_out.check()) file_interface.write_bag_trajectory(bag_out, traj_out, "/test", frame_id="map") bag_out.close() bag_in = rosbag.Bag(tmp_file.name, 'r') traj_in = file_interface.read_bag_trajectory(bag_in, "/test") self.assertIsInstance(traj_in, PoseTrajectory3D) self.assertTrue(traj_in.check()) self.assertTrue(traj_out == traj_in) self.assertEquals(traj_in.meta["frame_id"], "map")
def test_write_read_integrity(self): for reader_t, writer_t in zip([Rosbag1Reader, Rosbag2Reader], [Rosbag1Writer, Rosbag2Writer]): # TODO: rosbags cannot overwrite existing paths, this forces us # to do this here to get only a filepath: tmp_filename = tempfile.NamedTemporaryFile(delete=True).name bag_out = writer_t(tmp_filename) bag_out.open() traj_out = helpers.fake_trajectory(1000, 0.1) self.assertTrue(traj_out.check()) file_interface.write_bag_trajectory(bag_out, traj_out, "/test", frame_id="map") bag_out.close() bag_in = reader_t(tmp_filename) bag_in.open() traj_in = file_interface.read_bag_trajectory(bag_in, "/test") self.assertIsInstance(traj_in, PoseTrajectory3D) self.assertTrue(traj_in.check()) self.assertTrue(traj_out == traj_in) self.assertEqual(traj_in.meta["frame_id"], "map")
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 os import sys import logging import evo.algorithms.lie_algebra as lie from evo.algorithms import trajectory from evo.tools import file_interface, settings from evo.tools.settings import SETTINGS settings.configure_logging(verbose=True, silent=args.silent, debug=args.debug) if args.debug: import pprint logging.debug( "main_parser config:\n" + pprint.pformat({arg: getattr(args, arg) for arg in vars(args)}) + "\n") logging.debug(SEP) trajectories = [] ref_traj = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file != args.ref: trajectories.append( (traj_file, file_interface.read_tum_trajectory_file(traj_file))) if args.ref: ref_traj = file_interface.read_tum_trajectory_file(args.ref) elif args.subcommand == "kitti": for pose_file in args.pose_files: if pose_file != args.ref: trajectories.append( (pose_file, file_interface.read_kitti_poses_file(pose_file))) if args.ref: ref_traj = file_interface.read_kitti_poses_file(args.ref) elif args.subcommand == "euroc": for csv_file in args.state_gt_csv: if csv_file != args.ref: trajectories.append( (csv_file, file_interface.read_euroc_csv_trajectory(csv_file))) if args.ref: ref_traj = file_interface.read_euroc_csv_trajectory(args.ref) elif args.subcommand == "bag": import rosbag bag = rosbag.Bag(args.bag) try: if args.all_topics: topic_info = bag.get_type_and_topic_info() topics = sorted([ t for t in topic_info[1].keys() if topic_info[1][t][0] == "geometry_msgs/PoseStamped" and t != args.ref ]) if len(topics) == 0: logging.error("no geometry_msgs/PoseStamped topics found!") sys.exit(1) else: topics = args.topics if not topics: logging.warning( "no topics used - specify topics or use the --all_topics flag" ) sys.exit(1) for topic in topics: trajectories.append( (topic, file_interface.read_bag_trajectory(bag, topic))) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() else: raise RuntimeError("unsupported subcommand: " + args.subcommand) if args.transform_left or args.transform_right: tf_path = args.transform_left if args.transform_left else args.transform_right t, xyz, quat = file_interface.load_transform_json(tf_path) logging.debug(SEP) logging.debug("applying transformation to the trajectories:\n" + str(t)) if args.invert_transform: t = lie.se3_inverse(t) for name, traj in trajectories: traj.transform(t, right_mul=args.transform_right) if args.align or args.correct_scale: if args.ref: if args.subcommand == "kitti": traj_tmp, ref_traj_tmp = trajectories, [ ref_traj for n, t in trajectories ] else: traj_tmp, ref_traj_tmp = [], [] from evo.algorithms import sync for name, traj in trajectories: logging.debug(SEP) ref_assoc, traj_assoc = sync.associate_trajectories( ref_traj, traj, first_name="ref", snd_name=name) ref_traj_tmp.append(ref_assoc) traj_tmp.append((name, traj_assoc)) trajectories = traj_tmp correct_only_scale = args.correct_scale and not args.align trajectories_new = [] for nt, ref_assoc in zip(trajectories, ref_traj_tmp): logging.debug(SEP) logging.debug("aligning " + nt[0] + " to " + args.ref + "...") trajectories_new.append( (nt[0], trajectory.align_trajectory(nt[1], ref_assoc, args.correct_scale, correct_only_scale, args.n_to_align))) trajectories = trajectories_new for name, traj in trajectories: if args.t_offset and traj.timestamps.shape[0] != 0: logging.debug(SEP) logging.info("adding time offset to " + name + ": " + str(args.t_offset) + " (s)") traj.timestamps += args.t_offset print_traj_info(name, traj, args.full_check) if (args.align or args.correct_scale) and not args.ref: logging.debug(SEP) logging.warning("can't align without a reference! (--ref) *grunt*") if args.ref: print_traj_info(args.ref, ref_traj, args.full_check) if args.plot or args.save_plot or args.serialize_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] import numpy as np from evo.tools import plot import matplotlib.pyplot as plt import matplotlib.cm as cm plot_collection = plot.PlotCollection("evo_traj - trajectory plot") fig_xyz, axarr_xyz = plt.subplots(3, sharex="col", figsize=tuple(SETTINGS.plot_figsize)) fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize)) if (args.align or args.correct_scale) and not args.ref: plt.xkcd(scale=2, randomness=4) fig_traj.suptitle("what if --ref?") fig_xyz.suptitle("what if --ref?") ax_traj = plot.prepare_axis(fig_traj, plot_mode) if args.ref: short_traj_name = os.path.splitext(os.path.basename(args.ref))[0] if SETTINGS.plot_usetex: short_traj_name = short_traj_name.replace("_", "\\_") plot.traj(ax_traj, plot_mode, ref_traj, '--', 'grey', short_traj_name, alpha=0 if SETTINGS.plot_hideref else 1) plot.traj_xyz(axarr_xyz, ref_traj, '--', 'grey', short_traj_name, alpha=0 if SETTINGS.plot_hideref else 1) cmap_colors = None if SETTINGS.plot_multi_cmap.lower() != "none": cmap = getattr(cm, SETTINGS.plot_multi_cmap) cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories)))) for name, traj in trajectories: if cmap_colors is None: color = next(ax_traj._get_lines.prop_cycler)['color'] else: color = next(cmap_colors) short_traj_name = os.path.splitext(os.path.basename(name))[0] if SETTINGS.plot_usetex: short_traj_name = short_traj_name.replace("_", "\\_") plot.traj(ax_traj, plot_mode, traj, '-', color, short_traj_name) if args.ref and isinstance(ref_traj, trajectory.PoseTrajectory3D): start_time = ref_traj.timestamps[0] else: start_time = None plot.traj_xyz(axarr_xyz, traj, '-', color, short_traj_name, start_timestamp=start_time) plt.tight_layout() plot_collection.add_figure("trajectories", fig_traj) plot_collection.add_figure("xyz_view", fig_xyz) if args.plot: plot_collection.show() if args.save_plot: logging.debug(SEP) plot_collection.export(args.save_plot, confirm_overwrite=not args.no_warnings) if args.serialize_plot: logging.debug(SEP) plot_collection.serialize(args.serialize_plot, confirm_overwrite=not args.no_warnings) if args.save_as_tum: logging.debug(SEP) for name, traj in trajectories: dest = os.path.splitext(os.path.basename(name))[0] + ".tum" file_interface.write_tum_trajectory_file( dest, traj, confirm_overwrite=not args.no_warnings) if args.ref: dest = os.path.splitext(os.path.basename(args.ref))[0] + ".tum" file_interface.write_tum_trajectory_file( dest, ref_traj, confirm_overwrite=not args.no_warnings) if args.save_as_kitti: logging.debug(SEP) for name, traj in trajectories: dest = os.path.splitext(os.path.basename(name))[0] + ".kitti" file_interface.write_kitti_poses_file( dest, traj, confirm_overwrite=not args.no_warnings) if args.ref: dest = os.path.splitext(os.path.basename(args.ref))[0] + ".kitti" file_interface.write_kitti_poses_file( dest, ref_traj, confirm_overwrite=not args.no_warnings) if args.save_as_bag: logging.debug(SEP) import datetime import rosbag dest_bag_path = str( datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S.%f')) + ".bag" logging.debug("saving trajectories to " + dest_bag_path + "...") bag = rosbag.Bag(dest_bag_path, 'w') try: for name, traj in trajectories: dest_topic = os.path.splitext(os.path.basename(name))[0] file_interface.write_bag_trajectory(bag, traj, dest_topic) if args.ref: dest_topic = os.path.splitext(os.path.basename(args.ref))[0] file_interface.write_bag_trajectory(bag, ref_traj, dest_topic) finally: bag.close()
from __future__ import print_function print("loading required evo modules") from evo.core import trajectory, sync, metrics from evo.tools import file_interface import rosbag print("checking if bag has readable topics") bag_handle = rosbag.Bag( "/media/devavratjivani/New Volume/Ubuntu18_ExtendedStorage/results_slam/bags_stereo/2021-03-25-15-57-38.bag", 'r') print(file_interface.get_supported_topics(bag_handle)) print("loading trajectories") traj_ref = file_interface.read_bag_trajectory(bag_handle, "/tf:map.cam00") traj_est = file_interface.read_bag_trajectory(bag_handle, "/tf:map.camera_link") print("registering and aligning trajectories") traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est) traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False) print("calculating APE") data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() print("mean:", ape_statistics["mean"]) print("loading plot modules")
# -*- coding: UTF-8 -*- """ 按照一定规律过滤rosbag信息,输出tum格式位姿真值文件。 使用了evo包内文件: author: Junchuan Zhang """ import rospy import rosbag import time import sys import os sys.path.insert(0, os.path.abspath('./evo/')) from evo.tools import file_interface # 输入的rosbag文件名: bag_file_name = "/data/AirSim/ros/dataset_1time_2.bag" # 输出的文件名: gt_output_name = "/data/AirSim/ros/pose_record.txt" # 将要转换的全体rostopic名称: topics = ["/airsim_car_node/drone_1/odom_local_ned"] bag_handle = rosbag.Bag(bag_file_name) time_start = time.time() traj_record = file_interface.read_bag_trajectory(bag_handle, topics[0]) file_interface.write_tum_trajectory_file(gt_output_name, traj_record) time_end = time.time() print('time cost', time_end - time_start, 's')
for i, segment in enumerate(segments): plot.linear_vel(axarr[0:2], segment, '-', color, method, 1, b.timestamps[0]) tracking.angular_vel(axarr[2], segment, '-', color, method, 1, b.timestamps[0]) plt.style.use(['seaborn-whitegrid', 'stylerc']) # bag = rosbag.Bag("/home/kostas/results/experiment/test.bag") bag = rosbag.Bag("/home/kostas/experiments/datmo.bag") type_of_exp = 'experiment' distance = 0.35 ref = [] ref.append(('red', file_interface.read_bag_trajectory(bag, '/red_pose'))) tracks = [] # tracks.append(('mean' , file_interface.read_TrackArray(bag, '/tracks/mean',3))) # tracks.append(('mean_kf', file_interface.read_TrackArray(bag,'/tracks/mean_kf', 3))) tracks.append(('KF', file_interface.read_TrackArray(bag, '/tracks/box_kf', 1))) # tracks.append(('UKF', file_interface.read_TrackArray(bag, '/tracks/box_ukf', 3))) bag.close() palette = itertools.cycle(sns.color_palette()) fig, axarr = plt.subplots(2, 3) plot.traj_xyyaw(axarr[0, 0:3], ref[0][1], '-', 'gray', 'reference', 1, ref[0][1].timestamps[0]) plot.traj_vel(axarr[1, 0:3], ref[0][1], '-', 'gray')
def load_trajectories(args): from collections import OrderedDict from evo.tools import file_interface trajectories = OrderedDict() ref_traj = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file == args.ref: continue trajectories[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: continue trajectories[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: continue else: trajectories[ 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 in ("bag", "bag2"): if not (args.topics or args.all_topics): die("No topics used - specify topics or set --all_topics.") if not os.path.exists(args.bag): raise file_interface.FileInterfaceException( "File doesn't exist: {}".format(args.bag)) logger.debug("Opening bag file " + args.bag) if args.subcommand == "bag2": from rosbags.rosbag2 import Reader as Rosbag2Reader bag = Rosbag2Reader(args.bag) else: from rosbags.rosbag1 import Reader as Rosbag1Reader bag = Rosbag1Reader(args.bag) bag.open() try: if args.all_topics: # Note: args.topics can have TF stuff here, so we add it too. topics = args.topics topics += natsorted(file_interface.get_supported_topics(bag)) if args.ref in topics: topics.remove(args.ref) if len(topics) == 0: die("No topics of supported types: {}".format(" ".join( file_interface.SUPPORTED_ROS_MSGS))) else: topics = args.topics for topic in topics: if topic == args.ref: continue trajectories[topic] = file_interface.read_bag_trajectory( bag, topic) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() return trajectories, ref_traj
ape_result.add_trajectory(ref_name, traj_ref) ape_result.add_trajectory(est_name, traj_est) if isinstance(traj_est, trajectory.PoseTrajectory3D): seconds_from_start = [ t - traj_est.timestamps[0] for t in traj_est.timestamps ] ape_result.add_np_array("seconds_from_start", seconds_from_start) ape_result.add_np_array("timestamps", traj_est.timestamps) return ape_result # bag = rosbag.Bag(sys.argv[1]) bag = rosbag.Bag('/home/kostas/results/local.bag') mocap = file_interface.read_bag_trajectory(bag, '/mocap_pose') odom = file_interface.read_bag_trajectory(bag, '/odometry/wheel_imu') slam = file_interface.read_bag_trajectory(bag, '/poseupdate') fuse = file_interface.read_bag_trajectory(bag, '/odometry/map') bag.close() loc_table = Tabular('l c c c c c c c') loc_table.add_hline() loc_table.add_row( ('Method', 'RMSE', 'Mean', 'Median', 'STD', 'Min', 'Max', 'SSE')) loc_table.add_hline() loc_table.add_empty_row() def three_plots(ref, est, table, name): """Generates plots and statistics table into Report
style, markersize=1, color=color, alpha=alpha) axarr[1, 0].set(xlabel=ylabels[0], ylabel=ylabels[1]) traj_yaw(axarr[1, 1], traj, style, color, alpha=alpha, start_timestamp=start_timestamp) bag = rosbag.Bag('/home/kostas/experiments/mocap.bag') # bag = rosbag.Bag('/home/kostas/datmo_ws/src/datmo/examples/parallel.bag') # bag = rosbag.Bag('/home/kostas/experiments/dsv/parallel/14-18-17.bag_') ego = file_interface.read_bag_trajectory(bag, '/ego_pose') red = file_interface.read_bag_trajectory(bag, '/red_pose') bag.close() fig, axarr = plt.subplots(2, 2) traj_fourplots(axarr, ego, '-', 'gray', 'DSV') traj_fourplots(axarr, red, '-', 'red', 'Red') handles, labels = axarr[0, 0].get_legend_handles_labels() # fig.legend(handles, labels, loc='lower center', ncol = 2, # bbox_to_anchor=(0.5, 0)) fig.tight_layout() plt.show() # fig.waitforbuttonpress()
#!/usr/bin/env python from __future__ import print_function # print("loading required evo modules") from evo.core import trajectory, sync, metrics from evo.tools import file_interface from datmo.msg import Track, TrackArray import rosbag bag = rosbag.Bag('all_funny.bag') topics = ['/odometry/wheel_imu', '/poseupdate', '/odometry/map'] trajectories = {} for topic in topics: print(topic) trajectories[topic] = file_interface.read_bag_trajectory(bag, topic) ref_traj = file_interface.read_bag_trajectory(bag, '/mocap_pose') bag.close() # print("registering and aligning trajectories") for traj in trajectories: traj_ref, traj_est = sync.associate_trajectories(ref_traj, trajectories[traj]) traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False) print("calculating APE") data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data)