示例#1
0
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
示例#2
0
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
示例#3
0
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
示例#4
0
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
示例#5
0
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
示例#6
0
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
示例#7
0
 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)
示例#8
0
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
示例#9
0
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
示例#10
0
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)
示例#11
0
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
示例#12
0
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)
示例#14
0
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()
示例#15
0
文件: main_traj.py 项目: skylook/evo
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()
示例#16
0
 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)
示例#17
0
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)
示例#18
0
 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
示例#20
0
文件: main_ape.py 项目: skylook/evo
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)
示例#22
0
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
示例#23
0
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(
示例#24
0
 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)
示例#25
0
def get_and_save_results_from_folder(folder_with_predicted_poses,category):
    
    global args
    global kitti_eval_tool
    global folder_with_gt_poses
    global output_folder
    global t
    global results
    
    values_for_excel = []
    columns_for_excel = []
    type_of_statistics = 'mean'
    for filename in sorted(os.listdir(folder_with_predicted_poses)):
        if not(os.path.exists(os.path.join(folder_with_gt_poses, filename))):
            print("file with gt poses doesn't exist for "+filename)
            continue
        if filename.find('.txt') == -1:
            continue
        seq_results = {}
        seq_results['name_seq'] = filename[:filename.rfind('.')]
        seq_results['category'] = category
        folder_name = seq_results['category']
        seq_results['metrics'] = {}
        seq_results['lost'] = False
        
        os.makedirs(os.path.join(output_folder, folder_name), exist_ok=True)
        output_folder_seq = os.path.join(output_folder, folder_name, filename[:filename.rfind('.')])
        os.makedirs(output_folder_seq, exist_ok=True)
        if os.path.isfile(os.path.join(output_folder, folder_name,"results.txt")):
            file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "a")
        else:
            file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "w")
            file_results_txt.write("translation_error(%) rotation_error(deg/m) ATE(m) APE_translation_error_median(m) APE_rotation_error_median(deg) dst_to_trgt\n")
        
        # -------------------------------------Getting results---------------------------------------------------
        if args.gt_format == 'kitti':        
            traj_ref = file_interface.read_kitti_poses_file(os.path.join(folder_with_gt_poses, filename))
        if args.gt_format == 'tum':        
            traj_ref = file_interface.read_tum_trajectory_file(os.path.join(folder_with_gt_poses, filename))
            seq_results["length_of_ref_traj"] = traj_ref.path_length
            end_time_gt = traj_ref.get_infos()["t_end (s)"]
        if args.gt_format == 'euroc':        
            traj_ref = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_gt_poses, filename))
        if args.result_format == 'kitti':
            traj_est = file_interface.read_kitti_poses_file(os.path.join(folder_with_predicted_poses, filename))
        if args.result_format == 'tum':
            traj_est = file_interface.read_tum_trajectory_file(os.path.join(folder_with_predicted_poses, filename))
            seq_results["length_of_estimated_traj"] = traj_est.path_length
        if args.result_format == 'euroc':
            traj_est = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_predicted_poses, filename))
        if args.result_format == 'tum' and args.gt_format == 'tum':
            seq_results["num_gt_poses"] = traj_ref.num_poses
            seq_results["num_predicted_poses"] = traj_est.num_poses
            traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, args.max_diff)
            end_time_est = traj_est.get_infos()["t_end (s)"]
            if (abs(end_time_est - end_time_gt) > 0.2) or (traj_est.get_infos()["t_start (s)"] > 0.2):
                print('LOST in track '+filename[:filename.rfind('.')])
                seq_results['lost'] = True
                results.append(seq_results)
                t.update(1)
                continue
        if args.alignment != None:
            traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=args.alignment.find("scale") != -1, correct_only_scale=args.alignment=="scale")
        trajectory.align_trajectory_origin(traj_est, traj_ref)
        data = (traj_ref, traj_est)
        
        ape_metric_translation = metrics.APE(metrics.PoseRelation.translation_part)
        ape_metric_rotation = metrics.APE(metrics.PoseRelation.rotation_angle_deg)
        ape_metric_translation.process_data(data)
        ape_metric_rotation.process_data(data)
        ape_translation_statistics = ape_metric_translation.get_all_statistics()
        ape_rotation_statistics = ape_metric_rotation.get_all_statistics()
        
        ape_translation_statistics_plot = copy.deepcopy(ape_translation_statistics)
        ape_rotation_statistics_plot = copy.deepcopy(ape_rotation_statistics)
        ape_translation_statistics_plot.pop('sse')
        ape_translation_statistics_plot.pop('std')
        ape_translation_statistics_plot.pop('min')
        ape_translation_statistics_plot.pop('max')
        ape_rotation_statistics_plot.pop('sse')
        ape_rotation_statistics_plot.pop('std')
        ape_rotation_statistics_plot.pop('min')
        ape_rotation_statistics_plot.pop('max')
        
        kitti_trans_err, kitti_rot_err, ate = kitti_eval_tool.eval(traj_ref.poses_se3, 
                                                               traj_est.poses_se3, 
                                                               alignment=None)
    
        #---------------------------------adding results to variable seq_results for excel -----------------------------
        seq_results['metrics']['dist_to_trgt'] = traj_est.get_infos()['pos_end (m)'] - traj_ref.get_infos()['pos_end (m)']
        seq_results['metrics']['dist_to_trgt'] = np.sum(np.array(seq_results['metrics']['dist_to_trgt'])**2)**0.5
        seq_results['metrics']["Kitti trans err (%)"] = kitti_trans_err
        seq_results['metrics']["Kitti rot err (deg/m)"] = kitti_rot_err
        seq_results['metrics']["ATE (m)"] = ate
        seq_results['metrics']["APE(trans err) median (m)"] = ape_translation_statistics["median"]
        seq_results['metrics']["APE(rot err) median (deg)"] = ape_rotation_statistics["median"]
        #--------------------------------------------------------------------------------------------------------
        
        
        #-------------------------------------------------------------------------------------------------------    
    
        # --------------------------------printing results into console----------------------------------------------
        print('Results for "'+filename+'":')
        print('Kitti average translational error (%): {:.7f}'.format(kitti_trans_err))
        print('Kitti average rotational error (deg/m): {:.7f}'.format(kitti_rot_err))
        print('ATE (m): {:.7f}'.format(ate))
        print('APE(translation error) median (m): {:.7f}'.format(ape_translation_statistics["median"]))
        print('APE(rotation error) median (deg): {:.7f}'.format(ape_rotation_statistics["median"]))
        print('distance to target on the last frame: {:.7f}'.format(seq_results['metrics']['dist_to_trgt']))
        #------------------------------------------------------------------------------------------------------------
        
        #---------------------------------Saving results into overall results text file------------------------------
        file_results_txt.write('{:<24} '.format(filename[:filename.rfind('.')]))
        file_results_txt.write('{:>7.4f} '.format(kitti_trans_err))
        file_results_txt.write('{:>7.4f} '.format(kitti_rot_err))
        file_results_txt.write('{:>7.4f} '.format(ate))
        file_results_txt.write('{:>7.4f} '.format(ape_translation_statistics["median"]))
        file_results_txt.write('{:>7.4f} '.format(ape_rotation_statistics["median"]))
        file_results_txt.write('{:>7.4f}\n'.format(seq_results['metrics']['dist_to_trgt']))
        #------------------------------------------------------------------------------------------------------------
    
        # --------------------------------Saving metrics to text file for one track----------------------------------
        txt_filename = filename[:filename.rfind('.')]+"_metrics.txt"
        with open(os.path.join(output_folder_seq, txt_filename), "w") as txt_file:
            txt_file.write('Kitti average translational error (%): {:.7f}\n'.format(kitti_trans_err))
            txt_file.write('Kitti average rotational error (deg/m): {:.7f}\n'.format(kitti_rot_err))
            txt_file.write('ATE (m): {:.7f}\n'.format(ate))
            txt_file.write('APE(translation error) median (m): {:.7f}\n'.format(ape_translation_statistics["median"]))
            txt_file.write('APE(rotation error) median (deg): {:.7f}\n'.format(ape_rotation_statistics["median"]))
            txt_file.write('Distance to target on the last frame: {:.7f}\n'.format(seq_results['metrics']['dist_to_trgt']))
        #---------------------------------------------------------------------------------------------------------
    
        # ---------------------------------Saving values of errors for each frame to text file------------------------
        # ------------------------------------------for translation errors----------------------------------------
        txt_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.txt"
        output_folder_seq_translation = os.path.join(output_folder_seq,"translation")
        output_folder_seq_rotation = os.path.join(output_folder_seq,"rotation")
        os.makedirs(output_folder_seq_translation, exist_ok=True)
        os.makedirs(output_folder_seq_rotation, exist_ok=True)
        with open(os.path.join(output_folder_seq_translation, txt_filename), "w") as txt_file:
            for error in ape_metric_translation.error:
                txt_file.write('{:.10f}\n'.format(error))
        # -----------------------------------------for rotation degree errors--------------------------------------
        txt_filename = filename[:filename.rfind('.')]+"_APE(rotation_deg)_errors.txt"
        with open(os.path.join(output_folder_seq_rotation, txt_filename), "w") as txt_file:
            for error in ape_metric_rotation.error:
                txt_file.write('{:.10f}\n'.format(error))
        #----------------------------------------------------------------------------------------------------------
            
        # ---------------------------------------Saving plot of errors of each frame------------------------------
        # ------------------------------------------for translation errors----------------------------------------
        plot_collection = plot.PlotCollection("Example")
        fig_1 = plt.figure(figsize=(8, 8))
        plot.error_array(fig_1, ape_metric_translation.error, 
                         name="APE", title=str(ape_metric_translation), xlabel="Index of frame", ylabel='Error')
        plot_collection.add_figure("raw", fig_1)
        plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.png"
        plt.savefig(os.path.join(output_folder_seq_translation, plot_filename))
        plt.close(fig_1)
        # -----------------------------------------for rotation degree errors--------------------------------------
        plot_collection = plot.PlotCollection("Example")
        fig_1 = plt.figure(figsize=(8, 8))
        plot.error_array(fig_1, ape_metric_rotation.error, 
                         name="APE", title=str(ape_metric_rotation), xlabel="Index of frame", ylabel='Error')
        plot_collection.add_figure("raw", fig_1)
        plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_errors.png"
        plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename))
        plt.close(fig_1)
        #-----------------------------------------------------------------------------------------------------------
    
        # -----------------------------------------Saving trajectory plot------------------------------------------- 
        # ------------------------------------------for translation errors----------------------------------------
        fig_2 = plt.figure(figsize=(8, 8))
        ax = plot.prepare_axis(fig_2, plot_mode)
        plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
        plot.traj_colormap( ax, traj_est, ape_metric_translation.error, plot_mode, 
                           min_map=ape_translation_statistics["min"],
                           max_map=ape_translation_statistics["max"], title="APE translation mapped onto trajectory")
        plot_collection.add_figure("traj (error)", fig_2)
        plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_map.png"
        plt.savefig(os.path.join(output_folder_seq_translation,plot_filename))
        plt.close(fig_2)
        # -----------------------------------------for rotation degree errors--------------------------------------
        fig_2 = plt.figure(figsize=(8, 8))
        ax = plot.prepare_axis(fig_2, plot_mode)
        plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference')
        plot.traj_colormap( ax, traj_est, ape_metric_rotation.error, plot_mode, 
                           min_map=ape_rotation_statistics["min"],
                           max_map=ape_rotation_statistics["max"], title="APE rotation mapped onto trajectory")
        plot_collection.add_figure("traj (error)", fig_2)
        plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_map.png"
        plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename))
        plt.close(fig_2)
        #-----------------------------------------------------------------------------------------------------------
        print()
        
        active_worksheet = wb['sheet1']
        thin = Side(border_style="thin", color="000000")
        thick = Side(border_style="thick", color="000000")
        medium = Side(border_style="medium", color="000000")
        font_header = Font(name='Arial',
                       size=10,
                       bold=True,
                       italic=False,
                       vertAlign=None,
                       underline='none',
                       strike=False,
                       color='FF000000')
        font_values = Font(name='Arial',
                       size=10,
                       bold=False,
                       italic=False,
                       vertAlign=None,
                               underline='none',
                       strike=False,
                       color='FF000000')

        active_worksheet.row_dimensions[2].height = 35
        
        file_results_txt.close()
        results.append(seq_results)
        t.update(1)
def 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,
    )