o ॵi@shddlZddlZddlmmZddZdefddZdefdd Z d d Z d ej d ej fddZ dS)NcCst|d\}}}}d||d}td||||||||||||||||||||d||||||||||||||||||||d|||||f d}||jdddS)z Convert rotations given as quaternions to rotation matrices. Args: quaternions: quaternions with real part first, as tensor of shape (..., 4). Returns: Rotation matrices as tensor of shape (..., 3, 3). g@Nr)torchunbindsumstackreshapeshape) quaternionsrijktwo_sori/home/ubuntu/.local/lib/python3.10/site-packages/modelscope/utils/cv/motion_utils/rotation_conversions.pyquaternion_to_matrix s   raxisc Cst|}t|}t|}t|}|dkr$|||||| |||f }|dkr4||||||| ||f }|dkrD|| |||||||f }t|d|jdS)aM Return the rotation matrices for one of the rotations about an axis of which Euler angles describe, for each value of the angle given. Args: axis: Axis label "X" or "Y or "Z". angle: any shape tensor of Euler angles in radians Returns: Rotation matrices as tensor of shape (..., 3, 3). XYZrr)rcossin ones_like zeros_liker r r )ranglerronezeroR_flatrrr_axis_angle_rotation)s   r" conventioncCs|dks |jddkrtdt|dkrtd|d|d|dfvr/td|d |D]}|d vr?td |d q1tt|t|d}t tj |S) aW Convert rotations given as Euler angles in radians to rotation matrices. Args: euler_angles: Euler angles in radians as tensor of shape (..., 3). convention: Convention string of three uppercase letters from {"X", "Y", and "Z"}. Returns: Rotation matrices as tensor of shape (..., 3, 3). rrrzInvalid input euler angles.zConvention must have 3 letters.rzInvalid convention .)rrrzInvalid letter z in convention string.) dimr ValueErrorlenmapr"rr functoolsreducematmul) euler_anglesr#lettermatricesrrreuler_angles_to_matrixEs   r0cCs tt|S)a Convert rotations given as axis/angle to rotation matrices. Args: axis_angle: Rotations given as a vector in axis angle form, as a tensor of shape (..., 3), where the magnitude is the angle turned anticlockwise in radians around the vector's direction. Returns: Rotation matrices as tensor of shape (..., 3, 3). )raxis_angle_to_quaternion) axis_anglerrraxis_angle_to_matrix_s r3d6returncCs||dddf|dddf}}tj|dd}|||jddd|}tj|dd}tj||dd}tj|||fddS) a Converts 6D rotation representation by Zhou et al. [1] to rotation matrix using Gram--Schmidt orthogonalisation per Section B of [1]. Args: d6: 6D rotation representation, of size (*, 6) Returns: batch of rotation matrices of size (*, 3, 3) [1] Zhou, Y., Barnes, C., Lu, J., Yang, J., & Li, H. On the Continuity of Rotation Representations in Neural Networks. IEEE Conference on Computer Vision and Pattern Recognition, 2019. Retrieved from http://arxiv.org/abs/1812.07035 .Nrr)r&T)keepdim)F normalizerrcrossr )r4a1a2b1b2b3rrrrotation_6d_to_matrixos "r@) r*rtorch.nn.functionalnn functionalr8rstrr"r0r3Tensorr@rrrrs