def run(args): import sys from evo.algorithms import metrics from evo.tools import file_interface, settings # manually check bins and tols arguments to allow them to be in config files if not args.bins or not args.tols: logging.error( "the following arguments are required: -b/--bins, -t/--tols") sys.exit(1) settings.configure_logging(args.verbose, args.silent, 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) 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 logging.info( "forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)" ) logging.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 logging.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 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)
def run(args): from evo.algorithms import metrics from evo.tools import file_interface, settings settings.configure_logging(args.verbose, args.silent, 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) 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 logging.info( "forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)" ) logging.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 logging.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_est, pose_relation, args.align, args.correct_scale, ref_name, est_name, args.plot, args.save_plot, plot_mode, args.save_results, args.no_warnings, serialize_plot=args.serialize_plot)
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 delta_unit = None if args.delta_unit == "f": delta_unit = metrics.Unit.frames elif args.delta_unit == "d": delta_unit = metrics.Unit.degrees elif args.delta_unit == "r": delta_unit = metrics.Unit.radians elif args.delta_unit == "m": delta_unit = metrics.Unit.meters 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 {} ...".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 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_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, 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, )