def kitti_poses_and_timestamps_to_trajectory(poses_file, timestamp_file): pose_path = file_interface.read_kitti_poses_file(poses_file) raw_timestamps_mat = file_interface.csv_read_matrix(timestamp_file) error_msg = ("timestamp file must have one column of timestamps and same number of rows as the KITTI poses file") if len(raw_timestamps_mat) > 0 and len(raw_timestamps_mat[0]) != 1 or len(raw_timestamps_mat) != pose_path.num_poses: raise file_interface.FileInterfaceException(error_msg) try: timestamps_mat = np.array(raw_timestamps_mat).astype(float) except ValueError: raise file_interface.FileInterfaceException(error_msg) return PoseTrajectory3D(poses_se3=pose_path.poses_se3, timestamps=timestamps_mat)
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): 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: 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 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