o ݱi9 @sdZddlZddlZddlmmZddZddZddZ d6d d Z d ej d ej fddZ dej d ej fddZ dej d ej fddZdej d ej fddZdej d ej fddZdej d ej fddZdej d ej fddZdej d ej fd d!Zdej d ej fd"d#Zdej d$ed ej fd%d&Zd'ej d$ed ej fd(d)Zd*ed efd+d,Zd-ed.ed/ed0ed ej f d1d2Zd-ed3ej d ej fd4d5ZdS)7z, This file contains some PyTorch utilities. NcCt|}t|}|SzR Converts tensor with rot_6d representation to axis-angle representation. )rotation_6d_to_matrixmatrix_to_axis_anglerot_6drot_matZrotr @/data/cameron/vidgen/unified-world-model/datasets/droid/utils.pyrot_6d_to_axis_angle r cCs4t|}tjjj|}|d}t |}|S)M Converts tensor with rot_6d representation to euler representation. xyz) rscipyspatial transformZRotationZ from_matrixnumpyZas_eulertorchTensorrr r r rot_6d_to_euler_angless   rcCrr)axis_angle_to_matrixmatrix_to_rotation_6d) axis_anglerrr r r axis_angle_to_rot_6dr rXYZcCst|dd}t|}|S)r r) convention)euler_angles_to_matrixr) euler_anglesrrrr r r euler_angles_to_rot_6d's rxreturncCs(t|}|dk}t||||<|S)z[ Returns torch.sqrt(torch.max(0, x)) but with a zero subgradient where x is 0. r)r zeros_likesqrt)rretZ positive_maskr r r _sqrt_positive_part0s r$ quaternionscCst|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). @Nr*)runbindsumstackreshapeshape)r%rijkZtwo_sor r r quaternion_to_matrix;s   r5matrixc Cs|ddks|ddkrtd|jd|jdd}tj||ddd\ }}}}}}}} } ttjd ||| d ||| d ||| d ||| gdd} tjtj| d d | |||||gddtj| || d d ||||gddtj||||| d d || gddtj||||| || dd gddgdd} tdj | j | j d} | d| d | }|t j| jdddddkddf|dS)z Convert rotations given as rotation matrices to quaternions. Args: matrix: Rotation matrices as tensor of shape (..., 3, 3). Returns: quaternions with real part first, as tensor of shape (..., 4). r&r*Invalid rotation matrix shape .N) dim?).r).r().r>).r*g?)dtypedevicer').N) num_classes?)rA)size ValueErrorr/rr+r.r$r-tensortor?r@maxFone_hotargmax)r6 batch_dimZm00Zm01Zm02Zm10Zm11Zm12Zm20Zm21Zm22Zq_absZ quat_by_rijkZflrZquat_candidatesr r r matrix_to_quaternionYs> ((((  rMrcC 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). )r5axis_angle_to_quaternion)rr r r r rcCrN)ay Convert rotations given as rotation matrices to axis/angle. Args: matrix: Rotation matrices as tensor of shape (..., 3, 3). Returns: 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. )quaternion_to_axis_anglerM)r6r r r rrPrcCstj|dddd}|d}d}||k}t|}t||||||<d||||d||<tjt|||gdd}|S) a Convert rotations given as axis/angle to quaternions. 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: quaternions with real part first, as tensor of shape (..., 4). r>r&Tpr<keepdimrCư>0r;)rnormabs empty_likesincatcos)rangles half_angleseps small_anglessin_half_angles_over_anglesr%r r r rOs   rOcCstj|dddfdddd}t||dddf}d|}d}||k}t|}t||||||<d ||||d ||<|dddf|S) a Convert rotations given as quaternions to axis/angle. Args: quaternions: quaternions with real part first, as tensor of shape (..., 4). Returns: 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. .r(Nr>r&TrRrUrCrV)rrWatan2rXrYrZ)r%normsr^r]r_r`rar r r rQs   rQd6cCs||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 orthogonalization 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 .Nr*r&r;T)rTr7)rI normalizer,rcrossr-)rda1a2b1b2b3r r r rs "rcCs4|dd}|dddddf|dS)a Converts rotation matrices to 6D rotation representation by Zhou et al. [1] by dropping the last row. Note that 6D representation is not unique. Args: matrix: batch of rotation matrices of size (*, 3, 3) Returns: 6D rotation representation, of size (*, 6) [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 Nr7.r>))rDcloner.)r6rLr r r rs $rrc CsBt|dkr td|d|d|dfvrtd|d|D]}|dvr.td |d q |d dks=|d dkrFtd |jdt|d}t|d}||k}|rmt|d||f||dvrhdnd}n t|d||f}t|d|d|d|fd||t|d|d|d|ddfd|f}t |d S)a, Convert rotations given as rotation matrices to Euler angles in radians. Args: matrix: Rotation matrices as tensor of shape (..., 3, 3). convention: Convention string of three uppercase letters. Returns: Euler angles in radians as tensor of shape (..., 3). r*Convention must have 3 letters.r(rr>Invalid convention r9XYZInvalid letter  in convention string.r&r7r8.)r&r>gr=FNT) lenrErDr/_index_from_letterrasinacos_angle_from_tanr-)r6rletteri0i2 tait_bryanZ central_angler4r r r matrix_to_euler_angless6      rrcCs|dks |jddkrtdt|dkrtd|d|d|dfvr/td|d |D]}|d vr?td |d q1d dt|t|dD}tt|d|d|dS)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). rr&r*zInvalid input euler angles.rnr(r>ror9rprtrucSsg|] \}}t||qSr )_axis_angle_rotation).0cer r r Rsz*euler_angles_to_matrix..)r<r/rErvziprr+matmul)rrr{matricesr r r r=s   rr{cCs,|dkrdS|dkr dS|dkrdStd)Nrqrrrr(rsr> letter must be either X, Y or Z.)rE)r{r r r rwZsrwaxis other_axis horizontalr~cCsdddd|\}}|r||}}||dv}||kr)t|d|f|d|fS|r:t|d|f |d|fSt|d|f|d|f S)a Extract the first or third Euler angle from the two members of the matrix which are positive constant times its sine and cosine. Args: axis: Axis label "X" or "Y or "Z" for the angle we are finding. other_axis: Axis label "X" or "Y or "Z" for the middle axis in the convention. data: Rotation matrices as tensor of shape (..., 3, 3). horizontal: Whether we are looking for the angle for the third axis, which means the relevant entries are in the same row of the rotation matrix. If not, they are in the same column. tait_bryan: Whether the first and third axes in the convention differ. Returns: Euler Angles in radians for each matrix in data as a tensor of shape (...). )r>r()rr>)r(rrp)ZXYZYZZZX.)rrb)rrdatarr~i1r}evenr r r rzds  rzanglec Cst|}t|}t|}t|}|dkr%|||||| |||f }n&|dkr6||||||| ||f }n|dkrG|| |||||||f }ntdt|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). rqrrrsrr&r)) rr\rZ ones_liker!rEr-r.r/)rrr\rZonezeroZR_flatr r r rs   r)r)__doc__rrZtorch.nn.functionalnn functionalrIr rrrrr$r5rMrrrOrQrrstrrrintrwboolrzrr r r r s@    <*  !