def ape(traj_ref, traj_est, pose_relation, align=False, correct_scale=False, align_origin=False, ref_name="reference", est_name="estimate"): from evo.core import metrics from evo.core import trajectory # Align the trajectories. only_scale = correct_scale and not align if align or correct_scale: logger.debug(SEP) traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale, only_scale) elif align_origin: logger.debug(SEP) traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref) # Calculate APE. logger.debug(SEP) data = (traj_ref, traj_est) ape_metric = metrics.APE(pose_relation) ape_metric.process_data(data) title = str(ape_metric) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" ape_result = ape_metric.get_result(ref_name, est_name) ape_result.info["title"] = title logger.debug(SEP) logger.info(ape_result.pretty_str()) 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
def original_ape(traj_ref, traj_est, pose_relation, align=False, correct_scale=False, align_origin=False, ref_name="reference", est_name="estimate"): ''' Copied from main_ape.py ''' traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est) # Align the trajectories. only_scale = correct_scale and not align if align or correct_scale: traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale, only_scale) elif align_origin: traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref) # Calculate APE. data = (traj_ref, traj_est) ape_metric = metrics.APE(pose_relation) ape_metric.process_data(data) title = str(ape_metric) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" ape_result = ape_metric.get_result(ref_name, est_name) ape_result.info["title"] = title 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
def ape(traj_ref, traj_est, pose_relation, align=False, correct_scale=False, align_origin=False, ref_name="reference", est_name="estimate"): ''' Based on main_ape.py ''' traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est) # Align the trajectories. only_scale = correct_scale and not align if align or correct_scale: traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale, only_scale) elif align_origin: traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref) # Calculate APE. data = (traj_ref, traj_est) ape_metric = APE(pose_relation) ape_metric.process_data(data) ape_result = ape_metric.get_result(ref_name, est_name) 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
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, local_logfile=args.logfile) 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 = load_trajectories(args) if args.merge: if args.subcommand == "kitti": die("Can't merge KITTI files.") if len(trajectories) == 0: die("No trajectories to merge (excluding --ref).") trajectories = { "merged_trajectory": trajectory.merge(trajectories.values()) } 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 transform = file_interface.load_transform_json(tf_path) logger.debug(SEP) if not lie.is_se3(transform): logger.warning("Not a valid SE(3) transformation!") if args.invert_transform: transform = lie.se3_inverse(transform) logger.debug("Applying a {}-multiplicative transformation:\n{}".format( tf_type, transform)) for traj in trajectories.values(): traj.transform(transform, right_mul=args.transform_right, propagate=args.propagate_transform) if args.t_offset: logger.debug(SEP) for name, traj in trajectories.items(): if type(traj) is trajectory.PosePath3D: die("{} doesn't have timestamps - can't add time offset.". format(name)) logger.info("Adding time offset to {}: {} (s)".format( name, args.t_offset)) traj.timestamps += args.t_offset if args.n_to_align != -1 and not (args.align or args.correct_scale): die("--n_to_align is useless without --align or/and --correct_scale") if args.sync or args.align or args.correct_scale or args.align_origin: from evo.core import sync if not args.ref: logger.debug(SEP) die("Can't align or sync without a reference! (--ref) *grunt*") for name, traj in trajectories.items(): if args.subcommand == "kitti": ref_traj_tmp = ref_traj else: logger.debug(SEP) ref_traj_tmp, trajectories[name] = sync.associate_trajectories( ref_traj, traj, max_diff=args.t_max_diff, first_name="reference", snd_name=name) if args.align or args.correct_scale: logger.debug(SEP) logger.debug("Aligning {} to reference.".format(name)) trajectories[name] = trajectory.align_trajectory( trajectories[name], ref_traj_tmp, correct_scale=args.correct_scale, correct_only_scale=args.correct_scale and not args.align, n=args.n_to_align) if args.align_origin: logger.debug(SEP) logger.debug("Aligning {}'s origin to reference.".format(name)) trajectories[name] = trajectory.align_trajectory_origin( trajectories[name], ref_traj_tmp) print_compact_name = not args.subcommand == "bag" for name, traj in trajectories.items(): print_traj_info(name, traj, args.verbose, args.full_check, print_compact_name) if args.ref: print_traj_info(args.ref, ref_traj, args.verbose, args.full_check, print_compact_name) if args.plot or args.save_plot or args.serialize_plot: 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)) plot_mode = plot.PlotMode[args.plot_mode] 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, style=SETTINGS.plot_reference_linestyle, color=SETTINGS.plot_reference_color, label=short_traj_name, alpha=SETTINGS.plot_reference_alpha) plot.draw_coordinate_axes(ax_traj, ref_traj, plot_mode, SETTINGS.plot_axis_marker_scale) plot.traj_xyz(axarr_xyz, ref_traj, style=SETTINGS.plot_reference_linestyle, color=SETTINGS.plot_reference_color, label=short_traj_name, alpha=SETTINGS.plot_reference_alpha) plot.traj_rpy(axarr_rpy, ref_traj, style=SETTINGS.plot_reference_linestyle, color=SETTINGS.plot_reference_color, label=short_traj_name, alpha=SETTINGS.plot_reference_alpha) if args.ros_map_yaml: plot.ros_map(ax_traj, args.ros_map_yaml, plot_mode) 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.items(): if cmap_colors is None: color = next(ax_traj._get_lines.prop_cycler)['color'] else: color = next(cmap_colors) if print_compact_name: short_traj_name = os.path.splitext(os.path.basename(name))[0] else: short_traj_name = name if SETTINGS.plot_usetex: short_traj_name = short_traj_name.replace("_", "\\_") plot.traj(ax_traj, plot_mode, traj, SETTINGS.plot_trajectory_linestyle, color, short_traj_name, alpha=SETTINGS.plot_trajectory_alpha) plot.draw_coordinate_axes(ax_traj, traj, plot_mode, SETTINGS.plot_axis_marker_scale) 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, SETTINGS.plot_trajectory_linestyle, color, short_traj_name, alpha=SETTINGS.plot_trajectory_alpha, start_timestamp=start_time) plot.traj_rpy(axarr_rpy, traj, SETTINGS.plot_trajectory_linestyle, color, short_traj_name, alpha=SETTINGS.plot_trajectory_alpha, start_timestamp=start_time) if not SETTINGS.plot_usetex: fig_rpy.text(0., 0.005, "euler_angle_sequence: {}".format( SETTINGS.euler_angle_sequence), fontsize=6) 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.items(): 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.items(): 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: import datetime import rosbag dest_bag_path = str( datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag" logger.info(SEP) logger.info("Saving trajectories to " + dest_bag_path + "...") bag = rosbag.Bag(dest_bag_path, 'w') try: for name, traj in trajectories.items(): 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 rpe(traj_ref, traj_est, pose_relation, delta, delta_unit, rel_delta_tol=0.1, all_pairs=False, align=False, correct_scale=False, align_origin=False, ref_name="reference", est_name="estimate", support_loop=False): from evo.core import metrics from evo.core import trajectory # Align the trajectories. only_scale = correct_scale and not align if align or correct_scale: logger.debug(SEP) traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale, only_scale) elif align_origin: logger.debug(SEP) traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref) # Calculate RPE. logger.debug(SEP) data = (traj_ref, traj_est) rpe_metric = metrics.RPE(pose_relation, delta, delta_unit, rel_delta_tol, all_pairs) rpe_metric.process_data(data) title = str(rpe_metric) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" rpe_result = rpe_metric.get_result(ref_name, est_name) rpe_result.info["title"] = title logger.debug(SEP) logger.info(rpe_result.pretty_str()) # Restrict trajectories to delta ids for further processing steps. if support_loop: # Avoid overwriting if called repeatedly e.g. in Jupyter notebook. import copy traj_ref = copy.deepcopy(traj_ref) traj_est = copy.deepcopy(traj_est) traj_ref.reduce_to_ids(rpe_metric.delta_ids) traj_est.reduce_to_ids(rpe_metric.delta_ids) rpe_result.add_trajectory(ref_name, traj_ref) rpe_result.add_trajectory(est_name, traj_est) if isinstance(traj_est, trajectory.PoseTrajectory3D) and not all_pairs: seconds_from_start = [ t - traj_est.timestamps[0] for t in traj_est.timestamps ] rpe_result.add_np_array("seconds_from_start", seconds_from_start) rpe_result.add_np_array("timestamps", traj_est.timestamps) return rpe_result
def test_origin_alignment(self): traj_1 = helpers.fake_trajectory(1000, 1) traj_2 = helpers.fake_trajectory(1000, 1) self.assertFalse(np.allclose(traj_1.poses_se3[0], traj_2.poses_se3[0])) traj_2 = trajectory.align_trajectory_origin(traj_2, traj_1) self.assertTrue(np.allclose(traj_1.poses_se3[0], traj_2.poses_se3[0]))
def get_and_save_results_from_folder(folder_with_predicted_poses,category): global args global kitti_eval_tool global folder_with_gt_poses global output_folder global t global results values_for_excel = [] columns_for_excel = [] type_of_statistics = 'mean' for filename in sorted(os.listdir(folder_with_predicted_poses)): if not(os.path.exists(os.path.join(folder_with_gt_poses, filename))): print("file with gt poses doesn't exist for "+filename) continue if filename.find('.txt') == -1: continue seq_results = {} seq_results['name_seq'] = filename[:filename.rfind('.')] seq_results['category'] = category folder_name = seq_results['category'] seq_results['metrics'] = {} seq_results['lost'] = False os.makedirs(os.path.join(output_folder, folder_name), exist_ok=True) output_folder_seq = os.path.join(output_folder, folder_name, filename[:filename.rfind('.')]) os.makedirs(output_folder_seq, exist_ok=True) if os.path.isfile(os.path.join(output_folder, folder_name,"results.txt")): file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "a") else: file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "w") file_results_txt.write("translation_error(%) rotation_error(deg/m) ATE(m) APE_translation_error_median(m) APE_rotation_error_median(deg) dst_to_trgt\n") # -------------------------------------Getting results--------------------------------------------------- if args.gt_format == 'kitti': traj_ref = file_interface.read_kitti_poses_file(os.path.join(folder_with_gt_poses, filename)) if args.gt_format == 'tum': traj_ref = file_interface.read_tum_trajectory_file(os.path.join(folder_with_gt_poses, filename)) seq_results["length_of_ref_traj"] = traj_ref.path_length end_time_gt = traj_ref.get_infos()["t_end (s)"] if args.gt_format == 'euroc': traj_ref = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_gt_poses, filename)) if args.result_format == 'kitti': traj_est = file_interface.read_kitti_poses_file(os.path.join(folder_with_predicted_poses, filename)) if args.result_format == 'tum': traj_est = file_interface.read_tum_trajectory_file(os.path.join(folder_with_predicted_poses, filename)) seq_results["length_of_estimated_traj"] = traj_est.path_length if args.result_format == 'euroc': traj_est = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_predicted_poses, filename)) if args.result_format == 'tum' and args.gt_format == 'tum': seq_results["num_gt_poses"] = traj_ref.num_poses seq_results["num_predicted_poses"] = traj_est.num_poses traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, args.max_diff) end_time_est = traj_est.get_infos()["t_end (s)"] if (abs(end_time_est - end_time_gt) > 0.2) or (traj_est.get_infos()["t_start (s)"] > 0.2): print('LOST in track '+filename[:filename.rfind('.')]) seq_results['lost'] = True results.append(seq_results) t.update(1) continue if args.alignment != None: traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=args.alignment.find("scale") != -1, correct_only_scale=args.alignment=="scale") trajectory.align_trajectory_origin(traj_est, traj_ref) data = (traj_ref, traj_est) ape_metric_translation = metrics.APE(metrics.PoseRelation.translation_part) ape_metric_rotation = metrics.APE(metrics.PoseRelation.rotation_angle_deg) ape_metric_translation.process_data(data) ape_metric_rotation.process_data(data) ape_translation_statistics = ape_metric_translation.get_all_statistics() ape_rotation_statistics = ape_metric_rotation.get_all_statistics() ape_translation_statistics_plot = copy.deepcopy(ape_translation_statistics) ape_rotation_statistics_plot = copy.deepcopy(ape_rotation_statistics) ape_translation_statistics_plot.pop('sse') ape_translation_statistics_plot.pop('std') ape_translation_statistics_plot.pop('min') ape_translation_statistics_plot.pop('max') ape_rotation_statistics_plot.pop('sse') ape_rotation_statistics_plot.pop('std') ape_rotation_statistics_plot.pop('min') ape_rotation_statistics_plot.pop('max') kitti_trans_err, kitti_rot_err, ate = kitti_eval_tool.eval(traj_ref.poses_se3, traj_est.poses_se3, alignment=None) #---------------------------------adding results to variable seq_results for excel ----------------------------- seq_results['metrics']['dist_to_trgt'] = traj_est.get_infos()['pos_end (m)'] - traj_ref.get_infos()['pos_end (m)'] seq_results['metrics']['dist_to_trgt'] = np.sum(np.array(seq_results['metrics']['dist_to_trgt'])**2)**0.5 seq_results['metrics']["Kitti trans err (%)"] = kitti_trans_err seq_results['metrics']["Kitti rot err (deg/m)"] = kitti_rot_err seq_results['metrics']["ATE (m)"] = ate seq_results['metrics']["APE(trans err) median (m)"] = ape_translation_statistics["median"] seq_results['metrics']["APE(rot err) median (deg)"] = ape_rotation_statistics["median"] #-------------------------------------------------------------------------------------------------------- #------------------------------------------------------------------------------------------------------- # --------------------------------printing results into console---------------------------------------------- print('Results for "'+filename+'":') print('Kitti average translational error (%): {:.7f}'.format(kitti_trans_err)) print('Kitti average rotational error (deg/m): {:.7f}'.format(kitti_rot_err)) print('ATE (m): {:.7f}'.format(ate)) print('APE(translation error) median (m): {:.7f}'.format(ape_translation_statistics["median"])) print('APE(rotation error) median (deg): {:.7f}'.format(ape_rotation_statistics["median"])) print('distance to target on the last frame: {:.7f}'.format(seq_results['metrics']['dist_to_trgt'])) #------------------------------------------------------------------------------------------------------------ #---------------------------------Saving results into overall results text file------------------------------ file_results_txt.write('{:<24} '.format(filename[:filename.rfind('.')])) file_results_txt.write('{:>7.4f} '.format(kitti_trans_err)) file_results_txt.write('{:>7.4f} '.format(kitti_rot_err)) file_results_txt.write('{:>7.4f} '.format(ate)) file_results_txt.write('{:>7.4f} '.format(ape_translation_statistics["median"])) file_results_txt.write('{:>7.4f} '.format(ape_rotation_statistics["median"])) file_results_txt.write('{:>7.4f}\n'.format(seq_results['metrics']['dist_to_trgt'])) #------------------------------------------------------------------------------------------------------------ # --------------------------------Saving metrics to text file for one track---------------------------------- txt_filename = filename[:filename.rfind('.')]+"_metrics.txt" with open(os.path.join(output_folder_seq, txt_filename), "w") as txt_file: txt_file.write('Kitti average translational error (%): {:.7f}\n'.format(kitti_trans_err)) txt_file.write('Kitti average rotational error (deg/m): {:.7f}\n'.format(kitti_rot_err)) txt_file.write('ATE (m): {:.7f}\n'.format(ate)) txt_file.write('APE(translation error) median (m): {:.7f}\n'.format(ape_translation_statistics["median"])) txt_file.write('APE(rotation error) median (deg): {:.7f}\n'.format(ape_rotation_statistics["median"])) txt_file.write('Distance to target on the last frame: {:.7f}\n'.format(seq_results['metrics']['dist_to_trgt'])) #--------------------------------------------------------------------------------------------------------- # ---------------------------------Saving values of errors for each frame to text file------------------------ # ------------------------------------------for translation errors---------------------------------------- txt_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.txt" output_folder_seq_translation = os.path.join(output_folder_seq,"translation") output_folder_seq_rotation = os.path.join(output_folder_seq,"rotation") os.makedirs(output_folder_seq_translation, exist_ok=True) os.makedirs(output_folder_seq_rotation, exist_ok=True) with open(os.path.join(output_folder_seq_translation, txt_filename), "w") as txt_file: for error in ape_metric_translation.error: txt_file.write('{:.10f}\n'.format(error)) # -----------------------------------------for rotation degree errors-------------------------------------- txt_filename = filename[:filename.rfind('.')]+"_APE(rotation_deg)_errors.txt" with open(os.path.join(output_folder_seq_rotation, txt_filename), "w") as txt_file: for error in ape_metric_rotation.error: txt_file.write('{:.10f}\n'.format(error)) #---------------------------------------------------------------------------------------------------------- # ---------------------------------------Saving plot of errors of each frame------------------------------ # ------------------------------------------for translation errors---------------------------------------- plot_collection = plot.PlotCollection("Example") fig_1 = plt.figure(figsize=(8, 8)) plot.error_array(fig_1, ape_metric_translation.error, name="APE", title=str(ape_metric_translation), xlabel="Index of frame", ylabel='Error') plot_collection.add_figure("raw", fig_1) plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.png" plt.savefig(os.path.join(output_folder_seq_translation, plot_filename)) plt.close(fig_1) # -----------------------------------------for rotation degree errors-------------------------------------- plot_collection = plot.PlotCollection("Example") fig_1 = plt.figure(figsize=(8, 8)) plot.error_array(fig_1, ape_metric_rotation.error, name="APE", title=str(ape_metric_rotation), xlabel="Index of frame", ylabel='Error') plot_collection.add_figure("raw", fig_1) plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_errors.png" plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename)) plt.close(fig_1) #----------------------------------------------------------------------------------------------------------- # -----------------------------------------Saving trajectory plot------------------------------------------- # ------------------------------------------for translation errors---------------------------------------- fig_2 = plt.figure(figsize=(8, 8)) ax = plot.prepare_axis(fig_2, plot_mode) plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference') plot.traj_colormap( ax, traj_est, ape_metric_translation.error, plot_mode, min_map=ape_translation_statistics["min"], max_map=ape_translation_statistics["max"], title="APE translation mapped onto trajectory") plot_collection.add_figure("traj (error)", fig_2) plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_map.png" plt.savefig(os.path.join(output_folder_seq_translation,plot_filename)) plt.close(fig_2) # -----------------------------------------for rotation degree errors-------------------------------------- fig_2 = plt.figure(figsize=(8, 8)) ax = plot.prepare_axis(fig_2, plot_mode) plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference') plot.traj_colormap( ax, traj_est, ape_metric_rotation.error, plot_mode, min_map=ape_rotation_statistics["min"], max_map=ape_rotation_statistics["max"], title="APE rotation mapped onto trajectory") plot_collection.add_figure("traj (error)", fig_2) plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_map.png" plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename)) plt.close(fig_2) #----------------------------------------------------------------------------------------------------------- print() active_worksheet = wb['sheet1'] thin = Side(border_style="thin", color="000000") thick = Side(border_style="thick", color="000000") medium = Side(border_style="medium", color="000000") font_header = Font(name='Arial', size=10, bold=True, italic=False, vertAlign=None, underline='none', strike=False, color='FF000000') font_values = Font(name='Arial', size=10, bold=False, italic=False, vertAlign=None, underline='none', strike=False, color='FF000000') active_worksheet.row_dimensions[2].height = 35 file_results_txt.close() results.append(seq_results) t.update(1)
def ape(traj_ref_list, traj_est_list, pose_relation, align=False, correct_scale=False, align_origin=False, ref_name=["reference"], est_name=["estimate"]): from evo.core import metrics from evo.core import trajectory # Align the trajectories. only_scale = correct_scale and not align if align or correct_scale: logger.debug(SEP) for i in range(len(traj_ref_list)): print(traj_est_list[i], traj_ref_list[i]) traj_est_list[i] = trajectory.align_trajectory( traj_est_list[i], traj_ref_list[i], correct_scale, only_scale) elif align_origin: logger.debug(SEP) for i in range(len(traj_ref_list)): traj_est_list[i] = trajectory.align_trajectory_origin( traj_est_list[i], traj_ref_list[i]) # Calculate APE. logger.debug(SEP) data = [(traj_ref_list[i], traj_est_list[i]) for i in range(len(traj_ref_list))] ape_metric = [ metrics.APE(pose_relation) for i in range(len(traj_ref_list)) ] for i in range(len(traj_ref_list)): ape_metric[i].process_data(data[i]) ape_result = [ ape_metric[i].get_result(ref_name[i], est_name[i]) for i in range(len(traj_ref_list)) ] for i in range(len(traj_ref_list)): title = str(ape_metric[i]) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" ape_result[i].info["title"] = title logger.debug(SEP) logger.info(ape_result[i].pretty_str()) ape_result[i].add_trajectory(ref_name[i], traj_ref_list[i]) ape_result[i].add_trajectory(est_name[i], traj_est_list[i]) if isinstance(traj_est_list[i], trajectory.PoseTrajectory3D): seconds_from_start = [ t - traj_est_list[i].timestamps[0] for t in traj_est_list[i].timestamps ] ape_result[i].add_np_array("seconds_from_start", seconds_from_start) ape_result[i].add_np_array("timestamps", traj_est_list[i].timestamps) return ape_result
def four_plots(ref, est, table, name): """Generates plots and statistics table into Report :ref: PoseTrajectory3D object that is used as reference :est: PoseTrajectory3D object that is plotted against reference :table: Tabular object that is generated by Tabular('c c') :name: String that is used as name for file and table entry :returns: translation of reference against estimation """ ref, est = sync.associate_trajectories(ref, est) # est, rot, tra, s = trajectory.align_trajectory(est, # ref, correct_scale=False, return_parameters=True) est = trajectory.align_trajectory_origin(est, ref) data = (ref, est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() # Plot x, y, xy,yaw style = '-' if name == 'slam': style = 'o' plt.style.use(['seaborn-whitegrid', 'stylerc']) mpl.use('pgf') mpl.rcParams.update({ "text.usetex": True, "pgf.texsystem": "pdflatex", }) fig, axarr = plt.subplots(2, 2, figsize=(6.125, 4.8)) plot.traj_fourplots(axarr, est, style, sns.xkcd_rgb["pale red"], 'Estimation', 1, ref.timestamps[0]) plot.traj_fourplots(axarr, ref, '-', 'gray', 'MoCap Ground Truth') handles, labels = axarr[0, 0].get_legend_handles_labels() fig.legend(handles, labels, loc='lower center', ncol=2, bbox_to_anchor=(0.5, 0)) plt.tight_layout() fig.tight_layout() fig.subplots_adjust(bottom=0.18) fig.savefig("/home/kostas/report/figures/localization/" + name + ".pgf") if name == 'slam': name = name.upper() elif name == 'odometry': name = 'Odometry+IMU' else: name = name.capitalize() table.add_row(( name, round(ape_statistics["rmse"], 3), round(ape_statistics["mean"], 3), round(ape_statistics["median"], 3), round(ape_statistics["std"], 3), round(ape_statistics["min"], 3), round(ape_statistics["max"], 3), round(ape_statistics["sse"], 3), )) table.add_hline