Python euler.mat2euler方法代码示例

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def find_final_pose_inv(TRANSFORMATIONS_ip):
	# Arguments:
		# TRANSFORMATIONS: 			transformation matrix (batch_size x 4 x 4)
	# Output:
		# final_pose:				final pose predicted by network (batch_size x 6)

	TRANSFORMATIONS = np.copy(TRANSFORMATIONS_ip)
	final_pose = np.zeros((TRANSFORMATIONS.shape[0],6))		# Array to store the poses.
	for i in range(TRANSFORMATIONS.shape[0]):
		TRANSFORMATIONS[i] = np.linalg.inv(TRANSFORMATIONS[i])				
		rot = TRANSFORMATIONS[i,0:3,0:3]					# Extract rotation matrix.
		euler = t3d.mat2euler(rot,'szyx')					# Convert rotation matrix to euler angles. (Pre-multiplication)
		final_pose[i,3:6]=[euler[2],euler[1],euler[0]]		# Store the translation
		final_pose[i,0:3]=TRANSFORMATIONS[i,0:3,3].T 		# Store the euler angles.
	return final_pose

# Subtract the centroids from source and template (Like ICP) and then find the pose. 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def inverse_pose(pose):
	transformation_pose = np.zeros((4,4))
	transformation_pose[3,3]=1
	transformation_pose[0:3,0:3] = t3d.euler2mat(pose[5], pose[4], pose[3], 'szyx')
	transformation_pose[0,3] = pose[0]
	transformation_pose[1,3] = pose[1]
	transformation_pose[2,3] = pose[2]
	transformation_pose = np.linalg.inv(transformation_pose)
	pose_inv = np.zeros((1,6))[0]
	pose_inv[0] = transformation_pose[0,3]
	pose_inv[1] = transformation_pose[1,3]
	pose_inv[2] = transformation_pose[2,3]
	orient_inv = t3d.mat2euler(transformation_pose[0:3,0:3], 'szyx')
	pose_inv[3] = orient_inv[2]
	pose_inv[4] = orient_inv[1]
	pose_inv[5] = orient_inv[0]
	return pose_inv


###################### Shuffling Operations #########################

# Randomly shuffle given array of poses for training procedure. 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def find_final_pose(TRANSFORMATIONS):
	# Arguments:
		# TRANSFORMATIONS: 			transformation matrix (batch_size x 4 x 4)
	# Output:
		# final_pose:				final pose predicted by network (batch_size x 6)

	final_pose = np.zeros((TRANSFORMATIONS.shape[0],6))		# Array to store the poses.
	for i in range(TRANSFORMATIONS.shape[0]):				
		rot = TRANSFORMATIONS[i,0:3,0:3]					# Extract rotation matrix.
		euler = t3d.mat2euler(rot,'szyx')					# Convert rotation matrix to euler angles. (Pre-multiplication)
		final_pose[i,3:6]=[euler[2],euler[1],euler[0]]		# Store the translation
		final_pose[i,0:3]=TRANSFORMATIONS[i,0:3,3].T 		# Store the euler angles.
	return final_pose

