How to use the evo.core.trajectory.PosePath3D function in evo

To help you get started, we’ve selected a few evo examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github MichaelGrupp / evo / test / test_trajectory.py View on Github external
def test_init_correct(self):
        # only poses_se3
        path = helpers.fake_path(10)
        try:
            trajectory.PosePath3D(poses_se3=path.poses_se3)
        except trajectory.TrajectoryException:
            self.fail("unexpected init failure with only poses_se3")
        # xyz + quaternion
        try:
            trajectory.PosePath3D(path.positions_xyz,
                                  path.orientations_quat_wxyz)
        except trajectory.TrajectoryException:
            self.fail("unexpected init failure with xyz + quaternion")
        # all
        try:
            trajectory.PosePath3D(path.positions_xyz,
                                  path.orientations_quat_wxyz, path.poses_se3)
        except trajectory.TrajectoryException:
            self.fail(
                "unexpected init failure with xyz + quaternion + poses_se3")
github MichaelGrupp / evo / test / test_trajectory.py View on Github external
def test_init_wrong_args(self):
        path = helpers.fake_path(10)
        # no args
        with self.assertRaises(trajectory.TrajectoryException):
            trajectory.PosePath3D()
        # only quaternion
        with self.assertRaises(trajectory.TrajectoryException):
            trajectory.PosePath3D(
                orientations_quat_wxyz=path.orientations_quat_wxyz)
        # only xyz
        with self.assertRaises(trajectory.TrajectoryException):
            trajectory.PosePath3D(positions_xyz=path.positions_xyz)
github MichaelGrupp / evo / test / test_trajectory.py View on Github external
def test_init_wrong_args(self):
        path = helpers.fake_path(10)
        # no args
        with self.assertRaises(trajectory.TrajectoryException):
            trajectory.PosePath3D()
        # only quaternion
        with self.assertRaises(trajectory.TrajectoryException):
            trajectory.PosePath3D(
                orientations_quat_wxyz=path.orientations_quat_wxyz)
        # only xyz
        with self.assertRaises(trajectory.TrajectoryException):
            trajectory.PosePath3D(positions_xyz=path.positions_xyz)
github MichaelGrupp / evo / test / helpers.py View on Github external
def fake_path(length):
    return PosePath3D(poses_se3=random_se3_list(length))
github MichaelGrupp / evo / test / test_trajectory.py View on Github external
def test_init_correct(self):
        # only poses_se3
        path = helpers.fake_path(10)
        try:
            trajectory.PosePath3D(poses_se3=path.poses_se3)
        except trajectory.TrajectoryException:
            self.fail("unexpected init failure with only poses_se3")
        # xyz + quaternion
        try:
            trajectory.PosePath3D(path.positions_xyz,
                                  path.orientations_quat_wxyz)
        except trajectory.TrajectoryException:
            self.fail("unexpected init failure with xyz + quaternion")
        # all
        try:
            trajectory.PosePath3D(path.positions_xyz,
                                  path.orientations_quat_wxyz, path.poses_se3)
        except trajectory.TrajectoryException:
            self.fail(
                "unexpected init failure with xyz + quaternion + poses_se3")
github MichaelGrupp / evo / evo / main_rpe_for_each.py View on Github external
def main_rpe_for_each(traj_ref, traj_est, pose_relation, mode, bins, rel_tols,
                      align=False, correct_scale=False, ref_name="", est_name="",
                      show_plot=False, save_plot=None, save_results=None, no_warnings=False,
                      serialize_plot=None):

    from evo.core import metrics, result
    from evo.core import filters
    from evo.core import trajectory
    from evo.tools import file_interface
    from evo.tools.settings import SETTINGS

    if not bins or not rel_tols:
        raise RuntimeError("bins and tolerances must have more than one element")
    if len(bins) != len(rel_tols):
        raise RuntimeError("bins and tolerances must have the same number of elements")
    if mode in {"speed", "angular_speed"} and traj_est is trajectory.PosePath3D:
        raise RuntimeError("timestamps are required for mode: " + mode)

    bin_unit = None
    if mode == "speed":
        bin_unit = metrics.VelUnit.meters_per_sec
    elif mode == "path":
        bin_unit = metrics.Unit.meters
    elif mode == "angle":
        bin_unit = metrics.Unit.degrees
    elif mode == "angular_speed":
        bin_unit = metrics.VelUnit.degrees_per_sec

    rpe_unit = None
    if pose_relation is metrics.PoseRelation.translation_part:
        rpe_unit = metrics.Unit.meters
    elif pose_relation is metrics.PoseRelation.rotation_angle_deg:
