def calc_euroc_seq_errors(est_traj, gt_traj): gt_traj_synced, est_traj_synced = sync.associate_trajectories( gt_traj, est_traj, max_diff=0.01) est_traj_aligned = trajectory.align_trajectory(est_traj_synced, gt_traj_synced, correct_scale=False, correct_only_scale=False) pose_relation = metrics.PoseRelation.translation_part ape_metric = metrics.APE(pose_relation) ape_metric.process_data(( gt_traj_synced, est_traj_aligned, )) ape_stat = ape_metric.get_statistic(metrics.StatisticsType.rmse) # ape_metric = metrics.RPE(pose_relation) # ape_metric.process_data((gt_traj_synced, est_traj_aligned,)) # ape_stat = ape_metric.get_statistic(metrics.StatisticsType.rmse) # fig = plt.figure() # traj_by_label = { # "estimate (not aligned)": est_traj, # "estimate (aligned)": est_traj_aligned, # "reference": gt_traj # } # plot.trajectories(fig, traj_by_label, plot.PlotMode.xyz) # plt.show() return ape_metric, ape_stat
def test_association(self): traj_1 = helpers.fake_trajectory(10, 0.1) traj_2 = helpers.fake_trajectory(100, 0.01) traj_1_sync, traj_2_sync = sync.associate_trajectories(traj_1, traj_2) self.assertEqual(traj_1_sync.num_poses, traj_2_sync.num_poses) self.assertNotEqual(traj_2.num_poses, traj_2_sync.num_poses) self.assertEqual(traj_2_sync.num_poses, 10)
def stats_to_latex_table(traj_ref, segments, idx, table): """Associate segments of an object trajectory as given by a DATMO system with the object's reference trajectory :traj_ref: Reference trajectory :segments: All the segments of the robot trajectory :table: Latex table that the statistics get added to """ whole = trajectory.merge(segments) traj_ref, traj_est = sync.associate_trajectories(traj_ref, whole, max_diff=0.01) data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() # print(traj_est.get_infos()) table.add_row(( idx + 1, 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
def run(args): import evo.common_ape_rpe as common from evo import EvoException from evo.core import sync from evo.tools import file_interface, log from evo.tools.settings import SETTINGS 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) if (args.plot or args.save_plot or args.serialize_plot) and args.all_pairs: raise EvoException( "all_pairs mode cannot be used with plotting functions") traj_ref, traj_est, ref_name, est_name = common.load_trajectories(args) pose_relation = common.get_pose_relation(args) delta_unit = common.get_delta_unit(args) traj_ref_full = None if args.plot_full_ref: import copy traj_ref_full = copy.deepcopy(traj_ref) if args.subcommand != "kitti": 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) result = 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, 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(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: 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: 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) result = segment_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, segment_length=args.seg, ref_name=ref_name, est_name=est_name, plot=args.plot)
def ape(traj_ref, traj_est, pose_relation, align=False, correct_scale=False, align_origin=False, ref_name="reference", est_name="estimate"): 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: # 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 align_trajectory_by_vicon_whole(trj_est, trj_vicon): trj_vicon_sync, trj_est_sync = sync.associate_trajectories( trj_vicon, trj_est) trj_copy = copy.deepcopy( trj_est_sync ) # otherwise np arrays will be references and mess up stuff r_a, t_a, s = geometry.umeyama_alignment(trj_copy.positions_xyz.T, trj_vicon_sync.positions_xyz.T, False) res = lie.se3(r_a, t_a) return res
def evaluate_results(structvio_res_path, data_folder): ape_res = [] trj_est = read_structvio_trajectory_file(structvio_res_path) name = os.path.basename(data_folder) print(name) vicon_path = os.path.join(data_folder, 'vicon.txt') arcode_a_path = os.path.join(data_folder, name + '-ArUco-a.txt') arcode_b_path = os.path.join(data_folder, name + '-ArUco-b.txt') if (os.path.exists(vicon_path)): print("using Vicon ground truth:") print(vicon_path) #read the ground truth trj_vicon = read_tum_trajectory_file(vicon_path) res = align_trajectory_by_vicon(trj_est, trj_vicon) trj_est.transform(res[0]) trj1, trj2 = sync.associate_trajectories(trj_vicon, trj_est) ape_res = ape(trj1, trj2, PoseRelation.translation_part) elif os.path.exists(arcode_a_path) and os.path.exists(arcode_b_path): #read the ground truth print("using ArUco ground truth:") print(arcode_a_path) print(arcode_b_path) trj_ar = read_arcode_trajectory_files(data_folder) if trj_ar[1].timestamps[0] > trj_est.timestamps[-1]: print('Incomplete trajectory!') exit(-1) se3 = align_trajectory_by_arcode(trj_est, trj_ar) trj_est.transform(se3) trj_ar_merge = merge_arcode_trajeoctries(trj_ar[0], trj_ar[1]) trj1, trj2 = sync.associate_trajectories(trj_ar_merge, trj_est) ape_res = ape(trj1, trj2, PoseRelation.translation_part) else: print('cannot find any ground truth files!') exit(-1) return ape_res
def associate_segments_common_frame(traj, tracks, distance): """Associate segments of an object trajectory as given by a DATMO system with the object's reference trajectory :traj: Reference trajectory :tracks: All the tracks that got produced by the DATMO system :localization: The trajectory of the self-localization :returns: segments: The tracks that match to the reference trajectory :returns: traj_ref: The part of the reference trajectory that matches with tracks """ matches = [] for tr in tracks: # Find the best matching tracks to the object trajectory traj_ref, traj_est = sync.associate_trajectories(traj, tr, max_diff=0.1) # print("calculating APE for track of length", len(tr.timestamps)) data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() # print(ape_statistics) mismatch = ape_statistics['mean'] # print(mismatch) tuple = [ traj_est, mismatch, traj_est.get_infos()['t_start (s)'], traj_ref ] matches.append(tuple) matches.sort(key=lambda x: x[2]) segments_track = [] #The parts of the trajectory are added to this list segments_refer = [ ] #The parts of the reference trajectory are added to this list for m in matches: if m[1] < distance: # if the mismatch is smaller than 1 # print(m[1],distance) # print(m[0].get_statistics()['v_avg (m/s)']) segments_track.append(m[0]) segments_refer.append(m[3]) # print(m[0].get_infos()['t_start (s)'],m[0].get_infos()["path length (m)"]) # print(m[0].get_statistics()['v_avg (m/s)']) if len(segments_track) == 0: print("No matching segments") traj_ref = trajectory.merge(segments_refer) # print(traj_ref.length) return segments_track, traj_ref
def run(args): import evo.common_ape_rpe as common from evo.core import sync from evo.tools import file_interface, log from evo.tools.settings import SETTINGS 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_list, traj_est_list, ref_name, est_name = common.load_trajectories_multi( args) for i in range(len(traj_ref_list)): traj_ref_full = None if args.plot_full_ref: import copy traj_ref_full = copy.deepcopy(traj_ref_list[i]) if args.subcommand != "kitti": logger.debug("Synchronizing trajectories...") traj_ref_list[i], traj_est_list[i] = sync.associate_trajectories( traj_ref_list[i], traj_est_list[i], args.t_max_diff, args.t_offset, first_name=ref_name[i], snd_name=est_name[i]) pose_relation = common.get_pose_relation(args) result = ape( traj_ref_list=traj_ref_list, traj_est_list=traj_est_list, pose_relation=pose_relation, align=args.align, correct_scale=args.correct_scale, 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_multi(args, result, traj_ref_list, traj_est_list)
def eval(ref_p_path, ref_t_path, est_p_path, est_t_path): #pose1 ref_pose = file_interface.read_kitti_poses_file(ref_p_path) #PosePath3D ref_time = np.loadtxt(ref_t_path) #time assert len(ref_time) == ref_pose.num_poses ref_traj = path2trajectory(ref_pose, ref_time) #PoseTrajectory3D #pose2 est_pose = file_interface.read_kitti_poses_file(est_p_path) #PosePath3D est_time = np.loadtxt(est_t_path) #time assert len(est_time) == est_pose.num_poses est_traj = path2trajectory(est_pose, est_time) #PoseTrajectory3D #print(ref_traj.get_infos(),est_traj.get_infos()) #sync-基于少量数据同步 max_diff = 0.1 # 雷达10Hz, 最大为 traj_ref, traj_est = sync.associate_trajectories(ref_traj, est_traj, max_diff) data = (traj_ref, traj_est) #APE--全局评估 ape_pose_relation = metrics.PoseRelation.full_transformation ape_statis, ape_err = get_ape_static_raw(data, ape_pose_relation) ape_err = np.asarray(ape_err) #print(ape_statis,ape_total) #RPE-细节评估 rpe_relation_t = metrics.PoseRelation.translation_part rpe_relation_r = metrics.PoseRelation.rotation_part """ frame 一般不涉及单位,可以进行逐帧对比 取值delta=1,不太影响评估误差数量 使用frame,1,利用最小粒度单位进行详细评估,寻找异常帧位置,从而修改代码。 meters 可以通过设置10/20/50/100 m ,会缩小评估误差数量 degrees=radians, filter_pairs_by_angle """ delta = 20 #评估窗口 delta_unit = metrics.Unit.meters #评估单位 [meters,frames,degrees] all_pairs = False rpe_statis_t, rpe_trans = get_rpe_static_raw(data, rpe_relation_t, delta, delta_unit, all_pairs) #translation rpe_statis_r, rpe_rotat = get_rpe_static_raw(data, rpe_relation_r, delta, delta_unit, all_pairs) #rotation # static = np.vstack((ape_statis, rpe_statis_t, rpe_statis_r)) # 固定统计类,可合并 rpe_err = np.vstack((rpe_trans, rpe_rotat)) #数据不等长,不可合并 return static, ape_err, rpe_err
def three_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) data = (ref, est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() # [ Localization ] fig, axarr = plt.subplots(3) #sharex=True) fig.suptitle('Localization', fontsize=30) fig.tight_layout() plot.traj_xyyaw(axarr, est, '-', 'red', 'estimation', 1, ref.timestamps[0]) plot.traj_xyyaw(axarr, ref, '-', 'gray', 'original') fig.subplots_adjust(hspace=0.2) plt.waitforbuttonpress(0) plt.savefig("/home/kostas/results/latest/" + name + ".png", format='png', bbox_inches='tight') plt.close(fig) 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
def load_assoc_euroc_trajectories(ref_file, est_file, max_diff=0.01, offset_2=0.0, invert=False): """ parses ground truth trajectory from EuRoC MAV state estimate .csv and estimated TUM trajectory and returns the data with associated (matching) timestamps according to the time parameters :param ref_file: ground truth: <sequence>/mav0/state_groundtruth_estimate0/data.csv :param est_file: estimated TUM trajectory file :param max_diff: max. allowed absolute time difference for associating :param offset_2: optional time offset of second timestamp vector :param invert: set to True to match from longer list to short (default: short to longer list) :return: trajectory.PoseTrajectory3D objects traj_ref and traj_est """ traj_ref = read_euroc_csv_trajectory(ref_file) traj_est = read_tum_trajectory_file(est_file) return sync.associate_trajectories(traj_ref, traj_est, max_diff, offset_2, invert, ref_file, est_file)
def load_assoc_tum_trajectories(ref_file, est_file, max_diff=0.01, offset_2=0.0, invert=False): """ parses two trajectory files in TUM format (timestamp tx ty tz qx qy qz qw) and returns the data with associated (matching) timestamps according to the time parameters :param ref_file: first trajectory file :param est_file: second trajectory file :param max_diff: max. allowed absolute time difference for associating :param offset_2: optional time offset of second timestamp vector :param invert: set to True to match from longer list to short (default: short to longer list) :return: trajectory.PoseTrajectory3D objects traj_ref and traj_est """ traj_ref = read_tum_trajectory_file(ref_file) traj_est = read_tum_trajectory_file(est_file) return sync.associate_trajectories(traj_ref, traj_est, max_diff, offset_2, invert, ref_file, est_file)
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": args.align = True logger.info("Forcing trajectory alignment implicitly " "(EuRoC ground truth is in IMU frame).") logger.debug(SEP) traj_ref = file_interface.read_euroc_csv_trajectory(args.state_gt_csv) traj_est = file_interface.read_euroc_csv_trajectory(args.est_file) ref_name, est_name = args.state_gt_csv, args.est_file elif args.subcommand == "bag": import rosbag logger.debug("Opening bag file " + args.bag) 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)) if args.subcommand != "kitti": 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) return traj_ref, traj_est, ref_name, est_name
def load_assoc_bag_trajectories(bag_handle, ref_topic, est_topic, max_diff=0.01, offset_2=0.0, invert=False): """ reads trajectory data from a ROS bag file with two geometry_msgs/PoseStamped topics and returns the data with associated (matching) timestamps according to the time parameters :param bag_handle: opened bag handle, from rosbag.Bag(...) :param ref_topic: first geometry_msgs/PoseStamped topic :param est_topic: second geometry_msgs/PoseStamped topic :param max_diff: max. allowed absolute time difference for associating :param offset_2: optional time offset of second timestamp vector :param invert: set to True to match from longer list to short (default: short to longer list) :return: trajectory.PoseTrajectory3D objects traj_ref and traj_est """ traj_ref = read_bag_trajectory(bag_handle, ref_topic) traj_est = read_bag_trajectory(bag_handle, est_topic) return sync.associate_trajectories(traj_ref, traj_est, max_diff, offset_2, invert, ref_topic, est_topic)
def compare_using_APE(ref_file, est_file, use_aligned_trajectories=False): """ Compare two files using EVO API. Using the APE metric. :param ref_file: :param est_file: :param use_aligned_trajectories: True to align before comparing. False to leave original data as it is. :return: """ # Load trajectories traj_ref = file_interface.read_tum_trajectory_file(ref_file) traj_est = file_interface.read_tum_trajectory_file(est_file) # Sinchronize trajectories by timestamps max_diff = 0.01 traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, max_diff) # -------------EVO_APE------------- # Settings pose_relation = metrics.PoseRelation.translation_part use_aligned_trajectories = False # OPTION -va on the scripts. Is related to Uleyamas alignment # The aligned trajectories can be used if we want it to if use_aligned_trajectories: # Align trajectories with Uleyamas algorithm try: traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False, correct_only_scale=False) data = (traj_ref, traj_est_aligned) except GeometryException: print("Couldnt align with Uleyamas algorithm...") data = (traj_ref, traj_est) else: data = (traj_ref, traj_est) ape_metric = metrics.APE(pose_relation) # APE with only pose is in reality ATE (Absolute trajectory error) instead of APE (abs. pose error) ape_metric.process_data(data) # Get all stadistics in a dict ape_stats = ape_metric.get_all_statistics() return ape_stats
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": args.align = True logger.info("Forcing trajectory alignment implicitly " "(EuRoC ground truth is in IMU frame).") logger.debug(SEP) 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 rosbag logger.debug("Opening bag file " + args.bag) 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)) if args.subcommand != "kitti": 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) return traj_ref, traj_est, ref_name, est_name
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) 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 = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file != args.ref: trajectories.append( (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: trajectories.append( (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: trajectories.append( (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": import rosbag bag = rosbag.Bag(args.bag) try: if args.all_topics: topic_info = bag.get_type_and_topic_info() topics = sorted([ t for t in topic_info[1].keys() if topic_info[1][t][0] == "geometry_msgs/PoseStamped" and t != args.ref ]) if len(topics) == 0: logger.error("No geometry_msgs/PoseStamped topics found!") sys.exit(1) else: topics = args.topics if not topics: logger.warning( "No topics used - specify topics or set --all_topics.") sys.exit(1) for topic in topics: trajectories.append( (topic, file_interface.read_bag_trajectory(bag, topic))) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() else: raise RuntimeError("unsupported subcommand: " + args.subcommand) if args.merge: if args.subcommand == "kitti": raise TypeError( "can't merge KITTI files - but you can append them with 'cat'") if len(trajectories) == 0: raise RuntimeError("no trajectories to merge (excluding --ref)") merged_stamps = trajectories[0][1].timestamps merged_xyz = trajectories[0][1].positions_xyz merged_quat = trajectories[0][1].orientations_quat_wxyz for _, traj in trajectories[1:]: merged_stamps = np.concatenate((merged_stamps, traj.timestamps)) merged_xyz = np.concatenate((merged_xyz, traj.positions_xyz)) merged_quat = np.concatenate( (merged_quat, traj.orientations_quat_wxyz)) order = merged_stamps.argsort() merged_stamps = merged_stamps[order] merged_xyz = merged_xyz[order] merged_quat = merged_quat[order] trajectories = [("merged_trajectory", PoseTrajectory3D(merged_xyz, merged_quat, merged_stamps))] 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 t, xyz, quat = file_interface.load_transform_json(tf_path) logger.debug(SEP) if not lie.is_se3(t): logger.warning("Not a valid SE(3) transformation!") if args.invert_transform: t = lie.se3_inverse(t) logger.debug("Applying a {}-multiplicative transformation:\n{}".format( tf_type, t)) for name, traj in trajectories: traj.transform(t, right_mul=args.transform_right) if args.t_offset: logger.debug(SEP) for name, traj in trajectories: if type(traj) is trajectory.PosePath3D: logger.warning( "{} doesn't have timestamps - can't add time offset.". format(name)) else: logger.info("Adding time offset to {}: {} (s)".format( name, args.t_offset)) traj.timestamps += args.t_offset if args.align or args.correct_scale: if not args.ref: logger.debug(SEP) logger.warning("Can't align without a reference! (--ref) *grunt*") else: if args.subcommand == "kitti": traj_tmp, ref_traj_tmp = trajectories, [ ref_traj for n, t in trajectories ] else: traj_tmp, ref_traj_tmp = [], [] from evo.core import sync for name, traj in trajectories: logger.debug(SEP) ref_assoc, traj_assoc = sync.associate_trajectories( ref_traj, traj, max_diff=args.t_max_diff, first_name="ref", snd_name=name) ref_traj_tmp.append(ref_assoc) traj_tmp.append((name, traj_assoc)) trajectories = traj_tmp correct_only_scale = args.correct_scale and not args.align trajectories_new = [] for nt, ref_assoc in zip(trajectories, ref_traj_tmp): logger.debug(SEP) logger.debug("Aligning " + nt[0] + " to " + args.ref + "...") trajectories_new.append( (nt[0], trajectory.align_trajectory(nt[1], ref_assoc, args.correct_scale, correct_only_scale, args.n_to_align))) trajectories = trajectories_new for name, traj in trajectories: print_traj_info(name, traj, args.verbose, args.full_check) if args.ref: print_traj_info(args.ref, ref_traj, args.verbose, args.full_check) if args.plot or args.save_plot or args.serialize_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] 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)) if (args.align or args.correct_scale) and not args.ref: plt.xkcd(scale=2, randomness=4) fig_traj.suptitle("what if --ref?") fig_xyz.suptitle("what if --ref?") 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, '--', 'grey', short_traj_name, alpha=0 if SETTINGS.plot_hideref else 1) plot.traj_xyz(axarr_xyz, ref_traj, '--', 'grey', short_traj_name, alpha=0 if SETTINGS.plot_hideref else 1) plot.traj_rpy(axarr_rpy, ref_traj, '--', 'grey', short_traj_name, alpha=0 if SETTINGS.plot_hideref else 1) 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: if cmap_colors is None: color = next(ax_traj._get_lines.prop_cycler)['color'] else: color = next(cmap_colors) short_traj_name = os.path.splitext(os.path.basename(name))[0] if SETTINGS.plot_usetex: short_traj_name = short_traj_name.replace("_", "\\_") plot.traj(ax_traj, plot_mode, traj, '-', color, short_traj_name) 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, '-', color, short_traj_name, start_timestamp=start_time) plot.traj_rpy(axarr_rpy, traj, '-', color, short_traj_name, start_timestamp=start_time) plt.tight_layout() 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: 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: 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: logger.info(SEP) import datetime import rosbag dest_bag_path = str( datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S.%f')) + ".bag" logger.info("Saving trajectories to " + dest_bag_path + "...") bag = rosbag.Bag(dest_bag_path, 'w') try: for name, traj in trajectories: 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()
print("Left cam calibration matrix: ") print(body_T_leftCam) # %% gt_df = gt_df[~gt_df.index.duplicated()] # %% # Generate some trajectories for later plots # Convert to evo trajectory objects traj_ref_unassociated = pandas_bridge.df_to_trajectory(gt_df) # Use the mono ransac file as estimated trajectory. traj_est_unassociated = pandas_bridge.df_to_trajectory(mono_df) # Associate the trajectories traj_ref_abs, traj_est_rel = sync.associate_trajectories( traj_ref_unassociated, traj_est_unassociated) traj_ref_rel = convert_abs_traj_to_rel_traj(traj_ref_abs, up_to_scale=False) # Transform the relative gt trajectory from body to left camera frame traj_ref_cam_rel = convert_rel_traj_from_body_to_cam(traj_ref_rel, body_T_leftCam) # Remove the first timestamp; we don't have relative pose at first gt timestamp traj_est_rel = trajectory.PoseTrajectory3D( traj_est_rel._positions_xyz[1:], traj_est_rel._orientations_quat_wxyz[1:], traj_est_rel.timestamps[1:], ) print("traj_ref_rel: ", traj_ref_rel)
def associate_segments(traj, tracks): """Associate segments of an object trajectory as given by a DATMO system with the object's reference trajectory :traj: Reference trajectory :tracks: All the tracks that got produced by the DATMO system :localization: The trajectory of the self-localization :returns: segments: The tracks that match to the reference trajectory :returns: traj_ref: The part of the reference trajectory that matches with tracks """ matches = [] for tr in tracks: # Find the best matching tracks to the object trajectory traj_ref, traj_est = sync.associate_trajectories(traj, tr, max_diff=0.01) traj_est, rot, tra, _ = trajectory.align_trajectory( traj_est, traj_ref, correct_scale=False, return_parameters=True) # print("calculating APE for track of length", len(tr.timestamps)) data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() tra_dif = (tra - loc_tra) # print(tra) abs_tra_dif = abs((tra - loc_tra)[0]) + abs((tra - loc_tra)[1]) translation = abs(tra[0]) + abs(tra[1]) rot_dif = (rot - loc_rot) abs_rot_dif = 0 for i in range(0, len(rot_dif)): abs_rot_dif += abs(rot_dif[i][0])+ abs(rot_dif[i][1]) +\ abs(rot_dif[i][2]) # print(abs_tra_dif,abs_rot_dif) mismatch = abs_tra_dif + abs_rot_dif tuple = [traj_est, mismatch, traj_est.get_infos()['t_start (s)']] matches.append(tuple) matches.sort(key=lambda x: x[2]) segments = [] #The parts of the trajectory are added to this list for m in matches: # print(m[1]) if m[1] < 0.1: # if the mismatch is smaller than 1 # print(m[0].get_statistics()['v_avg (m/s)']) segments.append(m[0]) # print(m[0].get_infos()['t_start (s)'],m[0].get_infos()["path length (m)"]) # print(m[0].get_statistics()['v_avg (m/s)']) if len(segments) == 0: print("No matching segments") whole = trajectory.merge(segments) traj_ref, traj_est = sync.associate_trajectories(traj, whole, max_diff=0.01) traj_est, rot, tra, _ = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False, return_parameters=True) # print(traj_est.get_infos()) return segments, traj_ref, translation
#!/usr/bin/env python from __future__ import print_function print("loading required evo modules") from evo.core import trajectory, sync, metrics from evo.tools import file_interface print("loading trajectories") traj_ref = file_interface.read_tum_trajectory_file("../../test/data/fr2_desk_groundtruth.txt") traj_est = file_interface.read_tum_trajectory_file("../../test/data/fr2_desk_ORB.txt") print("registering and aligning trajectories") traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est) traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False) print("calculating APE") data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() print("mean:", ape_statistics["mean"]) print("loading plot modules") from evo.tools import plot import matplotlib.pyplot as plt print("plotting") plot_collection = plot.PlotCollection("Example") # metric values fig_1 = plt.figure(figsize=(8, 8))
def test_no_matches_due_to_offset(self): traj_1 = helpers.fake_trajectory(10, 0.1, start_time=0.) traj_2 = helpers.fake_trajectory(10, 0.1, start_time=2.) with self.assertRaises(sync.SyncException): sync.associate_trajectories(traj_1, traj_2)
def test_wrong_type(self): path_1 = helpers.fake_path(10) path_2 = helpers.fake_path(10) with self.assertRaises(sync.SyncException): sync.associate_trajectories(path_1, path_2)
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)
#!/usr/bin/env python from __future__ import print_function print("loading required evo modules") from evo.core import trajectory, sync, metrics from evo.tools import file_interface print("loading trajectories") traj_ref = file_interface.read_tum_trajectory_file( "../../test/data/fr2_desk_groundtruth.txt") traj_est = file_interface.read_tum_trajectory_file( "../../test/data/fr2_desk_ORB.txt") print("registering and aligning trajectories") traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est) traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False) print("calculating APE") data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() print("mean:", ape_statistics["mean"]) print("loading plot modules") from evo.tools import plot import matplotlib.pyplot as plt print("plotting") plot_collection = plot.PlotCollection("Example")
# %% gt_df = gt_df[~gt_df.index.duplicated()] # %% # Convert the gt relative-pose DataFrame to a trajectory object. traj_ref_complete = pandas_bridge.df_to_trajectory(gt_df) # Use the backend poses as trajectory. traj_est_unaligned = pandas_bridge.df_to_trajectory(output_poses_df) discard_n_start_poses = 0 discard_n_end_poses = 0 # Associate the data. traj_est = copy.deepcopy(traj_est_unaligned) traj_ref, traj_est = sync.associate_trajectories(traj_ref_complete, traj_est) traj_est = trajectory.align_trajectory( traj_est, traj_ref, correct_scale=False, discard_n_start_poses=int(discard_n_start_poses), discard_n_end_poses=int(discard_n_end_poses), ) print("traj_ref: ", traj_ref) print("traj_est: ", traj_est) # %% # plot ground truth trajectory with pose plot_mode = plot.PlotMode.xy fig = plt.figure()
def run_analysis(self, traj_ref_path, traj_vio_path, traj_pgo_path, segments, dataset_name="", discard_n_start_poses=0, discard_n_end_poses=0): """ Analyze data from a set of trajectory csv files. Args: traj_ref_path: string representing filepath of the reference (ground-truth) trajectory. traj_vio_path: string representing filepath of the vio estimated trajectory. traj_pgo_path: string representing filepath of the pgo estimated trajectory. segments: list of segments for RPE calculation, defined in the experiments yaml file. dataset_name: string representing the dataset's name discard_n_start_poses: int representing number of poses to discard from start of analysis. discard_n_end_poses: int representing the number of poses to discard from end of analysis. """ import copy # Mind that traj_est_pgo might be None traj_ref, traj_est_vio, traj_est_pgo = self.read_traj_files( traj_ref_path, traj_vio_path, traj_pgo_path) # We copy to distinguish from the pgo version that may be created traj_ref_vio = copy.deepcopy(traj_ref) # Register and align trajectories: evt.print_purple("Registering and aligning trajectories") traj_ref_vio, traj_est_vio = sync.associate_trajectories( traj_ref_vio, traj_est_vio) traj_est_vio = trajectory.align_trajectory( traj_est_vio, traj_ref_vio, correct_scale=False, discard_n_start_poses=int(discard_n_start_poses), discard_n_end_poses=int(discard_n_end_poses)) # We do the same for the PGO trajectory if needed: traj_ref_pgo = None if traj_est_pgo is not None: traj_ref_pgo = copy.deepcopy(traj_ref) traj_ref_pgo, traj_est_pgo = sync.associate_trajectories( traj_ref_pgo, traj_est_pgo) traj_est_pgo = trajectory.align_trajectory( traj_est_pgo, traj_ref_pgo, correct_scale=False, discard_n_start_poses=int(discard_n_start_poses), discard_n_end_poses=int(discard_n_end_poses)) # We need to pick the lowest num_poses before doing any computation: num_of_poses = traj_est_vio.num_poses if traj_est_pgo is not None: num_of_poses = min(num_of_poses, traj_est_pgo.num_poses) traj_est_pgo.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) traj_ref_pgo.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) traj_est_vio.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) traj_ref_vio.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) # Calculate all metrics: (ape_metric_vio, rpe_metric_trans_vio, rpe_metric_rot_vio, results_vio) = self.process_trajectory_data(traj_ref_vio, traj_est_vio, segments, True) # We do the same for the pgo trajectory if needed: ape_metric_pgo = None rpe_metric_trans_pgo = None rpe_metric_rot_pgo = None results_pgo = None if traj_est_pgo is not None: (ape_metric_pgo, rpe_metric_trans_pgo, rpe_metric_rot_pgo, results_pgo) = self.process_trajectory_data( traj_ref_pgo, traj_est_pgo, segments, False) # Generate plots for return: plot_collection = None if self.display_plots or self.save_plots: evt.print_green("Plotting:") log.info(dataset_name) plot_collection = plot.PlotCollection("Example") if traj_est_pgo is not None: # APE Metric Plot: plot_collection.add_figure( "PGO_APE_translation", plot_metric(ape_metric_pgo, "PGO + VIO APE Translation")) # Trajectory Colormapped with ATE Plot: plot_collection.add_figure( "PGO_APE_translation_trajectory_error", plot_traj_colormap_ape( ape_metric_pgo, traj_ref_pgo, traj_est_vio, traj_est_pgo, "PGO + VIO ATE Mapped Onto Trajectory")) # RPE Translation Metric Plot: plot_collection.add_figure( "PGO_RPE_translation", plot_metric(rpe_metric_trans_pgo, "PGO + VIO RPE Translation")) # Trajectory Colormapped with RTE Plot: plot_collection.add_figure( "PGO_RPE_translation_trajectory_error", plot_traj_colormap_rpe( rpe_metric_trans_pgo, traj_ref_pgo, traj_est_vio, traj_est_pgo, "PGO + VIO RPE Translation Error Mapped Onto Trajectory" )) # RPE Rotation Metric Plot: plot_collection.add_figure( "PGO_RPE_Rotation", plot_metric(rpe_metric_rot_pgo, "PGO + VIO RPE Rotation")) # Trajectory Colormapped with RTE Plot: plot_collection.add_figure( "PGO_RPE_rotation_trajectory_error", plot_traj_colormap_rpe( rpe_metric_rot_pgo, traj_ref_pgo, traj_est_vio, traj_est_pgo, "PGO + VIO RPE Rotation Error Mapped Onto Trajectory")) # Plot VIO results plot_collection.add_figure( "VIO_APE_translation", plot_metric(ape_metric_vio, "VIO APE Translation")) plot_collection.add_figure( "VIO_APE_translation_trajectory_error", plot_traj_colormap_ape(ape_metric_vio, traj_ref_vio, traj_est_vio, None, "VIO ATE Mapped Onto Trajectory")) plot_collection.add_figure( "VIO_RPE_translation", plot_metric(rpe_metric_trans_vio, "VIO RPE Translation")) plot_collection.add_figure( "VIO_RPE_translation_trajectory_error", plot_traj_colormap_rpe( rpe_metric_trans_vio, traj_ref_vio, traj_est_vio, None, "VIO RPE Translation Error Mapped Onto Trajectory")) plot_collection.add_figure( "VIO_RPE_Rotation", plot_metric(rpe_metric_rot_vio, "VIO RPE Rotation")) plot_collection.add_figure( "VIO_RPE_rotation_trajectory_error", plot_traj_colormap_rpe( rpe_metric_rot_vio, traj_ref_vio, traj_est_vio, None, "VIO RPE Rotation Error Mapped Onto Trajectory")) return [plot_collection, results_vio, results_pgo]
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 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) 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) 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.sync or args.align or args.correct_scale: 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) for name, traj in trajectories.items(): print_traj_info(name, traj, args.verbose, args.full_check) if args.ref: print_traj_info(args.ref, ref_traj, args.verbose, args.full_check) if args.plot or args.save_plot or args.serialize_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] 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)) ax_traj = plot.prepare_axis(fig_traj, plot_mode) pltstart = 0 pltend = traj.num_poses if args.plotstart: pltstart = args.plotstart if args.plotend != -1: pltend = args.plotend 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, start=pltstart, end=pltend) 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, start=pltstart, end=pltend) 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, start=pltstart, end=pltend) 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)))) fig_3 = plt.figure(figsize=tuple(SETTINGS.plot_figsize)) #plot_mode = plot.PlotMode.xz ax = plot.prepare_axis(fig_3, plot_mode) fig_3.axes.append(ax) for name, traj in trajectories.items(): num = traj.positions_xyz.shape[0] if pltstart >= num: print(name, "plotstart > len!", num) pltstart = 0 if pltend != -1 and (pltend > num or pltend < pltstart): print(name, "plotend > len!", num) pltend = traj.num_poses pltstart = int(pltstart) pltend = int(pltend) if cmap_colors is None: color = next(ax_traj._get_lines.prop_cycler)['color'] else: color = next(cmap_colors) short_traj_name = os.path.splitext(os.path.basename(name))[0] if SETTINGS.plot_usetex: short_traj_name = short_traj_name.replace("_", "\\_") plot.traj(ax_traj, plot_mode, traj, '-', color, short_traj_name, start=pltstart, end=pltend) 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, '-', color, short_traj_name, start_timestamp=start_time, start=pltstart, end=pltend) plot.traj_rpy(axarr_rpy, traj, '-', color, short_traj_name, start_timestamp=start_time, start=pltstart, end=pltend) speeds = [ trajectory.calc_speed(traj.positions_xyz[i], traj.positions_xyz[i + 1], traj.timestamps[i], traj.timestamps[i + 1]) for i in range(pltstart, pltend - 1) ] speeds.append(0) #plot.traj(ax, plot_mode, traj, '--', 'gray', 'reference') plot.traj_colormap(ax, traj, speeds, plot_mode, min_map=min(speeds), max_map=max(max(speeds), 10), title="speed mapped onto trajectory", start=pltstart, end=pltend) plot_collection.add_figure("trajectories", fig_traj) plot_collection.add_figure("xyz_view", fig_xyz) plot_collection.add_figure("rpy_view", fig_rpy) plot_collection.add_figure("traj (speed)", fig_3) 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()