o oiZ@sddlmZddlmZmZmZmZddlZddlm Z m Z m Z m Z m Z mZmZmZmZmZddlmZddlmZmZmZmZmZmZmZddlmZGdd d e Zdd d d eej d d fd dZ dS))pi)AnyOptionalTupleUnionN) DeviceDtypeModule ParameterTensor concatenaterandstacktensorwhere)KORNIA_CHECK_TYPE)axis_angle_to_quaternioneuler_from_quaternionnormalize_quaternionquaternion_from_eulerquaternion_to_axis_anglequaternion_to_rotation_matrixrotation_matrix_to_quaternion)batched_dot_productc seZdZUdZeeefed<deeefddffdd Zde d e ddfd d Z d eee fddfd dZ de fddZdeeefddfddZduddZdedee fddfddZdedee fddfddZdedee fddfddZdeee fddfddZdeede fddfd d!Zdeede fddfd"d#Zdeee fddfd$d%Zdeee fddfd&d'Zdeee fddfd(d)Zdeee fddfd*d+Zd,e ddfd-d.Zedefd/d0Zede eeeeffd1d2Z!edefd3d4Z"edefd5d6Z#edefd7d8Z$edefd9d:Z%edefd;d<Z&edefd=d>Z'edefd?d@Z(edefdAdBZ)ede edCffdDdEZ*edefdFdGZ+defdHdIZ,e-dJeddfdKdLZ.e-dMedNedOeddfdPdQZ/de eeeffdRdSZ0e-dTeddfdUdVZ1defdWdXZ2e- dvdYe3edZe3e4d[e5ddfd\d]Z6e-d^e d_e d`e dae ddf dbdcZ7e- dvdYe3edZe3e4d[e5ddfdddeZ8dfdd,e ddfdgdhZ9dwdje:defdkdlZ;dudmdnZdefdsdtZ?Z@S)x QuaternionaBase class to represent a Quaternion. A quaternion is a four dimensional vector representation of a rotation transformation in 3d. See more: https://en.wikipedia.org/wiki/Quaternion The general definition of a quaternion is given by: .. math:: Q = a + b \cdot \mathbf{i} + c \cdot \mathbf{j} + d \cdot \mathbf{k} Thus, we represent a rotation quaternion as a contiguous tensor structure to perform rigid bodies transformations: .. math:: Q = \begin{bmatrix} q_w & q_x & q_y & q_z \end{bmatrix} Example: >>> q = Quaternion.identity(batch_size=4) >>> q.data tensor([[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.]]) >>> q.real tensor([1., 1., 1., 1.]) >>> q.vec tensor([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.], [0., 0., 0.]]) _datadatareturnNcs4tt|ttfstdt|||_dS)a"Construct a quaternion from tensor or parameter data. Args: data: tensor or parameter containing the quaternion data with the shape of :math:`(B, 4)`. Example: >>> # Create with tensor (no gradients tracked by default) >>> data = torch.tensor([1., 0., 0., 0.]) >>> q1 = Quaternion(data) >>> # Create with parameter (gradients tracked) >>> param_data = torch.nn.Parameter(torch.tensor([1., 0., 0., 0.])) >>> q2 = Quaternion(param_data) z"Expected Tensor or Parameter, got N)super__init__ isinstancer r TypeErrortyper)selfr __class__N/home/ubuntu/.local/lib/python3.10/site-packages/kornia/geometry/quaternion.pyrOs  zQuaternion.__init__argskwargscOst|jj|i|S)zMove and/or cast the quaternion data. Args: *args: Arguments to pass to tensor.to() **kwargs: Keyword arguments to pass to tensor.to() Returns: A new Quaternion with converted data. )rrto)r#r(r)r&r&r'r*ds z Quaternion.tovaluec Cst|ttfrtj||jj|jjd}nt|tjr%|j |jj|jjd}z t |j j |j }Wnt yJ}ztd|j j d|j |d}~ww|j |||}|dkrb||}nt||j }t|djg|j dR}tj|d|gdd}t|S) aMConvert a scalar, tensor, or numeric value to a scalar quaternion. A scalar quaternion has the form [real, 0, 0, 0] where real is the input value. Args: value: The scalar, tensor, or numeric value to convert. Returns: A Quaternion object representing the scalar quaternion. devicedtypezCannot broadcast shapes z and Nrdim)r intfloattorchrrr-r.r r*broadcast_shapesrealshape RuntimeError ValueErrorexpandr2 expand_as broadcast_to zeros_like unsqueezecatr)r#r+ target_shapee broadcastedexpanded_valuezerosscalar_quat_datar&r&r'_to_scalar_quaternionps"    "z Quaternion._to_scalar_quaternioncCs|jSNrr#r&r&r'__repr__szQuaternion.__repr__idxcCst|j|SrHrr)r#rLr&r&r' __getitem__szQuaternion.__getitem__cCs t|j S)zInverts the sign of the quaternion data. Example: >>> q = Quaternion.identity() >>> -q.data tensor([-1., -0., -0., -0.]) rMrJr&r&r'__neg__s zQuaternion.__neg__rightcCs4t|tr t|j|jS||}t|j|jS)aQAdd a given quaternion, scalar, or tensor. Args: right: the quaternion, scalar, or tensor to add. Example: >>> q1 = Quaternion.identity() >>> q2 = Quaternion(tensor([2., 0., 1., 1.])) >>> q3 = q1 + q2 >>> q3.data tensor([3., 0., 1., 1.]) )r rrrG)r#rP right_quatr&r&r'__add__s  zQuaternion.__add__cCsHt|tr t|j|jS||}|j|j}t|tr |j}t|S)a[Subtract a given quaternion, scalar, or tensor. Args: right: the quaternion, scalar, or tensor to subtract. Example: >>> q1 = Quaternion(tensor([2., 0., 1., 1.])) >>> q2 = Quaternion.identity() >>> q3 = q1 - q2 >>> q3.data tensor([1., 0., 1., 1.]) )r rrrGr )r#rPrQ result_datar&r&r'__sub__s    zQuaternion.__sub__cCst|tr8|j|jt|j|j}|jd|j|jd|jtjj|j|jdd}tt|d|fdS| |}|j|jt|j|j}|jd|j|jd|jtjj|j|jdd}tt|d|fdS)N.Nr/r1) r rr7rvecr5linalgcrossr rG)r#rPnew_realnew_vecrQr&r&r'__mul__s$  zQuaternion.__mul__leftcCsp||}|j|jt|j|j}|jd|j|jd|jtjj|j|jdd}tt|d|fdS)zDRight multiplication (left * self) where left is a scalar or tensor.rUr/r1) rGr7rrVr5rWrXrr )r#r\ left_quatrYrZr&r&r'__rmul__s zQuaternion.__rmul__cCst|tr ||St|ttfrtj||jj|jj d}n |j |jj|jj d}| dkr@| |jd d |j}n | d |j}|j|}t|trV|j}t|S)Nr,r.rr/)r rinvr3r4r5rrr-r.r*r2r<r?r )r#rP right_tensordivisorrSr&r&r'__div__s      zQuaternion.__div__cC ||SrH)rc)r#rPr&r&r' __truediv__ s zQuaternion.__truediv__cCs||}||S)z>Right addition (left + self) where left is a scalar or tensor.rGr#r\r]r&r&r'__radd__ zQuaternion.__radd__cCs||}||S)zARight subtraction (left - self) where left is a scalar or tensor.rfrgr&r&r'__rsub__rizQuaternion.__rsub__cCs||}||Sz>Right division (left / self) where left is a scalar or tensor.rfrgr&r&r' __rtruediv__rizQuaternion.__rtruediv__cCrdrk)rl)r#r\r&r&r'__rdiv__s zQuaternion.__rdiv__tcCsd|jd}|jjddd}t|dk|j||jd}||}|||}tt||fdS)zReturn the power of a quaternion raised to exponent t. Args: t: raised exponent. Example: >>> q = Quaternion(tensor([1., .5, 0., 0.])) >>> q_pow = q**2 rUr/T)r2keepdimr) polar_anglerVnormrcossinrr )r#rnthetavec_normnwxyzr&r&r'__pow__#s  zQuaternion.__pow__cC|jS)z5Return the underlying data with shape :math:`(B, 4)`.)rrJr&r&r'r5szQuaternion.datacCs|j|j|j|jfS)z>Return a tuple with the underlying coefficients in WXYZ order.)rwxyzrJr&r&r'coeffs:szQuaternion.coeffscCrz)zReturn the real part with shape :math:`(B,)`. Alias for :func: `~kornia.geometry.quaternion.Quaternion.w` )rwrJr&r&r'r7?zQuaternion.realcCs|jdddfS)zDReturn the vector with the imaginary part with shape :math:`(B, 3)`..NrIrJr&r&r'rVHszQuaternion.veccCrz)zReturn the underlying data with shape :math:`(B, 4)`. Alias for :func:`~kornia.geometry.quaternion.Quaternion.data` rIrJr&r&r'qMsz Quaternion.qcCrz)zReturn a scalar with the real with shape :math:`(B,)`. Alias for :func: `~kornia.geometry.quaternion.Quaternion.w` )r7rJr&r&r'scalarUrzQuaternion.scalarcC |jdS)z/Return the :math:`q_w` with shape :math:`(B,)`.r_rIrJr&r&r'rw^ z Quaternion.wcCr)z/Return the :math:`q_x` with shape :math:`(B,)`.).rrIrJr&r&r'r{crz Quaternion.xcCr)z/Return the :math:`q_y` with shape :math:`(B,)`.).rIrJr&r&r'r|hrz Quaternion.ycCr)z/Return the :math:`q_z` with shape :math:`(B,)`.).r0rIrJr&r&r'r}mrz Quaternion.z.cCs t|jjS)zBReturn the shape of the underlying data with shape :math:`(B, 4)`.)tuplerr8rJr&r&r'r8rs zQuaternion.shapecCs|j|S)zReturn the polar angle with shape :math:`(B,1)`. Example: >>> q = Quaternion.identity() >>> q.polar_angle tensor(0.) )rrqacosrJr&r&r'rpws zQuaternion.polar_anglecC t|jS)a%Convert the quaternion to a rotation matrix of shape :math:`(B, 3, 3)`. Example: >>> q = Quaternion.identity() >>> m = q.matrix() >>> m tensor([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) )rrrJr&r&r'matrixs zQuaternion.matrixrcC |t|S)a8Create a quaternion from a rotation matrix. Args: matrix: the rotation matrix to convert of shape :math:`(B, 3, 3)`. Example: >>> m = torch.eye(3)[None] >>> q = Quaternion.from_matrix(m) >>> q.data tensor([[1., 0., 0., 0.]]) )r)clsrr&r&r' from_matrix zQuaternion.from_matrixrollpitchyawc Cs0t|||d\}}}}t||||fd}||S)aCreate a quaternion from Euler angles. Args: roll: the roll euler angle. pitch: the pitch euler angle. yaw: the yaw euler angle. Example: >>> roll, pitch, yaw = tensor(0), tensor(1), tensor(0) >>> q = Quaternion.from_euler(roll, pitch, yaw) >>> q.data tensor([0.8776, 0.0000, 0.4794, 0.0000]) )rrrr/)rr) rrrrrwr{r|r}rr&r&r' from_eulerszQuaternion.from_eulercCst|j|j|j|jS)a[Convert the quaternion to a triple of Euler angles (roll, pitch, yaw). Example: >>> q = Quaternion(tensor([2., 0., 1., 1.])) >>> roll, pitch, yaw = q.to_euler() >>> roll tensor(2.0344) >>> pitch tensor(1.5708) >>> yaw tensor(2.2143) )rrwr{r|r}rJr&r&r'to_eulerszQuaternion.to_euler axis_anglecCr)abCreate a quaternion from axis-angle representation. Args: axis_angle: rotation vector of shape :math:`(B, 3)`. Example: >>> axis_angle = torch.tensor([[1., 0., 0.]]) >>> q = Quaternion.from_axis_angle(axis_angle) >>> q.data tensor([[0.8776, 0.4794, 0.0000, 0.0000]]) )r)rrr&r&r'from_axis_anglerzQuaternion.from_axis_anglecCr)zConvert the quaternion to an axis-angle representation. Example: >>> q = Quaternion.identity() >>> axis_angle = q.to_axis_angle() >>> axis_angle tensor([0., 0., 0.]) )rrrJr&r&r' to_axis_angles zQuaternion.to_axis_angle batch_sizer-r.cCs.tgd||d}|dur||d}||S)aaCreate a quaternion representing an identity rotation. Args: batch_size: the batch size of the underlying data. device: device to place the result on. dtype: dtype of the result. Example: >>> q = Quaternion.identity() >>> q.data tensor([1., 0., 0., 0.]) )?rrr,Nr)rrepeat)rrr-r.rr&r&r'identitys zQuaternion.identityrwr{r|r}cCs|t||||gS)aCreate a quaternion from the data coefficients. Args: w: a float representing the :math:`q_w` component. x: a float representing the :math:`q_x` component. y: a float representing the :math:`q_y` component. z: a float representing the :math:`q_z` component. Example: >>> q = Quaternion.from_coeffs(1., 0., 0., 0.) >>> q.data tensor([1., 0., 0., 0.]) )r)rrwr{r|r}r&r&r' from_coeffsszQuaternion.from_coeffsc Cs|dur|fnd}tdg|R||d\}}}d|dt|}d|dt|} |dt|} |dt|} |t|| | | fdS)aCreate a random unit quaternion of shape :math:`(B, 4)`. Uniformly distributed across the rotation space as per: http://planning.cs.uiuc.edu/node198.html Args: batch_size: the batch size of the underlying data. device: device to place the result on. dtype: dtype of the result. Example: >>> q = Quaternion.random() >>> q = Quaternion.random(batch_size=2) Nr&r0r,rrr/)r sqrtrrsrrr) rrr-r. rand_shaper1r2r3q1q2q3q4r&r&r'random szQuaternion.randomrcCs.t|t|}|}||||S)aReturn a unit quaternion spherically interpolated between quaternions self.q and q1. See more: https://en.wikipedia.org/wiki/Slerp Args: q1: second quaternion to be interpolated between. t: interpolation ratio, range [0-1] Example: >>> q0 = Quaternion.identity() >>> q1 = Quaternion(torch.tensor([1., .5, 0., 0.])) >>> q2 = q0.slerp(q1, .3) )rr normalizer`)r#rrnq0r&r&r'slerp&s zQuaternion.slerpFrocCs|jdd|S)a8Compute the norm (magnitude) of the quaternion. Args: keepdim: whether to retain the last dimension. Returns: The norm of the quaternion(s) as a tensor. Example: >>> q = Quaternion.identity() >>> q.norm() tensor(1.) rr/)rrq)r#ror&r&r'rq:szQuaternion.normcCstt|jS)zReturn a normalized (unit) quaternion. Returns: The normalized quaternion. Example: >>> q = Quaternion(tensor([2., 1., 0., 0.])) >>> q_norm = q.normalize() )rrrrJr&r&r'rLs zQuaternion.normalizecCstt|jd|j fdS)zCompute the conjugate of the quaternion. Returns: The conjugate quaternion, with the vector part negated. Example: >>> q = Quaternion(tensor([1., 2., 3., 4.])) >>> q_conj = q.conj() rUr/)rr r7rVrJr&r&r'conjYs zQuaternion.conjcCs||S)zCompute the inverse of the quaternion. Returns: The inverse quaternion. Example: >>> q = Quaternion.identity() >>> q_inv = q.inv() )r squared_normrJr&r&r'r`fs zQuaternion.invcCst|j|j|jdS)aCompute the squared norm (magnitude) of the quaternion. Returns: The squared norm of the quaternion(s) as a tensor. Example: >>> q = Quaternion.identity() >>> q.squared_norm() tensor(1.) r)rrVr7rJr&r&r'rss zQuaternion.squared_norm)rr)NNN)F)A__name__ __module__ __qualname____doc__rr r __annotations__rrr*r4rGstrrKr3slicerNrOrRrTr[r^rcrerhrjrlrmrypropertyrrr~r7rVrrrwr{r|r}r8rpr classmethodrrrrrrrrrrrrboolrqrrr`r __classcell__r&r&r$r'r)s # (           rQrwrcCs|j}t|t|jd}|dur|j||}n+|j|j|jd}||kr4t d|d||| }|jt ||}t j |\}}|ddt |f}||}t|dS)aWCompute (weighted) average of multiple quaternions. Args: Q (Quaternion): quaternion object containing data of shape (M, 4). w (torch.Tensor, optional): Weights of shape (M,). If None, uniform weights are used. Returns: Quaternion: averaged quaternion (shape (4,)), wrapped back in the Quaternion class. rN)r.zweights length z" must match number of quaternions )rrrr8Tr*r-r.numelr:sumr5diagrWeighargmaxrqr?)rrwrMA eigenvalues eigenvectorsq_avgr&r&r'average_quaternionss     rrH)!mathrtypingrrrrr5 kornia.corerrr r r r r rrrkornia.core.checkrkornia.geometry.conversionsrrrrrrrkornia.geometry.linalgrrrr&r&r&r's 0 $ "]