o ߥi&@sddlZddlZddlmmZddlmZddZd(ddZ d d Z d ejfd d Z d)ddZ ddZ ddZddZdejdejfddZdejdejfddZdejdejfdd Zdejdejfd!d"Zd*d$ed%edefd&d'ZdS)+N)TensorcCsF|jd}|dd|jdd}|||jd|dddS)a" https://github.com/pytorch/pytorch/issues/14489#issuecomment-607730242 :param matrix: Sparse or dense matrix, size (m, n). :param matrix_batch: Batched dense matrices, size (b, n, k). :return: The batched matrix-matrix product, size (m, n) x (b, n, k) = (b, m, k). r)shape transposereshapemm)matrix matrix_batch batch_sizevectorsr e/home/ubuntu/.local/lib/python3.10/site-packages/modelscope/models/cv/human3d_animation/transforms.pybatch_mm s rwxyzTcCs|jddd}|}d||dk<||}tj|jddd|j|jd}|d }|d krFt|d|d <t |||d ddf<n|d krat |||d ddf<t|d|d<|ru|d dk}||ddfd9<|S)a? Convert angle-axis representation to wxyz quaternion and to the half plan (w >= 0) @param rots: angle-axis rotations, (*, 3) @param form: quaternion format, either 'wxyz' or 'xyzw' @param unified_orient: Use unified orientation for quaternion (quaternion is dual cover of SO3) :return: rT)dimkeepdimr:0yE>Ndevicedtype?r.r.xyzw.rr) normclonetorchemptyrrrcossqueezesin)rotsformunified_orientanglesraxisquatsidxr r raa2quats$   r,cCsd|d}|dddf}|jdd}|}d||dk<||d}t||d}||dS) z^ Convert wxyz quaternions to angle-axis representation :param quats: :return: r.rNrrgHz>)rr unsqueezer atan2)r*_cosxyz_sinrr)angler r rquat2aa>s  r5r*cCs|d}|d}|d}|d}||}||}||}||}||} ||} ||} ||} ||} ||}||}||}tj|jddd|j|jd}d | ||d <| ||d <|| |d <| ||d <d |||d<| | |d<|| |d<| | |d<d || |d<|S)z Convert (w, x, y, z) quaternions to 3x3 rotation matrix :param quats: quaternions of shape (..., 4) :return: rotation matrices of shape (..., 3, 3) r.r.r.rNr)rrr?).rr).rr).rr.).rr).rr).rr.).r.r).r.r).r.r.)r r!rrr)r*qwqxqyqzx2y2z2xxyywxxyyzwyxzzzwzmr r rquat2matNs:      rJr2cCs|d}|d}|d}|d}tj|jd|j|jd}|dkrqtd|||||||||||||d<td||||d d |d<td|||||||||||||d<ntd ||r|d t j }|S) za Convert (w, x, y, z) quaternions to xyz euler angles. This is used for bvh output. rr6r7r)rrr2r.rrzCannot convert to ordering %s) r r!rrrr0asinclipNotImplementedErrornppi)qorderdegreesq0q1q2q3esr r r quat2eulerus"& rYcCst|}t|}|S)zv Convert angle-axis representation to rotation matrix :param rots: angle-axis representation :return: )r,rJ)r%quatmatr r raa2matsr\cCsRt|jddd}d|d<tj||fdd}t|}|dddddfS) z< Calculate the inverse of any affine transformation Nr.)rrrrr-.r)r zerosrcatinverse)r[affinevert_mat vert_mat_invr r r inv_affines  rccCs^|}|dddfdd|dddf<t|dddf|ddd |d<|S)z@ Calculate the inverse of a rigid affine transformation .Nrrr)rrr matmulr/r#)r[resr r rinv_rigid_affines $(rgxreturncCs(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)rhret positive_maskr r r_sqrt_positive_parts rnr c 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). rrrdzInvalid rotation matrix shape .N) r-r8rr.r6r7rg?rrg@).Nr) num_classesrr)size ValueErrorrr unbindrrnstacktensortorrmaxFone_hotargmax)r batch_dimm00m01m02m10m11m12m20m21m22q_abs quat_by_rijkflrquat_candidatesr r rmatrix_to_quaternionsZ  """"   r quaternionscCstj|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. .rNr.rT)prrgư>r0)r rr0abs empty_liker$)rnorms half_anglesr(eps small_anglessin_half_angles_over_anglesr r rquaternion_to_axis_angles  rcCs tt|S)a{ 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. )rr)r r r rmat2aa s rrrot_vecsepsilonc Cs(t|jdksJd|j|jd}|j}|j}tj||dddd}||}tjt|dd}tjt|dd}tj |ddd\} } } tj |ddf||d } tj |df||d } tj | | | | | | | | | g dd |ddf} tj d||d jdd}||| d|t| | }|S) a= Calculates the rotation matrices for a batch of rotation vectors Parameters ---------- rot_vecs: torch.tensor Nx3 array of N axis-angle vectors Returns ------- R: torch.tensor Nx3x3 The rotation matrices for the given axis-angle parameters r.z+Expects an array of size Bx3, but received rrT)rrrr-rrq)lenrrrr rr/r"r$splitr]r^vieweyebmm)rrr rrr4rot_dirr"r$rxryrzKr]identrot_matr r rbatch_rodriguess&  $  r)rT)r2T)r)numpyrOr torch.nn.functionalnn functionalrzrrr,r5rJrYr\rcrgrnrrrfloatrr r r rs"   '    2