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 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)