o oig5@sddlmZddlmZddlZddlmZmZmZm Z m Z m Z m Z m Z mZmZddlmZddlmZddlmZddlmZdd lmZGd d d eZdS) ) annotations)OptionalN) DeviceDtypeModuleTensor concatenateeyestackwherezeros zeros_like)KORNIA_CHECK_TYPE)vector_to_skew_symmetric_matrix)batched_dot_product Quaternion)Vector3csBeZdZdZdIfdd ZdJd d ZdKd dZdLddZedMddZ e dNddZ dOddZ e dPddZ e dQddZdOd d!ZedRd#d$ZedSd&d'Ze (dTdUd/d0ZdVd1d2Ze (dTdUd3d4ZedWd6d7ZedXd9d:ZedYdd?Ze dZdAdBZe dZdCdDZe dZdEdFZe dZdGdHZZS)[So3aBase class to represent the So3 group. The SO(3) is the group of all rotations about the origin of three-dimensional Euclidean space :math:`R^3` under the operation of composition. See more: https://en.wikipedia.org/wiki/3D_rotation_group We internally represent the rotation by a unit quaternion. Example: >>> q = Quaternion.identity() >>> s = So3(q) >>> s.q tensor([1., 0., 0., 0.]) qrreturnNonecstt|t||_dS)amConstruct the base class. Internally represented by a unit quaternion `q`. Args: q: Quaternion with the shape of :math:`(B, 4)`. Example: >>> data = torch.ones((2, 4)) >>> q = Quaternion(data) >>> So3(q) tensor([[1., 1., 1., 1.], [1., 1., 1., 1.]]) N)super__init__rr_q)selfr __class__P/home/ubuntu/.local/lib/python3.10/site-packages/kornia/geometry/liegroup/so3.pyr3s   z So3.__init__strcCs|jSN)rrrrr__repr__Gsz So3.__repr__idx int | slicecCst|j|Sr!)rr)rr$rrr __getitem__JszSo3.__getitem__rightcCst|tr t|j|jSt|ttfrOtg|jdddR|j|jd}t t ||j fd}|j||j j }t|trD|St|trMt|SdStdt|)zCompose two So3 transformations. Args: right: the other So3 transformation. Return: The resulting So3 transformation. NdevicedtypezNot So3 or Tensor type. Got: ) isinstancerrrrr shaper+r,rrdataconjvec TypeErrortype)rr'wquatoutrrr__mul__Ms (  z So3.__mul__cCs|jS)z4Return the underlying data with shape :math:`(B,4)`.)rr"rrrrfszSo3.qvrc Cs|jddd}d|}t|}t|jjd}||k}t||}d||d}t|||}||} tj|| fdd} t t | S)aConvert elements of lie algebra to elements of lie group. See more: https://vision.in.tum.de/_media/members/demmeln/nurlanov2021so3log.pdf Args: v: vector of shape :math:`(B,3)`. Example: >>> v = torch.zeros((2, 3)) >>> s = So3.exp(v) >>> s tensor([[1., 0., 0., 0.], [1., 0., 0., 0.]]) r(Tdimkeepdimg?g@@gH@)r:) normtorchcosfinfor,epssinr catrr) r8theta theta_halfr4r@ small_maskb_largeb_smallbxyzrrrrexpks  zSo3.expcCsbt|jj|jj}t|ddkd|jjd|jj|dd|jj|jjd}|S)aConvert elements of lie group to elements of lie algebra. Example: >>> data = torch.ones((2, 4)) >>> q = Quaternion(data) >>> So3(q).log() tensor([[0., 0., 0.], [0., 0., 0.]]) .Nr)rrr1sqrtr realacos)rrComegarrrlogs  "zSo3.logVector3 | TensorcCst|tr|d|d|d}}}n |j|j|j}}}t|}t|| |fd}t||| fd}t| ||fd}t|||fdS)aConvert elements from vector space to lie algebra. Returns matrix of shape :math:`(B,3,3)`. Args: v: Vector3 or tensor of shape :math:`(B,3)`. Example: >>> v = torch.ones((1,3)) >>> m = So3.hat(v) >>> m tensor([[[ 0., -1., 1.], [ 1., 0., -1.], [-1., 1., 0.]]]) ).r).r)).rLr()r-rxyzr r )r8arHcrVrow0row1row2rrrhats zSo3.hatrPcCs,|d|d|d}}}t|||fdS)aConvert elements from lie algebra to vector space. Returns vector of shape :math:`(B,3)`. .. math:: omega = \begin{bmatrix} 0 & -c & b \\ c & 0 & -a \\ -b & a & 0\end{bmatrix} Args: omega: 3x3-matrix representing lie algebra. Example: >>> v = torch.ones((1,3)) >>> omega = So3.hat(v) >>> So3.vee(omega) tensor([[1., 1., 1.]]) ).rLr)).rrL).r)rr()r )rPrWrHrXrrrveeszSo3.veec CsX|jjd}|jjd|jjd|jjd}}}dd|dd|d}d||d||}d||d||}t|||fd}d||d||}dd|dd|d}d||d||}t|||fd} d||d||}d||d||}dd|dd|d}t|||fd} t|| | fdS)aConvert the quaternion to a rotation matrix of shape :math:`(B,3,3)`. The matrix is of the form: .. math:: \begin{bmatrix} 1-2y^2-2z^2 & 2xy-2zw & 2xy+2yw \\ 2xy+2zw & 1-2x^2-2z^2 & 2yz-2xw \\ 2xz-2yw & 2yz+2xw & 1-2x^2-2y^2\end{bmatrix} Example: >>> s = So3.identity() >>> m = s.matrix() >>> m tensor([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) rKr)rLr(rS)rr4rTrUrVrr ) rr4rTrUrVq0q1q2rYrZr[rrrmatrixs (z So3.matrixracCs|t|S)aCreate So3 from a rotation matrix. Args: matrix: the rotation matrix to convert of shape :math:`(B,3,3)`. Example: >>> m = torch.eye(3) >>> s = So3.from_matrix(m) >>> s tensor([1., 0., 0., 0.]) )r from_matrix)clsrarrrrbszSo3.from_matrixwxyzcCs |t|S)a1Create So3 from a tensor representing a quaternion. Args: wxyz: the quaternion to convert of shape :math:`(B,4)`. Example: >>> q = torch.tensor([1., 0., 0., 0.]) >>> s = So3.from_wxyz(q) >>> s tensor([1., 0., 0., 0.]) r)rcrdrrr from_wxyzs z So3.from_wxyzN batch_size Optional[int]r+Optional[Device]r,Optional[Dtype]cC|t|||S)aCreate a So3 group 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: >>> s = So3.identity() >>> s tensor([1., 0., 0., 0.]) >>> s = So3.identity(batch_size=2) >>> s tensor([[1., 0., 0., 0.], [1., 0., 0., 0.]]) )ridentityrcrfr+r,rrrrksz So3.identitycCst|jS)zReturn the inverse transformation. Example: >>> s = So3.identity() >>> s.inverse() tensor([1., -0., -0., -0.]) )rrr0r"rrrinverse,s z So3.inversecCrj)aECreate a So3 group representing a random rotation. Args: batch_size: the batch size of the underlying data. device: device to place the result on. dtype: dtype of the result. Example: >>> s = So3.random() >>> s = So3.random(batch_size=3) )rrandomrlrrrrn7sz So3.randomrTcCst|}|t|||fdS)z_Construct a x-axis rotation. Args: x: the x-axis rotation angle. r(r rJr )rcrTzsrrrrot_xIz So3.rot_xrUcCst|}|t|||fdS)z_Construct a z-axis rotation. Args: y: the y-axis rotation angle. r(ro)rcrUrprrrrot_yTrrz So3.rot_yrVcCst|}|t|||fdS)z_Construct a z-axis rotation. Args: z: the z-axis rotation angle. r(ro)rcrVrprrrrot_z_rrz So3.rot_zcCs|S)zReturn the adjoint matrix of shape :math:`(B, 3, 3)`. Example: >>> s = So3.identity() >>> s.adjoint() tensor([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) )rar"rrradjointjs z So3.adjointr1cCsht|}|jdddd}td|j|jd}|d||d||||d||}|S) atCompute the right Jacobian of So3. Args: vec: the input point of shape :math:`(B, 3)`. Example: >>> vec = torch.tensor([1., 2., 3.]) >>> So3.right_jacobian(vec) tensor([[-0.0687, 0.5556, -0.0141], [-0.2267, 0.1779, 0.6236], [ 0.5074, 0.3629, 0.5890]]) r(Tr9rKr*r)rLrr<r r+r,r>rA)r1R_skewrCIJrrrrright_jacobianw 8zSo3.right_jacobiancC t|S)zlAlias for right jacobian. Args: vec: the input point of shape :math:`(B, 3)`. )rr{r1rrrrz zSo3.JrcCsht|}|jdddd}td|j|jd}|d||d||||d||}|S) arCompute the left Jacobian of So3. Args: vec: the input point of shape :math:`(B, 3)`. Example: >>> vec = torch.tensor([1., 2., 3.]) >>> So3.left_jacobian(vec) tensor([[-0.0687, -0.2267, 0.5074], [ 0.5556, 0.1779, 0.3629], [-0.0141, 0.6236, 0.5890]]) r(Tr9rKrvr*r)rLrw)r1rxrCryJlrrr left_jacobianr|zSo3.left_jacobiancCr})zkAlias for left jacobian. Args: vec: the input point of shape :math:`(B, 3)`. )rrr~rrrrrzSo3.Jl)rrrr)rr )r$r%rr)r'rrr)rr)r8rrr)rr)r8rRrr)rPrrr)rarrr)rdrrr)NNN)rfrgr+rhr,rirr)rr)rTrrr)rUrrr)rVrrr)r1rrr)__name__ __module__ __qualname____doc__rr#r&r7propertyr staticmethodrJrQr\r]ra classmethodrbrerkrmrnrqrsrtrur{rzrr __classcell__rrrrr"sT         #           r) __future__rtypingrr= kornia.corerrrrrr r r r r kornia.core.checkrkornia.geometry.conversionsrkornia.geometry.linalgrkornia.geometry.quaternionrkornia.geometry.vectorrrrrrrs  0