def run(args: argparse.Namespace) -> None:
log.configure_logging(args.verbose,
args.silent,
args.debug,
local_logfile=args.logfile)
if args.debug:
from pprint import pformat
parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)})
logger.debug("main_parser config:\n{}".format(parser_str))
logger.debug(SEP)
traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args)
traj_ref_full = None
if args.plot_full_ref:
import copy
traj_ref_full = copy.deepcopy(traj_ref)
if isinstance(traj_ref, PoseTrajectory3D) and isinstance(
traj_est, PoseTrajectory3D):
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)
pose_relation = common.get_pose_relation(args)
result = ape(
traj_ref=traj_ref,
traj_est=traj_est,
pose_relation=pose_relation,
align=args.align,
correct_scale=args.correct_scale,
n_to_align=args.n_to_align,
align_origin=args.align_origin,
ref_name=ref_name,
est_name=est_name,
)
if args.plot or args.save_plot or args.serialize_plot:
common.plot_result(args,
result,
traj_ref,
result.trajectories[est_name],
traj_ref_full=traj_ref_full)
if args.save_results:
logger.debug(SEP)
if not SETTINGS.save_traj_in_zip:
del result.trajectories[ref_name]
del result.trajectories[est_name]
file_interface.save_res_file(args.save_results,
result,
confirm_overwrite=not args.no_warnings)
def run(args: argparse.Namespace) -> None:
print('using this main_ape')
log.configure_logging(args.verbose,
args.silent,
args.debug,
local_logfile=args.logfile)
if args.debug:
from pprint import pformat
parser_str = pformat({arg: getattr(args, arg) for arg in vars(args)})
logger.debug("main_parser config:\n{}".format(parser_str))
logger.debug(SEP)
traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args)
traj_ref_full = None
if args.plot_full_ref:
import copy
traj_ref_full = copy.deepcopy(traj_ref)
if args.flip_xy:
print("flip xy", args.flip_xy)
flip_rotation = transformations.rotation_matrix(math.pi / 2, [0, 0, 1])
print('transformation matrix,', flip_rotation)
traj_est.transform_rotation_only(flip_rotation)
if isinstance(traj_ref, PoseTrajectory3D) and isinstance(
traj_est, PoseTrajectory3D):
logger.debug(SEP)
if args.t_start or args.t_end:
if args.t_start:
logger.info("Using time range start: {}s".format(args.t_start))
if args.t_end:
logger.info("Using time range end: {}s".format(args.t_end))
traj_ref.reduce_to_time_range(args.t_start, args.t_end)
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)
pose_relation = common.get_pose_relation(args)
print('align_odom: ', args.align_odom)
result = ape(
traj_ref=traj_ref,
traj_est=traj_est,
pose_relation=pose_relation,
align=args.align,
correct_scale=args.correct_scale,
n_to_align=args.n_to_align,
align_origin=args.align_origin,
align_odom=args.align_odom,
ref_name=ref_name,
est_name=est_name,
)
if args.plot or args.save_plot or args.serialize_plot:
common.plot_result(args,
result,
traj_ref,
result.trajectories[est_name],
traj_ref_full=traj_ref_full)
if args.save_results:
logger.debug(SEP)
if not SETTINGS.save_traj_in_zip:
del result.trajectories[ref_name]
del result.trajectories[est_name]
file_interface.save_res_file(args.save_results,
result,
confirm_overwrite=not args.no_warnings)