# Convert the Final Transformation Matrix to Translation + Orientation (Euler Angles in Degrees) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def xyz_to_rotations_debug(skel, position):
    all_rotations = {}
    all_rotation_matrices = {}
    children_dict = get_child_dict(skel)
    while len(children_dict.keys()) - 1 > len(all_rotation_matrices.keys()):
        for bone in children_dict.keys():
            if bone == None:
                continue
            parent = skel[bone]['parent']
            if bone in all_rotation_matrices.keys():
                continue
            if parent not in all_rotation_matrices.keys() and parent != None:
                continue
            upper = parent
            parent_rot = np.identity(3)
            while upper != None:
                upper_rot = all_rotation_matrices[upper]
                parent_rot = np.dot(upper_rot, parent_rot)
                upper = skel[upper]['parent']

            children = children_dict[bone]
            children_xyz = np.zeros([len(children), 3])
            children_orig = np.zeros([len(children), 3])
            for i in range(len(children)):
                children_xyz[i, :] = np.array(position[children[i]]) - np.array(position[bone])
                children_orig[i, :] = np.array(skel[children[i]]['offsets'])
                children_xyz[i, :] = children_xyz[i, :] * np.linalg.norm(children_orig[i, :]) / np.linalg.norm(children_xyz[i, :])
                assert np.allclose(np.linalg.norm(children_xyz[i, :]), np.linalg.norm(children_orig[i, :]))

            parent_space_children_xyz = np.dot(children_xyz, parent_rot)
            rotation = kabsch(parent_space_children_xyz, children_orig)
            if bone == 'hip':
                all_rotations[bone] = np.array(euler.mat2euler(rotation, 'sxyz')) * (180.0 / math.pi)
            else:
                angles = np.array(euler.mat2euler(rotation, 'syxz')) * (180.0 / math.pi)
                all_rotations[bone] = [
                 angles[1], angles[0], angles[2]]
            all_rotation_matrices[bone] = rotation

    return (all_rotation_matrices, all_rotations) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def peaks_from_best_vector_match(single_match_result, library, rank=0):
    """Takes a VectorMatchingResults object and return the associated peaks,
    to be used in combination with map().

    Parameters
    ----------
    single_match_result : ndarray
        An entry in a VectorMatchingResults
    library : DiffractionLibrary
        Diffraction library containing the phases and rotations
    rank : int
        Get peaks from nth best orientation (default: 0, best vector match)

    Returns
    -------
    peaks : ndarray
        Coordinates of peaks in the matching results object in calibrated units.
    """
    best_fit = get_nth_best_solution(single_match_result, "vector", rank=rank)
    phase_index = best_fit.phase_index

    rotation_orientation = mat2euler(best_fit.rotation_matrix)
    # Don't change the original
    structure = library.structures[phase_index]
    sim = library.diffraction_generator.calculate_ed_data(
        structure,
        reciprocal_radius=library.reciprocal_radius,
        rotation=rotation_orientation,
        with_direct_beam=False,
    )

    # Cut z
    return sim.coordinates[:, :2] 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def write_joint_rotation_matrices_to_bvh(bvh_filename, hip_pose_seq, r_matrix_seq):

    #print (quaternion_data.shape)
    out_seq = []
    seq_len = hip_pose_seq.shape[0]
    for i in range(seq_len):
        hip_pose= hip_pose_seq[i] #3
        out_frame = [hip_pose[0], hip_pose[1], hip_pose[2]]
        hip_x, hip_y,hip_z = euler.mat2euler(r_matrix_seq[i, 0],'sxyz') #hip euler rotation
        out_frame += [hip_z* 180/ np.pi, hip_y* 180/ np.pi,hip_x* 180/ np.pi]#notice in cmu bvh files, the channel for hip rotation is z y x
        for joint in skeleton:
            if(("hip" not in joint) and  (len(skeleton[joint]["channels"])==3)):
                index=joint_index[joint]
                y,x,z=euler.mat2euler(r_matrix_seq[i, index], 'syxz')
                out_frame += [z* 180/ np.pi,x* 180/ np.pi,y* 180/ np.pi]  #notice in cmu bvh files, the channel for joint rotation is z x y
        out_seq +=[out_frame]
    
    ##out_seq now should be seq_len*(3+3*joint_num)
    out_seq =np.array(out_seq)
    out_seq=np.round(out_seq, 6)
    
    #out_seq2=np.ones(out_seq.shape)
    #out_seq2[:,3:out_seq2.shape[1]]=out_seq[:,3:out_seq2.shape[1]].copy()
    
    #print ("bvh data shape")
    #print (out_seq.shape)
    write_frames(standard_bvh_file, bvh_filename, out_seq) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def blender_pose_to_blender_euler(pose):
        euler = [r / np.pi * 180 for r in mat2euler(pose, axes='szxz')]
        euler[0] = -(euler[0] + 90) % 360
        euler[1] = euler[1] - 90
        return np.array(euler) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def rot_euler(self):
        """Returns the (rx, ry, rz) Euler rotations."""
        if self._rot_euler is not None:
            return self._rot_euler
        if self._rot_mat is not None:
            self._rot_euler = mat2euler(self.rot, axes='rxyz')
        return self._rot_euler 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def calculate_relative_camera_pose(full_path1, full_path2, fixated=True, raw=False):
    """
    Given two file path to two json files, extract the 'camera_location'
    and 'camera_rotation_final' field, and calcualte the relative camera pose

    Parameters:
    __________
        full_path1, full_path2: paths to json information

    Returns:
    __________
        camera_poses: vector that encode the camera pose info for two images
    """
    assert os.path.isfile(full_path1) and os.path.isfile(full_path2)
    with open(full_path1, 'r') as fp:
        data1 = json.load(fp)
    with open(full_path2, 'r') as fp:
        data2 = json.load(fp)
    key = ['camera_location', 'camera_rotation_final']
    location1 = np.asarray(data1[key[0]])
    rotation1 = data1[key[1]]
    matrix1 = euler.euler2mat(*rotation1, axes='sxyz')
    location2 = np.asarray(data2[key[0]])
    rotation2 = data2[key[1]]
    matrix2 = euler.euler2mat(*rotation2, axes='sxyz')
    relative_rotation_matrix = np.matmul(np.transpose( matrix2 ), matrix1)
    relative_rotation = euler.mat2euler(relative_rotation_matrix, axes='sxyz')
    translation = np.matmul(np.transpose(matrix2), location1 - location2)
    pose = np.hstack((relative_rotation, translation))
    if not raw:
        if fixated:
            std  = np.asarray([ 10.12015407, 8.1103528, 1.09171896, 1.21579016, 0.26040945, 10.05966329])
            mean = np.asarray([ -2.67375523e-01, -1.19147040e-02, 1.14497274e-02, 1.10903410e-03, 2.10509948e-02, -4.02013549e+00])
        else:
            mean = np.asarray([ -9.53197445e-03,  -1.05196691e-03,  -1.07545642e-02,
                        2.08785638e-02,  -9.27858049e-02,  -2.58052205e+00])
            std = np.asarray([ 1.02316223,  0.66477511,  1.03806996,  5.75692889,  1.37604962,
                        7.43157247])
        pose = (pose - mean)/std   
    return pose


