def test_axangle_euler():
# Conversion between axis, angle and euler
for x, y, z in euler_tuples:
M1 = ttb.euler2mat(z, y, x)
ax, angle = ttb.euler2axangle(z, y, x)
M2 = taa.axangle2mat(ax, angle)
assert_array_almost_equal(M1, M2)
zp, yp, xp = ttb.axangle2euler(ax, angle)
M3 = ttb.euler2mat(zp, yp, xp)
assert_array_almost_equal(M1, M3)
def test_quats():
for x, y, z in euler_tuples:
M1 = ttb.euler2mat(z, y, x)
quatM = tq.mat2quat(M1)
quat = ttb.euler2quat(z, y, x)
assert tq.nearly_equivalent(quatM, quat)
quatS = sympy_euler2quat(z, y, x)
assert tq.nearly_equivalent(quat, quatS)
zp, yp, xp = ttb.quat2euler(quat)
# The parameters may not be the same as input, but they give the
# same rotation matrix
M2 = ttb.euler2mat(zp, yp, xp)
assert_array_almost_equal(M1, M2)
def test_euler_mat():
M = ttb.euler2mat(0, 0, 0)
assert_array_equal(M, np.eye(3))
for x, y, z in euler_tuples:
M1 = ttb.euler2mat(z, y, x)
M2 = sympy_euler(z, y, x)
assert_array_almost_equal(M1, M2)
M3 = np.dot(x_only(x), np.dot(y_only(y), z_only(z)))
assert_array_almost_equal(M1, M3)
zp, yp, xp = ttb.mat2euler(M1)
# The parameters may not be the same as input, but they give the
# same rotation matrix
M4 = ttb.euler2mat(zp, yp, xp)
assert_array_almost_equal(M1, M4)
def _create_geometry_rot2(cls, line1, line2, family=None, use_angle=False, **kw):
"""
:param line1:
:param line2:
:param family:
:param use_angle:
:return:
"""
own_angle = line1.angle_to(line2)
famx, famy = cls.family_directions(family)
famz = np.array([0, 0, 1])
if use_angle is True:
famy = np.array([np.sin(own_angle), np.cos(own_angle), 0])
all_axes = np.stack([famz, famx, famy]).T
# cross product of lines to get their plane normal
tgt_cross = np.cross(line1.unit_vector, line2.unit_vector)
# axis - angle to align normals
M = geo.rotation_matrix(famz, tgt_cross)
zrot, yrot, xrot = tb.mat2euler(M)
axa = tb.euler2axangle(zrot, yrot, xrot)
# create euler matrix for first transformation
m1, m2, m3 = tb.euler2mat(zrot, 0, 0), tb.euler2mat(0, yrot, 0), tb.euler2mat(0, 0, xrot)
A = geombase.euler_composition(m1, m2, m3)
# create euler matrix for first transformation
family_transformed1 = np.dot(A, all_axes).T
# now the normal planes are aligned, but need to get 2nd rotation
# which occurs around target plane's normal
a11, a12, a21, a22 = cls._directed_combos(family_transformed1, line1, line2)
tests = [a11, -a11, a12, -a12, a21, -a21, a22, -a22]
final_angle, best = 0, 1e6
while tests:
t = tests.pop()
res = cls._directs(family_transformed1, tgt_cross, line1, line2, t)
if res < best:
best, final_angle = res, t
if np.dot(line1.unit_vector, line2.unit_vector) > 0:
own_angle = math.pi - own_angle
tsf1 = axa[0].tolist() + [axa[1]]
tsf2 = tgt_cross.tolist() + [final_angle, own_angle]
return line1.numpy[0].tolist() + tsf1 + tsf2
def test_euler_imps():
for x, y, z in euler_tuples:
M1 = tg.euler_matrix(z, y, x, 'szyx')[:3, :3]
M2 = ttb.euler2mat(z, y, x)
assert_array_almost_equal(M1, M2)
q1 = tg.quaternion_from_euler(z, y, x, 'szyx')
q2 = ttb.euler2quat(z, y, x)
assert tq.nearly_equivalent(q1, q2)
def test_euler_instability():
# Test for numerical errors in mat2euler
# problems arise for cos(y) near 0
po2 = pi / 2
zyx = po2, po2, po2
M = ttb.euler2mat(*zyx)
# Round trip
M_back = ttb.euler2mat(*ttb.mat2euler(M))
assert np.allclose(M, M_back)
# disturb matrix slightly
M_e = M - FLOAT_EPS
# round trip to test - OK
M_e_back = ttb.euler2mat(*ttb.mat2euler(M_e))
assert np.allclose(M_e, M_e_back)
# not so with crude routine
M_e_back = ttb.euler2mat(*crude_mat2euler(M_e))
assert not np.allclose(M_e, M_e_back)
def _directs(cls, va, tgt_cross, line1, line2, angle):
eul = tb.axangle2euler(tgt_cross, angle)
mat = tb.euler2mat(*eul)
va2 = np.dot(mat, va.T).T
a11, a12, a21, a22 = cls._directed_combos(va2, line1, line2)
t1 = np.array([a11 / line1.length, a22 / line2.length]).sum()
t2 = np.array([a12 / line1.length, a21 / line2.length]).sum()
return min([t1, t2])
def test_quaternion_imps():
for x, y, z in euler_tuples:
M = ttb.euler2mat(z, y, x)
quat = tq.mat2quat(M)
# Against transformations code
tM = tg.quaternion_matrix(quat)
assert_array_almost_equal(M, tM[:3, :3])
M44 = np.eye(4)
M44[:3, :3] = M
tQ = tg.quaternion_from_matrix(M44)
assert tq.nearly_equivalent(quat, tQ)
def test_de_compose():
# Make a series of translations etc, compose and decompose
for trans in permutations([10,20,30]):
for rots in permutations([0.2,0.3,0.4]):
for zooms in permutations([1.1,1.9,2.3]):
for shears in permutations([0.01, 0.04, 0.09]):
Rmat = euler2mat(*rots)
M = compose(trans, Rmat, zooms, shears)
for func in decompose, decompose44:
T, R, Z, S = func(M)
assert (
np.allclose(trans, T) and
np.allclose(Rmat, R) and
np.allclose(zooms, Z) and
np.allclose(shears, S))
def handle_angle_data(euler_angles):
#euler angles = [ [pitch_t0 , roll_t0 , yaw_t0] , [pitch_t1 , roll_t1 , yaw_t1] ...] in degrees
list_of_rot_mats = []
for t_slice in euler_angles:
#Negatives added upon testing to assure it looked right
pitch = np.deg2rad(-t_slice[0])
roll = np.deg2rad(-t_slice[1])
yaw = np.deg2rad(t_slice[2])
_rotmat = taitbryan.euler2mat(yaw, pitch, roll)
list_of_rot_mats.append(_rotmat)
return list_of_rot_mats
def test_angle_axis_imps():
for x, y, z in euler_tuples:
M = ttb.euler2mat(z, y, x)
q = tq.mat2quat(M)
vec, theta = tq.quat2axangle(q)
M1 = axangle2mat(vec, theta)
M2 = axangle2aff(vec, theta)[:3,:3]
assert_array_almost_equal(M1, M2)
v1, t1 = mat2axangle(M1)
M3 = axangle2mat(v1, t1)
assert_array_almost_equal(M, M3)
A = np.eye(4)
A[:3,:3] = M
v2, t2, point = aff2axangle(A)
M4 = axangle2mat(v2, t2)
assert_array_almost_equal(M, M4)
def rotate(points, range=None):
""" Rotates the points around the origin with a uniformly sampled rotation matrix.
:param range: The range/extent of rotation angle for the Z-, Y-, and X-axis rotations.
The default range is the full set of Euler angles, i.e.,
[0, 2pi) for Z- and X-axis, and [0, pi) for the Y-axis.
The rotation will be applied always in the order Z first, then Y, and finally X.
:param points: N x 3 tensor
"""
range = (2 * np.pi, np.pi, 2 * np.pi) if range is None else range
assert type(range) == tuple and len(range) == 3
az = np.random.uniform(0, range[0], 1)
ay = np.random.uniform(0, range[1], 1)
ax = np.random.uniform(0, range[2], 1)
# Static frame rotation around z-axis, then y-axis, then x-axis (Tait-Bryan).
mat = euler2mat(az, ay, ax)
mat = torch.as_tensor(mat, dtype=points.dtype, device=points.device)
points = torch.matmul(points, mat.transpose(0, 1))
return points, mat
def update(self, psi, theta, phi):
# calculate rotation matrix
self.rotMatrix = euler2mat(psi, theta, phi)
# calculate new axes vectors
new_x = np.dot(self.rotMatrix, self.x_body)
new_y = np.dot(self.rotMatrix, self.y_body)
new_z = np.dot(self.rotMatrix, self.z_body)
# update x axis
self.x.set_xdata(np.array([0, new_x[0]]))
self.x.set_ydata(np.array([0, new_x[1]]))
self.x.set_3d_properties(np.array([0, new_x[2]]))
# update y axis
self.y.set_xdata(np.array([0, new_y[0]]))
self.y.set_ydata(np.array([0, new_y[1]]))
self.y.set_3d_properties(np.array([0, new_y[2]]))
# update z axis
self.z.set_xdata(np.array([0, new_z[0]]))
self.z.set_ydata(np.array([0, new_z[1]]))
self.z.set_3d_properties(np.array([0, new_z[2]]))
self.psiText.set_text('psi={:+07.2f}'.format(psi * to_deg))
self.thetaText.set_text('theta={:+07.2f}'.format(theta * to_deg))
self.phiText.set_text('phi={:+07.2f}'.format(phi * to_deg))
return
def test_basic_euler():
# some example rotations, in radians
zr = 0.05
yr = -0.4
xr = 0.2
# Rotation matrix composing the three rotations
M = ttb.euler2mat(zr, yr, xr)
# Corresponding individual rotation matrices
M1 = ttb.euler2mat(zr, 0, 0)
M2 = ttb.euler2mat(0, yr, 0)
M3 = ttb.euler2mat(0, 0, xr)
# which are all valid rotation matrices
for rot in (M, M1, M2, M3):
assert is_valid_rotation(rot)
# Full matrix is composition of three individual matrices
assert np.allclose(M, np.dot(M3, np.dot(M2, M1)))
# Applying an opposite rotation same as inverse (the inverse is
# the same as the transpose, but just for clarity)
assert np.allclose(ttb.euler2mat(0, 0, -xr),
np.linalg.inv(ttb.euler2mat(0, 0, xr)))
def test_with_euler2mat():
# Test against Tait-Bryan implementation
for a, b, c in euler_tuples:
tb_mat = tb.euler2mat(a, b, c)
gen_mat = euler2mat(a, b, c, 'szyx')
assert_almost_equal(tb_mat, gen_mat)
""" Various samples for testing
"""
import numpy as np
from transforms3d.utils import inique, permuted_signs, permuted_with_signs
from transforms3d.taitbryan import euler2mat
# Regular points around a sphere
_r13 = np.sqrt(1 / 3.0)
_r12 = np.sqrt(0.5)
sphere_points = (tuple(inique(permuted_with_signs([1, 0, 0]))) +
tuple(inique(permuted_with_signs([_r12, _r12, 0]))) +
tuple(inique(permuted_signs([_r13, _r13, _r13]))))
# Example rotations '''
euler_tuples = []
params = np.arange(-np.pi, np.pi, np.pi / 2)
euler_tuples = tuple((x, y, z) for x in params for y in params for z in params)
euler_mats = tuple(euler2mat(*t) for t in euler_tuples)
def test_with_euler2mat():
# Test against Tait-Bryan implementation
for a, b, c in euler_tuples:
tb_mat = tb.euler2mat(a, b, c)
gen_mat = euler2mat(a, b, c, 'szyx')
assert_almost_equal(tb_mat, gen_mat)
def placePose(fileName, controlList, dofList, attrList, useRootPos=False, rootPos=np.array([0,0,0]), hScale=1, trackFeet=False):
# first reset pose
resetGesture(controlList, dofList)
df = pd.read_csv(fileName, delimiter=',', header=None)
poseList = [np.array([x[0], -x[1], -x[2]]) for x in df.values]
assert(len(poseList) == 25)
poseDict = {}
poseDict["head"] = poseList[0];
poseDict["neck"] = poseList[1];
poseDict["rightArm"] = poseList[2];
poseDict["rightForeArm"] = poseList[3];
poseDict["rightHand"] = poseList[4];
poseDict["leftArm"] = poseList[5];
poseDict["leftForeArm"] = poseList[6];
poseDict["leftHand"] = poseList[7];
poseDict["hip"] = poseList[8];
poseDict["rightUpLeg"] = poseList[9];
poseDict["rightLeg"] = poseList[10];
poseDict["rightFoot"] = poseList[24];
poseDict["leftUpLeg"] = poseList[12];
poseDict["leftLeg"] = poseList[13];
poseDict["leftFoot"] = poseList[21];
poseDict["rightToe"] = poseList[22];
poseDict["leftToe"] = poseList[19];
# spine
vec0 = poseDict['leftArm'] - poseDict['hip']
vec1 = poseDict['rightArm'] - poseDict['hip']
body_norm = np.cross(vec0, vec1)
body_up = 0.5*(vec0 + vec1)
vec2 = np.array(mc.getAttr(controlList[13] + '.' + attrList[1])) - np.array(mc.getAttr(controlList[0] + '.' + attrList[1]))
vec3 = np.array(mc.getAttr(controlList[10] + '.' + attrList[1])) - np.array(mc.getAttr(controlList[0] + '.' + attrList[1]))
body_up_1 = 0.5*(vec2[0] + vec3[0])
body_norm_1 = np.cross(vec2[0], vec3[0])
_, _, euler = vectorPairsToEuler(body_up, body_up_1, body_norm, body_norm_1)
mc.setAttr(controlList[1] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
# head
vec = poseDict["head"] - poseDict["neck"]
ref = poseDict["neck"] - poseDict["hip"]
euler = vectors2Euler(ref, vec)
#print("head:")
#print(euler)
mc.setAttr(controlList[16] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
# right arm
vec = poseDict["rightForeArm"] - poseDict["rightArm"]
ref = np.cross(body_norm, body_up)
euler = vectors2Euler(ref, vec)
#print("right arm:")
#print(euler)
mc.setAttr(controlList[10] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
# left arm
vec = poseDict["leftForeArm"] - poseDict["leftArm"]
ref = np.cross(body_up, body_norm)
euler = vectors2Euler(ref, vec)
#print("left arm:")
#print(euler)
mc.setAttr(controlList[13] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
# right forearm
vec = poseDict["rightHand"] - poseDict["rightForeArm"]
ref = poseDict["rightForeArm"] - poseDict["rightArm"]
euler = vectors2Euler(ref, vec)
#print("right forearm:")
#print(euler)
mc.setAttr(controlList[11] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
# left forearm
vec = poseDict["leftHand"] - poseDict["leftForeArm"]
ref = poseDict["leftForeArm"] - poseDict["leftArm"]
euler = vectors2Euler(ref, vec)
#print("left forearm:")
#print(euler)
mc.setAttr(controlList[14] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
# right up leg
vec = poseDict["rightLeg"] - poseDict["rightUpLeg"]
ref = -body_up
euler = vectors2Euler(ref, vec)
#print("right up leg:")
#print(euler)
mc.setAttr(controlList[4] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
# left up leg
vec = poseDict["leftLeg"] - poseDict["leftUpLeg"]
ref = -body_up
euler = vectors2Euler(ref, vec)
#print("left up leg:")
#print(euler)
mc.setAttr(controlList[7] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
# right leg
vec = poseDict["rightFoot"] - poseDict["rightLeg"]
ref = poseDict["rightLeg"] - poseDict["rightUpLeg"]
euler = vectors2Euler(ref, vec)
#print("right leg:")
#print(euler)
mc.setAttr(controlList[5] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
# left leg
vec = poseDict["leftFoot"] - poseDict["leftLeg"]
ref = poseDict["leftLeg"] - poseDict["leftUpLeg"]
euler = vectors2Euler(ref, vec)
#print("left leg:")
#print(euler)
mc.setAttr(controlList[8] + '.' + attrList[0], euler[0], euler[1], euler[2], type='double3')
if trackFeet:
# right foot
vec = poseDict["rightToe"] - poseDict["rightFoot"]
ref = poseDict["rightFoot"] - poseDict["rightLeg"]
mat = np.array(trans2.euler2mat(0, 0, -np.pi/4))
ref = mat.dot(ref)
euler = vectors2Euler(ref, vec)
#print("right foot:")
#print(euler)
mc.setAttr(controlList[6] + '.' + attrList[0], euler[0], euler[1], 0, type='double3')
# left foot
vec = poseDict["leftToe"] - poseDict["leftFoot"]
ref = poseDict["leftFoot"] - poseDict["leftLeg"]
mat = np.array(trans2.euler2mat(0, 0, -np.pi/4))
ref = mat.dot(ref)
euler = vectors2Euler(ref, vec)
#print("left foot:")
#print(euler)
mc.setAttr(controlList[9] + '.' + attrList[0], euler[0], euler[1], 0, type='double3')
# whole body rotation
vec = poseDict['neck'] - poseDict['hip']
ref = np.array(mc.getAttr(controlList[17] + '.' + attrList[1])) - np.array(mc.getAttr(controlList[0] + '.' + attrList[1]))
euler1 = vectors2Euler(ref[0], vec)
mc.setAttr(controlList[0] + '.' + attrList[0], euler1[0], euler1[1], euler1[2], type='double3')
# whole body translation
if useRootPos:
vec = hScale*(poseDict['hip'] - rootPos)
mc.setAttr(controlList[0] + '.' + attrList[1], vec[0], vec[1], vec[2], type='double3')
def panorama(idx, cam_prefix, imu_ts, ukf_euler):
# camera projection params
Wfov = np.pi / 3
Hfov = np.pi / 4
frame = None
# load cam data
cam = io.loadmat(cam_prefix + str(idx) + ".mat")
cam_vals = np.array(cam['cam']) # m*n*3*k
cam_ts = np.array(cam['ts']).T # k*1
imu_idx = 0
# init video writer
outfile = 'result/panorama' + str(idx) + '.avi'
fourcc = cv2.VideoWriter_fourcc('D', 'I', 'V', 'X')
video = cv2.VideoWriter(filename=outfile,
fourcc=fourcc,
fps=20.0,
frameSize=(1920, 960))
for i in range(cam_ts.shape[0]):
RGB = cam_vals[:, :, :, i]
height, width, channel = RGB.shape
cam_t = cam_ts[i]
# generate spherical coordinate
radius = 1
azimuth = -(np.arange(width) / (width - 1) - 0.5) * Wfov
altitude = -(np.arange(height) / (height - 1) - 0.5) * Hfov
# negate because pixel origin is at top left
azimuth, altitude = np.meshgrid(azimuth, altitude)
# transform to cartesian coordinates on unit sphere
X = radius * np.cos(altitude) * np.cos(azimuth)
Y = radius * np.cos(altitude) * np.sin(azimuth)
Z = radius * np.sin(altitude)
C = np.dstack((X, Y, Z)) # (m,n,3)
# rotate by the nearest timestamp
if imu_idx > len(imu_ts) - 1: # check boundary
break
while imu_ts[imu_idx] < cam_t and imu_idx < len(imu_ts) - 1:
imu_idx += 1
if imu_ts[imu_idx] + imu_ts[imu_idx - 1] > 2 * cam_t: # choose closest
imu_idx -= 1
euler = ukf_euler[imu_idx]
imu_idx += 1 # prevent replication
R = taitbryan.euler2mat(euler[0], euler[1], euler[2])
C = np.einsum('pr,mnr->mnp', R, C)
# transform cartesian back to spherical
X = C[:, :, 0]
Y = C[:, :, 1]
Z = C[:, :, 2]
azimuth = -np.arctan2(Y, X)
altitude = -np.arctan2(
Z, np.sqrt(X**2 + Y**2)) # altitude range(-pi/2,pi/2)
# negate back
# radius doesn't change
# project sphere to a plane, convert plane into pixel by scaling
Px = ((azimuth + np.pi) / Wfov * width).astype(np.uint)
Py = ((altitude + np.pi / 2) / Hfov * height).astype(np.uint)
# Py = np.flipud(Py.reshape((height, width))).reshape((1,-1))
# paint your pixels on to image
if frame is None: # init panorama after knowing image size and camera params
frame = np.zeros(
(int(np.pi / Hfov * height), int(2 * np.pi / Wfov * width), 3),
dtype=np.uint8)
frame[Py, Px, :] = RGB
# display animation
cv2.imshow('Stitching Panorama', frame)
cv2.waitKey(10)
# write frame to video file
video.write(frame)
video.release()
cv2.destroyAllWindows()
""" Various samples for testing
"""
import numpy as np
from transforms3d.utils import inique, permuted_signs, permuted_with_signs
from transforms3d.taitbryan import euler2mat
# Regular points around a sphere
_r13 = np.sqrt(1/3.0)
_r12 = np.sqrt(0.5)
sphere_points = (
tuple(inique(permuted_with_signs([1, 0, 0]))) +
tuple(inique(permuted_with_signs([_r12, _r12, 0]))) +
tuple(inique(permuted_signs([_r13, _r13, _r13])))
)
# Example rotations '''
euler_tuples = []
params = np.arange(-np.pi,np.pi,np.pi/2)
euler_tuples = tuple((x, y, z)
for x in params
for y in params
for z in params)
euler_mats = tuple(euler2mat(*t) for t in euler_tuples)
