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):
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)
from evo.tools import file_interface
pretty_printer = pprint.PrettyPrinter(indent=4)
logging.basicConfig(format="%(message)s",
stream=sys.stdout,
level=logging.DEBUG)
# load trajectories (examples from TUM benchmark)
ref_file = "data/freiburg1_xyz-groundtruth.txt"
est_file = "data/freiburg1_xyz-rgbdslam.txt"
max_diff = 0.01
offset = 0.0
start = time.clock()
traj_ref, traj_est = file_interface.load_assoc_tum_trajectories(
est_file,
ref_file,
max_diff,
offset,
)
stop = time.clock()
load_time = stop - start
print("elapsed time for trajectory loading (seconds): \t",
"{0:.6f}".format(load_time))
"""
test relative pose error algorithms
for different types of pose relations
"""
delta = 1
delta_unit = metrics.Unit.frames
for pose_relation in metrics.PoseRelation:
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,
)