def load_trajectories(args): import os from collections import OrderedDict from evo.tools import file_interface trajectories = OrderedDict() ref_traj = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file == args.ref: continue trajectories[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: continue trajectories[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: continue else: trajectories[ 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": if not (args.topics or args.all_topics): die("No topics used - specify topics or set --all_topics.") if not os.path.exists(args.bag): raise file_interface.FileInterfaceException( "File doesn't exist: {}".format(args.bag)) import rosbag bag = rosbag.Bag(args.bag) try: if args.all_topics: topics = file_interface.get_supported_topics(bag) if args.ref in topics: topics.remove(args.ref) if len(topics) == 0: die("No topics of supported types: {}".format(" ".join( file_interface.SUPPORTED_ROS_MSGS))) else: topics = args.topics for topic in topics: if topic == args.ref: continue trajectories[topic] = file_interface.read_bag_trajectory( bag, topic) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() return trajectories, ref_traj
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": 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 os logger.debug("Opening bag file " + args.bag) if not os.path.exists(args.bag): raise file_interface.FileInterfaceException( "File doesn't exist: {}".format(args.bag)) import rosbag 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)) return traj_ref, traj_est, ref_name, est_name
def load_trajectories(args): from evo.tools import file_interface trajectories = {} ref_traj = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file == args.ref: continue trajectories[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: continue trajectories[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: continue else: trajectories[ 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": if not (args.topics or args.all_topics): die("No topics used - specify topics or set --all_topics.") 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] in { "geometry_msgs/PoseStamped", "geometry_msgs/PoseWithCovarianceStamped", "nav_msgs/Odometry" } and t != args.ref ]) if len(topics) == 0: die("No geometry_msgs/PoseStamped, " "geometry_msgs/PoseWithCovarianceStamped or " "nav_msgs/Odometry topics found!") else: topics = args.topics for topic in topics: trajectories[topic] = file_interface.read_bag_trajectory( bag, topic) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() return trajectories, ref_traj
def load_trajectories(args): from evo.tools import file_interface trajectories = {} ref_traj = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file == args.ref: continue trajectories[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: continue trajectories[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: continue else: trajectories[ 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": if not (args.topics or args.all_topics): die("No topics used - specify topics or set --all_topics.") 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] in file_interface.SUPPORTED_ROS_MSGS and t != args.ref ]) if len(topics) == 0: die("No topics of supported types: {}".format(" ".join( file_interface.SUPPORTED_ROS_MSGS))) else: topics = args.topics for topic in topics: trajectories[topic] = file_interface.read_bag_trajectory( bag, topic) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() return trajectories, ref_traj
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 print(ref_p_path) 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 print(est_p_path) est_traj = path2trajectory(est_pose, est_time) #PoseTrajectory3D
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 test_write_read_integrity(self): traj_out = helpers.fake_path(1000) self.assertTrue(traj_out.check()) file_interface.write_kitti_poses_file(self.mock_file, traj_out) self.mock_file.seek(0) traj_in = file_interface.read_kitti_poses_file(self.mock_file) self.assertIsInstance(traj_in, PosePath3D) self.assertTrue(traj_in.check()) self.assertTrue(traj_out == traj_in)
def load_trajectories( args: argparse.Namespace ) -> typing.Tuple[PosePath3D, PosePath3D, str, str]: from evo.tools import file_interface traj_ref: typing.Union[PosePath3D, PoseTrajectory3D] traj_est: typing.Union[PosePath3D, PoseTrajectory3D] 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": 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 in ("bag", "bag2"): import os logger.debug("Opening bag file " + args.bag) if not os.path.exists(args.bag): raise file_interface.FileInterfaceException( "File doesn't exist: {}".format(args.bag)) if args.subcommand == "bag2": from rosbags.rosbag2 import Reader as Rosbag2Reader bag = Rosbag2Reader(args.bag) # type: ignore else: from rosbags.rosbag1 import Reader as Rosbag1Reader bag = Rosbag1Reader(args.bag) # type: ignore try: bag.open() 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)) return traj_ref, traj_est, ref_name, est_name
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 kitti_poses_and_timestamps_to_trajectory(poses_file, timestamp_file): pose_path = file_interface.read_kitti_poses_file(poses_file) raw_timestamps_mat = file_interface.csv_read_matrix(timestamp_file) error_msg = ("timestamp file must have one column of timestamps and same number of rows as the KITTI poses file") if len(raw_timestamps_mat) > 0 and len(raw_timestamps_mat[0]) != 1 or len(raw_timestamps_mat) != pose_path.num_poses: raise file_interface.FileInterfaceException(error_msg) try: timestamps_mat = np.array(raw_timestamps_mat).astype(float) except ValueError: raise file_interface.FileInterfaceException(error_msg) return PoseTrajectory3D(poses_se3=pose_path.poses_se3, timestamps=timestamps_mat)
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 load_trajectories(args): from collections import OrderedDict from evo.tools import file_interface trajectories = OrderedDict() ref_traj = None if args.subcommand == "tum": for traj_file in args.traj_files: if traj_file == args.ref: continue trajectories[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: continue trajectories[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: continue else: trajectories[ 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 in ("bag", "bag2"): if not (args.topics or args.all_topics): die("No topics used - specify topics or set --all_topics.") if not os.path.exists(args.bag): raise file_interface.FileInterfaceException( "File doesn't exist: {}".format(args.bag)) logger.debug("Opening bag file " + args.bag) if args.subcommand == "bag2": from rosbags.rosbag2 import Reader as Rosbag2Reader bag = Rosbag2Reader(args.bag) else: from rosbags.rosbag1 import Reader as Rosbag1Reader bag = Rosbag1Reader(args.bag) bag.open() try: if args.all_topics: # Note: args.topics can have TF stuff here, so we add it too. topics = args.topics topics += natsorted(file_interface.get_supported_topics(bag)) if args.ref in topics: topics.remove(args.ref) if len(topics) == 0: die("No topics of supported types: {}".format(" ".join( file_interface.SUPPORTED_ROS_MSGS))) else: topics = args.topics for topic in topics: if topic == args.ref: continue trajectories[topic] = file_interface.read_bag_trajectory( bag, topic) if args.ref: ref_traj = file_interface.read_bag_trajectory(bag, args.ref) finally: bag.close() return trajectories, ref_traj
def main(): hvd.init() args = parser.parse_args() if hvd.size() > 1: printcolor('----------- DISTRIBUTED DATA PARALLEL -----------', 'cyan') device_id = hvd.local_rank() torch.cuda.set_device(device_id) if hvd.rank() == 0: os.makedirs(args.output_dir, exist_ok=True) depth_net = DispResnet(version='18_pt') depth_net.load_state_dict(torch.load(args.depth_model, map_location='cpu')) depth_net = depth_net.cuda() # move to GPU keypoint_net = KeypointResnet() keypoint_net.load_state_dict( torch.load(args.keypoint_model, map_location='cpu')) keypoint_net.cuda() def _set_seeds(seed=42): """Set Python random seeding and PyTorch seeds. Parameters ---------- seed: int, default: 42 Random number generator seeds for PyTorch and python """ np.random.seed(seed) random.seed(seed) torch.manual_seed(seed) torch.cuda.manual_seed_all(seed) def _worker_init_fn(worker_id): """Worker init fn to fix the seed of the workers""" _set_seeds(42 + worker_id) if args.run_evaluation: all_trel = [] all_rrel = [] ########## STACK PREDICTED POSES INTO FULL TRAJECTORY ########## printcolor('Evaluation sequences {}'.format(args.sequence)) for seq in args.sequence: dataset = KITTIODOMETRYDataset( args.dataset_dir, [seq], back_context=0, forward_context=1, image_shape=(args.input_height, args.input_width), ) trajectory_length = sum(len(imgs) for imgs in dataset.img_files) printcolor('Evaluating SEQ {}'.format(seq)) sampler = None if not (hvd.size() > 1) else \ torch.utils.data.distributed.DistributedSampler( dataset, num_replicas=hvd.size(), rank=hvd.rank()) dl = DataLoader(dataset, batch_size=1, pin_memory=False, shuffle=False, num_workers=48, worker_init_fn=_worker_init_fn, sampler=sampler) all_poses = accumulate_pose_single_frame( dataloader=dl, depth_net=depth_net, keypoint_net=keypoint_net, trajectory_length=trajectory_length, rank=hvd.rank()) if hvd.rank() == 0: np.savetxt(os.path.join(args.output_dir, str(seq) + '_ori.txt'), all_poses.reshape(trajectory_length, -1)[:, :12]) if args.align_trajectory: if hvd.rank() == 0: printcolor('Computing scale alignment.') # Load ground truth poses ground_truth_trajectory = file_interface.read_kitti_poses_file( os.path.join(os.path.join(args.dataset_dir, 'poses'), seq) + '.txt') # Convert predicted trajectory predicted_trajectory = PosePath3D(poses_se3=all_poses) # Umeyama alignment with scaling only predicted_trajectory_aligned = copy.deepcopy( predicted_trajectory) predicted_trajectory_aligned.align(ground_truth_trajectory, correct_only_scale=True) # Save aligned trajectory file_interface.write_kitti_poses_file( os.path.join(args.output_dir, seq) + '.txt', predicted_trajectory_aligned) if args.run_evaluation and hvd.rank() == 0: pose_test_executable = "./kp3d/externals/cpp/evaluate_odometry" p = subprocess.Popen( [pose_test_executable, args.dataset_dir, args.output_dir, seq], stdout=subprocess.PIPE, stderr=subprocess.PIPE) p.wait() seq_output = os.path.join(args.output_dir, seq + '_stats.txt') with open(seq_output, 'r') as f: pose_result = f.readlines()[0].split(' ') pose_result = [float(p) for p in pose_result] traj_pos_error = pose_result[0] traj_rot_error = pose_result[1] all_trel.append(traj_pos_error) all_rrel.append(traj_rot_error) printcolor( 'SEQ {} -- Mean TRAJ (pos: {:.4f}, rot: {:.4f})'.format( seq, traj_pos_error, traj_rot_error), 'green') printcolor(all_trel) printcolor(all_rrel)
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()
def run(args): import os import sys import logging import evo.algorithms.lie_algebra as lie from evo.algorithms import trajectory from evo.tools import file_interface, settings from evo.tools.settings import SETTINGS settings.configure_logging(verbose=True, silent=args.silent, debug=args.debug) if args.debug: import pprint logging.debug( "main_parser config:\n" + pprint.pformat({arg: getattr(args, arg) for arg in vars(args)}) + "\n") logging.debug(SEP) 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: logging.error("no geometry_msgs/PoseStamped topics found!") sys.exit(1) else: topics = args.topics if not topics: logging.warning( "no topics used - specify topics or use the --all_topics flag" ) 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.transform_left or args.transform_right: tf_path = args.transform_left if args.transform_left else args.transform_right t, xyz, quat = file_interface.load_transform_json(tf_path) logging.debug(SEP) logging.debug("applying transformation to the trajectories:\n" + str(t)) if args.invert_transform: t = lie.se3_inverse(t) for name, traj in trajectories: traj.transform(t, right_mul=args.transform_right) if args.align or args.correct_scale: if args.ref: 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.algorithms import sync for name, traj in trajectories: logging.debug(SEP) ref_assoc, traj_assoc = sync.associate_trajectories( ref_traj, traj, 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): logging.debug(SEP) logging.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: if args.t_offset and traj.timestamps.shape[0] != 0: logging.debug(SEP) logging.info("adding time offset to " + name + ": " + str(args.t_offset) + " (s)") traj.timestamps += args.t_offset print_traj_info(name, traj, args.full_check) if (args.align or args.correct_scale) and not args.ref: logging.debug(SEP) logging.warning("can't align without a reference! (--ref) *grunt*") if args.ref: print_traj_info(args.ref, ref_traj, 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_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) 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) plt.tight_layout() plot_collection.add_figure("trajectories", fig_traj) plot_collection.add_figure("xyz_view", fig_xyz) if args.plot: plot_collection.show() if args.save_plot: logging.debug(SEP) plot_collection.export(args.save_plot, confirm_overwrite=not args.no_warnings) if args.serialize_plot: logging.debug(SEP) plot_collection.serialize(args.serialize_plot, confirm_overwrite=not args.no_warnings) if args.save_as_tum: logging.debug(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: logging.debug(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: logging.debug(SEP) import datetime import rosbag dest_bag_path = str( datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S.%f')) + ".bag" logging.debug("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] file_interface.write_bag_trajectory(bag, traj, dest_topic) if args.ref: dest_topic = os.path.splitext(os.path.basename(args.ref))[0] file_interface.write_bag_trajectory(bag, ref_traj, dest_topic) finally: bag.close()
def test_too_few_columns_with_trailing_delim(self): self.mock_file.write(u"1 2 3 4 5 6 7 8 9 10 11 ") self.mock_file.seek(0) with self.assertRaises(file_interface.FileInterfaceException): file_interface.read_kitti_poses_file(self.mock_file)
def run(args): import sys from evo.algorithms import metrics from evo.tools import file_interface, settings # manually check bins and tols arguments to allow them to be in config files if not args.bins or not args.tols: logging.error( "the following arguments are required: -b/--bins, -t/--tols") sys.exit(1) settings.configure_logging(args.verbose, args.silent, args.debug) if args.debug: import pprint logging.debug( "main_parser config:\n" + pprint.pformat({arg: getattr(args, arg) for arg in vars(args)}) + "\n") logging.debug(SEP) pose_relation = None if args.pose_relation == "trans_part": pose_relation = metrics.PoseRelation.translation_part elif args.pose_relation == "angle_deg": pose_relation = metrics.PoseRelation.rotation_angle_deg elif args.pose_relation == "angle_rad": pose_relation = metrics.PoseRelation.rotation_angle_rad traj_ref, traj_est, stamps_est = None, None, None ref_name, est_name = "", "" if args.subcommand == "tum": traj_ref, traj_est = file_interface.load_assoc_tum_trajectories( args.ref_file, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.ref_file, args.est_file elif args.subcommand == "kitti": traj_ref = file_interface.read_kitti_poses_file(args.ref_file) traj_est = file_interface.read_kitti_poses_file(args.est_file) ref_name, est_name = args.ref_file, args.est_file elif args.subcommand == "euroc": args.align = True logging.info( "forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)" ) logging.debug(SEP) traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories( args.state_gt_csv, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.state_gt_csv, args.est_file elif args.subcommand == "bag": import rosbag logging.debug("opening bag file " + args.bag) bag = rosbag.Bag(args.bag, 'r') try: traj_ref, traj_est = file_interface.load_assoc_bag_trajectories( bag, args.ref_topic, args.est_topic, args.t_max_diff, args.t_offset, ) finally: bag.close() ref_name, est_name = args.ref_topic, args.est_topic main_rpe_for_each(traj_ref, traj_est, pose_relation, args.mode, args.bins, args.tols, args.align, args.correct_scale, ref_name, est_name, args.plot, args.save_plot, args.save_results, args.no_warnings, serialize_plot=args.serialize_plot)
def test_trailing_delim(self): self.mock_file.write(u"1 0 0 0.1 0 1 0 0.2 0 0 1 0.3 ") self.mock_file.seek(0) with self.assertRaises(file_interface.FileInterfaceException): file_interface.read_kitti_poses_file(self.mock_file)
import copy import numpy as np import context from evo.core import trajectory, geometry from evo.core import lie_algebra as lie from evo.tools import file_interface # use absolute paths! here = os.path.dirname(os.path.abspath(__file__)) tum_traj_file = os.path.join(here, "data/fr2_desk_ORB.txt") kitti_traj_file = os.path.join(here, "data/KITTI_00_gt.txt") ex_tum_traj = file_interface.read_tum_trajectory_file(tum_traj_file) ex_kitti_traj = file_interface.read_kitti_poses_file(kitti_traj_file) ex_kitti_traj_wrong = copy.deepcopy(ex_kitti_traj) ex_kitti_traj_wrong._poses_se3 = [ np.zeros((4, 4)) for i in range(ex_kitti_traj.num_poses) ] ex_tum_traj_wrong_quat = copy.deepcopy(ex_tum_traj) ex_tum_traj_wrong_quat._orientations_quat_wxyz[3] = [5000, 0, 0, 0] ex_tum_traj_wrong_stamps = copy.deepcopy(ex_tum_traj) ex_tum_traj_wrong_stamps.timestamps = [1, 2, 3] class TestPosePath3D(unittest.TestCase): def test_init_wrong_args(self): # no args
def run(args): from evo.algorithms import metrics from evo.tools import file_interface, settings settings.configure_logging(args.verbose, args.silent, args.debug) if args.debug: import pprint logging.debug( "main_parser config:\n" + pprint.pformat({arg: getattr(args, arg) for arg in vars(args)}) + "\n") logging.debug(SEP) pose_relation = None if args.pose_relation == "full": pose_relation = metrics.PoseRelation.full_transformation elif args.pose_relation == "rot_part": pose_relation = metrics.PoseRelation.rotation_part elif args.pose_relation == "trans_part": pose_relation = metrics.PoseRelation.translation_part elif args.pose_relation == "angle_deg": pose_relation = metrics.PoseRelation.rotation_angle_deg elif args.pose_relation == "angle_rad": pose_relation = metrics.PoseRelation.rotation_angle_rad traj_ref, traj_est, stamps_est = None, None, None ref_name, est_name = "", "" plot_mode = None # no plot imports unless really needed (slow) if args.subcommand == "tum": traj_ref, traj_est = file_interface.load_assoc_tum_trajectories( args.ref_file, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.ref_file, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "kitti": traj_ref = file_interface.read_kitti_poses_file(args.ref_file) traj_est = file_interface.read_kitti_poses_file(args.est_file) ref_name, est_name = args.ref_file, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "euroc": args.align = True logging.info( "forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)" ) logging.debug(SEP) traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories( args.state_gt_csv, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.state_gt_csv, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "bag": import rosbag logging.debug("opening bag file " + args.bag) bag = rosbag.Bag(args.bag, 'r') try: traj_ref, traj_est = file_interface.load_assoc_bag_trajectories( bag, args.ref_topic, args.est_topic, args.t_max_diff, args.t_offset, ) finally: bag.close() ref_name, est_name = args.ref_topic, args.est_topic if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xy if not args.plot_mode else PlotMode[ args.plot_mode] main_ape(traj_ref, traj_est, pose_relation, args.align, args.correct_scale, ref_name, est_name, args.plot, args.save_plot, plot_mode, args.save_results, args.no_warnings, serialize_plot=args.serialize_plot)
import logging import sys from evo.core import trajectory from evo.tools import plot, file_interface import evo.core.lie_algebra as lie import numpy as np import matplotlib.pyplot as plt logging.basicConfig(format="%(message)s", stream=sys.stdout, level=logging.DEBUG) traj_ref = file_interface.read_kitti_poses_file("../data/KITTI_00_gt.txt") traj_est = file_interface.read_kitti_poses_file("../data/KITTI_00_ORB.txt") # add artificial Sim(3) transformation traj_est.transform(lie.se3(np.eye(3), [0, 0, 0])) traj_est.scale(0.5) logging.info("\nUmeyama alignment without scaling") traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref) logging.info("\nUmeyama alignment with scaling") traj_est_aligned_scaled = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=True) logging.info("\nUmeyama alignment with scaling only") traj_est_aligned_only_scaled = trajectory.align_trajectory( traj_est, traj_ref, correct_only_scale=True)
along with evo. If not, see <http://www.gnu.org/licenses/>. """ from __future__ import print_function # Python 2.7 backwards compatibility import unittest import copy import numpy as np from evo.core import trajectory, geometry from evo.core import lie_algebra as lie from evo.tools import file_interface ex_tum_traj = file_interface.read_tum_trajectory_file("data/fr2_desk_ORB.txt") ex_kitti_traj = file_interface.read_kitti_poses_file("data/KITTI_00_gt.txt") ex_kitti_traj_wrong = copy.deepcopy(ex_kitti_traj) ex_kitti_traj_wrong._poses_se3 = [ np.zeros((4, 4)) for i in range(ex_kitti_traj.num_poses) ] ex_tum_traj_wrong_quat = copy.deepcopy(ex_tum_traj) ex_tum_traj_wrong_quat._orientations_quat_wxyz[3] = [5000, 0, 0, 0] ex_tum_traj_wrong_stamps = copy.deepcopy(ex_tum_traj) ex_tum_traj_wrong_stamps.timestamps = [1, 2, 3] class TestPosePath3D(unittest.TestCase): def test_init_wrong_args(self): # no args
import logging import sys import evo.core.lie_algebra as lie from evo.core import trajectory from evo.tools import plot, file_interface, log import numpy as np import matplotlib.pyplot as plt logger = logging.getLogger("evo") log.configure_logging(verbose=True) traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt") traj_est = file_interface.read_kitti_poses_file("../test/data/KITTI_00_ORB.txt") # add artificial Sim(3) transformation traj_est.transform(lie.se3(np.eye(3), [0, 0, 0])) traj_est.scale(0.5) logger.info("\nUmeyama alignment without scaling") traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref) logger.info("\nUmeyama alignment with scaling") traj_est_aligned_scaled = trajectory.align_trajectory( traj_est, traj_ref, correct_scale=True) logger.info("\nUmeyama alignment with scaling only") traj_est_aligned_only_scaled = trajectory.align_trajectory(
def test_too_many_columns(self): self.mock_file.write(u"1 2 3 4 5 6 7 8 9 10 11 12 13") self.mock_file.seek(0) with self.assertRaises(file_interface.FileInterfaceException): file_interface.read_kitti_poses_file(self.mock_file)
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 run(args): from evo.core import metrics from evo.tools import file_interface, log log.configure_logging(args.verbose, args.silent, args.debug) if args.debug: import pprint logger.debug( "main_parser config:\n" + pprint.pformat({arg: getattr(args, arg) for arg in vars(args)}) + "\n") logger.debug(SEP) pose_relation = None if args.pose_relation == "full": pose_relation = metrics.PoseRelation.full_transformation elif args.pose_relation == "rot_part": pose_relation = metrics.PoseRelation.rotation_part elif args.pose_relation == "trans_part": pose_relation = metrics.PoseRelation.translation_part elif args.pose_relation == "angle_deg": pose_relation = metrics.PoseRelation.rotation_angle_deg elif args.pose_relation == "angle_rad": pose_relation = metrics.PoseRelation.rotation_angle_rad delta_unit = None if args.delta_unit == "f": delta_unit = metrics.Unit.frames elif args.delta_unit == "d": delta_unit = metrics.Unit.degrees elif args.delta_unit == "r": delta_unit = metrics.Unit.radians elif args.delta_unit == "m": delta_unit = metrics.Unit.meters traj_ref, traj_est, stamps_est = None, None, None ref_name, est_name = "", "" plot_mode = None # no plot imports unless really needed (slow) if args.subcommand == "tum": traj_ref, traj_est = file_interface.load_assoc_tum_trajectories( args.ref_file, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.ref_file, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "kitti": traj_ref = file_interface.read_kitti_poses_file(args.ref_file) traj_est = file_interface.read_kitti_poses_file(args.est_file) ref_name, est_name = args.ref_file, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "euroc": args.align = True logger.info( "Forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)" ) logger.debug(SEP) traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories( args.state_gt_csv, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.state_gt_csv, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "bag": import rosbag logger.debug("Opening bag file {} ...".format(args.bag)) bag = rosbag.Bag(args.bag, 'r') try: traj_ref, traj_est = file_interface.load_assoc_bag_trajectories( bag, args.ref_topic, args.est_topic, args.t_max_diff, args.t_offset, ) finally: bag.close() ref_name, est_name = args.ref_topic, args.est_topic if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xy if not args.plot_mode else PlotMode[ args.plot_mode] main_rpe( traj_ref=traj_ref, traj_est=traj_est, pose_relation=pose_relation, delta=args.delta, delta_unit=delta_unit, rel_delta_tol=args.delta_tol, all_pairs=args.all_pairs, align=args.align, correct_scale=args.correct_scale, ref_name=ref_name, est_name=est_name, show_plot=args.plot, save_plot=args.save_plot, plot_mode=plot_mode, save_results=args.save_results, no_warnings=args.no_warnings, serialize_plot=args.serialize_plot, plot_colormap_max=args.plot_colormap_max, plot_colormap_min=args.plot_colormap_min, plot_colormap_max_percentile=args.plot_colormap_max_percentile, )