########################################
# Fixated and Non-fixated Camera Pose  #
######################################## 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import mat2euler [as 别名]
def room_layout( filename ):
    '''
    Room Bounding Box.
    Returns:
    --------
        bb: length 6 vector
    '''
    root, domain, model_id, point_id, view_id = parse_filename(filename)

    fname = 'point_{point_id}_view_{view_id}_domain_{domain}.json'.format(
                    point_id=point_id,
                    view_id=view_id,
                    domain='fixatedpose')
    json_file = os.path.join(root, model_id, 'points', fname)
    with open(json_file) as fp:
        data = json.load(fp)

    def homogenize( M ):
        return np.concatenate( [M, np.ones( (M.shape[0],1) )], axis=1 )

    def convert_world_to_cam( points, cam_mat=None ):
        new_points = points.T
        homogenized_points = homogenize( new_points )
        new_points = np.dot( homogenized_points, np.linalg.inv(cam_mat).T )[:,:3]
        return new_points
    
    mean = np.array([0.006072743318127848, 0.010272365569691076, -3.135909774145468, 
            1.5603802322235532, 5.6228218371102496e-05, -1.5669352793761442,
            5.622875878174759, 4.082800262277375, 2.7713941642895956])
    std = np.array([0.8669452525283652, 0.687915294956501, 2.080513632043758, 
            0.19627420479282623, 0.014680602791251812, 0.4183827359302299,
            3.991778013006544, 2.703495278378409, 1.2269185938626304])
    camera_matrix, bb = get_room_layout_cam_mat_and_ranges(data, make_x_major=True)
    camera_matrix_euler = transforms3d.euler.mat2euler(camera_matrix[:3,:3], axes='sxyz')
    vertices = np.array(list(itertools.product( *bb )))
    vertices_cam = convert_world_to_cam(vertices.T, camera_matrix)
    cube_center = np.mean(vertices_cam, axis=0)

    x_scale, y_scale, z_scale = bb[:,1] - bb[:,0] # maxes - mins
    bbox_cam = np.hstack(
        (cube_center, 
        camera_matrix_euler,
        x_scale, y_scale, z_scale))
    bbox_cam = (bbox_cam - mean) / std 
    return bbox_cam

####################
# ImageNet Softmax #
####################