github MichaelGrupp / evo / evo / tools / file_interface.py View on Github external
"and no trailing delimiter at the end of the rows (space)")
    if len(raw_mat) > 0 and len(raw_mat[0]) != 12:
        raise FileInterfaceException(error_msg)
    try:
        mat = np.array(raw_mat).astype(float)
    except ValueError:
        raise FileInterfaceException(error_msg)
    # yapf: disable
    poses = [np.array([[r[0], r[1], r[2], r[3]],
                       [r[4], r[5], r[6], r[7]],
                       [r[8], r[9], r[10], r[11]],
                       [0, 0, 0, 1]]) for r in mat]
    # yapf: enable
    if not hasattr(file_path, 'read'):  # if not file handle
        logger.debug("Loaded {} poses from: {}".format(len(poses), file_path))
    return PosePath3D(poses_se3=poses)
github MichaelGrupp / evo / evo / tools / file_interface.py View on Github external
if isinstance(zip_path, str):
        logger.debug("Saving results to " + zip_path + "...")
    if confirm_overwrite and not user.check_and_confirm_overwrite(zip_path):
        return
    with zipfile.ZipFile(zip_path, 'w') as archive:
        archive.writestr("info.json", json.dumps(result_obj.info))
        archive.writestr("stats.json", json.dumps(result_obj.stats))
        for name, array in result_obj.np_arrays.items():
            buffer = io.BytesIO()
            np.save(buffer, array)
            buffer.seek(0)
            archive.writestr("{}.npz".format(name), buffer.read())
            buffer.close()
        for name, traj in result_obj.trajectories.items():
            buffer = io.StringIO()
            if type(traj) is PosePath3D:
                fmt_suffix = ".kitti"
                write_kitti_poses_file(buffer, traj)
            elif type(traj) is PoseTrajectory3D:
                fmt_suffix = ".tum"
                write_tum_trajectory_file(buffer, traj)
            else:
                raise FileInterfaceException(
                    "unknown format of trajectory {}".format(name))
            buffer.seek(0)
            archive.writestr("{}{}".format(name, fmt_suffix),
                             buffer.read().encode("utf-8"))
            buffer.close()
github MichaelGrupp / evo / evo / main_traj.py View on Github external
transform = file_interface.load_transform_json(tf_path)
        logger.debug(SEP)
        if not lie.is_se3(transform):
            logger.warning("Not a valid SE(3) transformation!")
        if args.invert_transform:
            transform = lie.se3_inverse(transform)
        logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
            tf_type, transform))
        for traj in trajectories.values():
            traj.transform(transform, right_mul=args.transform_right,
                           propagate=args.propagate_transform)

    if args.t_offset:
        logger.debug(SEP)
        for name, traj in trajectories.items():
            if type(traj) is trajectory.PosePath3D:
                die("{} doesn't have timestamps - can't add time offset.".
                    format(name))
            logger.info("Adding time offset to {}: {} (s)".format(
                name, args.t_offset))
            traj.timestamps += args.t_offset

    if args.n_to_align != -1 and not (args.align or args.correct_scale):
        die("--n_to_align is useless without --align or/and --correct_scale")

    # TODO: this is fugly, but is a quick solution for remembering each synced
    # reference when plotting pose correspondences later...
    synced = (args.subcommand == "kitti" and ref_traj) or any(
        (args.sync, args.align, args.correct_scale, args.align_origin))
    synced_refs = {}
    if synced:
        from evo.core import sync