Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
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")
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)
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)
def fake_path(length):
return PosePath3D(poses_se3=random_se3_list(length))
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")
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:
"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)
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()
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