File size: 2,764 Bytes
4409449 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 |
from .geometry_tools import *
def rep_to_rep(oldtype, newtype, rotations):
if newtype in ["matrix"]:
return to_matrix(oldtype, rotations)
if oldtype in ["rotvec", "axisangle"]:
return axis_angle_to(newtype, rotations)
elif oldtype in ["matrix"]:
return matrix_to(newtype, rotations)
else:
raise NotImplementedError("Only rotvec and matrix are supported.")
def nfeats_of(rottype):
if rottype in ["rotvec", "axisangle"]:
return 3
elif rottype in ["rotquat", "quaternion"]:
return 4
elif rottype in ["rot6d", "6drot", "rotation6d"]:
return 6
elif rottype in ["rotmat"]:
return 9
else:
return TypeError("This rotation type doesn't have features.")
def axis_angle_to(newtype, rotations):
if newtype in ["matrix"]:
rotations = axis_angle_to_matrix(rotations)
return rotations
elif newtype in ["rotmat"]:
rotations = axis_angle_to_matrix(rotations)
rotations = matrix_to("rotmat", rotations)
return rotations
elif newtype in ["rot6d", "6drot", "rotation6d"]:
rotations = axis_angle_to_matrix(rotations)
rotations = matrix_to("rot6d", rotations)
return rotations
elif newtype in ["rotquat", "quaternion"]:
rotations = axis_angle_to_quaternion(rotations)
return rotations
elif newtype in ["rotvec", "axisangle"]:
return rotations
else:
raise NotImplementedError
def matrix_to(newtype, rotations):
if newtype in ["matrix"]:
return rotations
if newtype in ["rotmat"]:
rotations = rotations.reshape((*rotations.shape[:-2], 9))
return rotations
elif newtype in ["rot6d", "6drot", "rotation6d"]:
rotations = matrix_to_rotation_6d(rotations)
return rotations
elif newtype in ["rotquat", "quaternion"]:
rotations = matrix_to_quaternion(rotations)
return rotations
elif newtype in ["rotvec", "axisangle"]:
rotations = matrix_to_axis_angle(rotations)
return rotations
else:
raise NotImplementedError
def to_matrix(oldtype, rotations):
if oldtype in ["matrix"]:
return rotations
if oldtype in ["rotmat"]:
rotations = rotations.reshape((*rotations.shape[:-2], 3, 3))
return rotations
elif oldtype in ["rot6d", "6drot", "rotation6d"]:
rotations = rotation_6d_to_matrix(rotations)
return rotations
elif oldtype in ["rotquat", "quaternion"]:
rotations = quaternion_to_matrix(rotations)
return rotations
elif oldtype in ["rotvec", "axisangle"]:
rotations = axis_angle_to_matrix(rotations)
return rotations
else:
raise NotImplementedError
|