o oiG5@sddlmZddlmZmZddlmZmZmZm Z m Z m Z m Z m Z mZmZmZmZddlmZmZmZddlmZmZmZddlmZddlmZdddZGdddeZdS)) annotations)Optionaloverload) DeviceDtypeModule ParameterTensor concatenatepadrandstacktensorwhere zeros_like) KORNIA_CHECKKORNIA_CHECK_SAME_DEVICESKORNIA_CHECK_TYPE)check_se2_omega_shapecheck_se2_t_shape check_v_shape)So2)Vector2rrtr returnNonecCsb|jj}t|dkrt|jdkst|dkr$t|jdkr$t|dStd|jjd|j)NrzKInvalid input, both the inputs should be either batched or unbatched. Got: z and )zshapelenr ValueError)rrz_shaper$P/home/ubuntu/.local/lib/python3.10/site-packages/kornia/geometry/liegroup/se2.py_check_se2_r_t_shape,s 4 r&csVeZdZdZdKfdd ZdLd d ZdMddZdNddZedNddZ edOddZ dPddZ e dQddZ e dQddZ e dRdd Z e dQd!d"Ze dRd#d$ZedSd&d'ZdTd(d)ZedUd*d+ZedVd-d.ZedWdXd6d7ZdTd8d9ZedYd;d<ZdZd=d>ZedWdXd?d@Zed[dCdDZed\dEdFZed]dGdHZdTdIdJZZS)^Se2a Base class to represent the Se2 group. The SE(2) is the group of rigid body transformations about the origin of two-dimensional Euclidean space :math:`R^2` under the operation of composition. See more: Example: >>> so2 = So2.identity(1) >>> t = torch.ones((1, 2)) >>> se2 = Se2(so2, t) >>> se2 rotation: Parameter containing: tensor([1.+0.j], requires_grad=True) translation: Parameter containing: tensor([[1., 1.]], requires_grad=True) rotationr translationVector2 | Tensorrrcsjtt|tt|ttfstdt||||_ t|tr0t ||t ||_ dS||_ dS)aaConstruct the base class. Internally represented by a complex number `z` and a translation 2-vector. Args: rotation: So2 group encompassing a rotation. translation: translation vector with the shape of :math:`(B, 2)`. Example: >>> so2 = So2.identity(1) >>> t = torch.ones((1, 2)) >>> se2 = Se2(so2, t) >>> se2 rotation: Parameter containing: tensor([1.+0.j], requires_grad=True) translation: Parameter containing: tensor([[1., 1.]], requires_grad=True) ztranslation type is N) super__init__rr isinstancerr TypeErrortype _rotationr&r _translation)selfr(r) __class__r$r%r,Is     z Se2.__init__strcCsd|jd|jS)Nz rotation: z translation: )rrr2r$r$r%__repr__ksz Se2.__repr__idx int | slicecCst|j||j|SN)r'r0r1)r2r8r$r$r% __getitem__nszSe2.__getitem__rightcCs.|j}|j}||j}|||j}t||Sr:)so2rr')r2r<r=r_r_tr$r$r%_mul_se2qs   z Se2._mul_se2cCdSr:r$r2r<r$r$r%__mul__xz Se2.__mul__r cCrAr:r$rBr$r$r%rC{rD Se2 | TensorcCsV|j}|j}t|trt|t||St|ttfr"|||Stdt |)zCompose two Se2 transformations. Args: right: the other Se2 transformation. Return: The resulting Se2 transformation. zUnsupported type: ) r=rr-r'rr@rr r.r/)r2r<r=rr$r$r%rC~s     cC|jSz&Return the underlying `rotation(So2)`.r0r6r$r$r%r=zSe2.so2cCrFrGrHr6r$r$r%rrIzSe2.rVector2 | ParametercCrFz@Return the underlying translation vector of shape :math:`(B,2)`.r1r6r$r$r%rrIzSe2.tcCrFrGrHr6r$r$r%r(rIz Se2.rotationcCrFrKrLr6r$r$r%r)rIzSe2.translationvc Cst||d}t|}td|j|jd}|dk}t||jj||}t|d|jj ||}|d}|d}t ||||||||fd} t || S)aConvert elements of lie algebra to elements of lie group. Args: v: vector of shape :math:`(B, 3)`. Example: >>> v = torch.ones((1, 3)) >>> s = Se2.exp(v) >>> s.r Parameter containing: tensor([0.5403+0.8415j], requires_grad=True) >>> s.t Parameter containing: tensor([[0.3818, 1.3012]], requires_grad=True) .rdevicedtype?.r.r) rrexprrQrRrrimagrealr r') rMthetar=rtheta_nonzerosabxyrr$r$r%rWs & zSe2.expc Cs|j}d|}|jjjd}t|dk||jjj |td|j|jd}t ||fd}t | |fd}t ||fd}||j j d}t |d |d |fdS) zConvert elements of lie group to elements of lie algebra. Example: >>> v = torch.ones((1, 3)) >>> s = Se2.exp(v).log() >>> s tensor([[1.0000, 1.0000, 1.0000]], grad_fn=) g?rrrOrPrV.N).rr).rr) r=logrrYrrXrrQrRr rdata) r2rZ half_thetadenomr\row0row1V_invupsilonr$r$r%rbs (zSe2.logcCsJt|t|d|dfd}|d}tt||dfd}t|dS)aQConvert elements from vector space to lie algebra. Returns matrix of shape :math:`(B, 3, 3)`. Args: v: vector of shape:math:`(B, 3)`. Example: >>> theta = torch.tensor(3.1415/2) >>> So2.hat(theta) tensor([[0.0000, 1.5707], [1.5707, 0.0000]]) rTrUrVrNr`)rr)rr r rhat unsqueezer )rMrirZcol0r$r$r%rjs  zSe2.hatomegacCsHt||ddddf}t|dddddf}t||dfdS)a{Convert elements from lie algebra to vector space. Args: omega: 3x3-matrix representing lie algebra of shape :math:`(B, 3, 3)`. Returns: vector of shape :math:`(B, 3)`. Example: >>> v = torch.ones(3) >>> omega_hat = Se2.hat(v) >>> Se2.vee(omega_hat) tensor([1., 1., 1.]) .rNrarV)rrveer )rmrirZr$r$r%rnszSe2.veeN batch_size Optional[int]rQOptional[Device]rRrcCsNtddg||d}|durt|dkdd||d}|t|||t|S)aCreate a Se2 group representing an identity rotation and zero translation. Args: batch_size: the batch size of the underlying data. device: device to place the result on. dtype: dtype of the result. Example: >>> s = Se2.identity(1) >>> s.r Parameter containing: tensor([1.+0.j], requires_grad=True) >>> s.t x: tensor([0.]) y: tensor([0.]) rOrPNrbatch_size must be positivemsg)rrrepeatridentityr)clsrorQrRrr$r$r%rv s  z Se2.identitycCs2t|j|jjdfd}t|d}d|d<|S)a0Return the matrix representation of shape :math:`(B, 3, 3)`. Example: >>> s = Se2(So2.identity(1), torch.ones(1, 2)) >>> s.matrix() tensor([[[1., -0., 1.], [0., 1., 1.], [0., 0., 1.]]], grad_fn=) rarV)rrrrrS).rVrV)r rmatrixrrcr )r2rtrt_3x3r$r$r%rx&s z Se2.matrixrxcCs8t|dddddf}|ddddf}|||S)aCreate an Se2 group from a matrix. Args: matrix: tensor of shape :math:`(B, 3, 3)`. Example: >>> s = Se2.from_matrix(torch.eye(3).repeat(2, 1, 1)) >>> s.r Parameter containing: tensor([1.+0.j, 1.+0.j], requires_grad=True) >>> s.t Parameter containing: tensor([[0., 0.], [0., 0.]], requires_grad=True) .NrrV)r from_matrix)rwrxrrr$r$r%r{6s zSe2.from_matrixcCs4|j}d|j}t|trtdt|||S)avReturn the inverse transformation. Example: >>> s = Se2(So2.identity(1), torch.ones(1,2)) >>> s_inv = s.inverse() >>> s_inv.r Parameter containing: tensor([1.+0.j], requires_grad=True) >>> s_inv.t Parameter containing: tensor([[-1., -1.]], requires_grad=True) rVz*Unexpected integer from `-1 * translation`)rinverserr-intr.r')r2r_invr?r$r$r%r|Ms   z Se2.inversecCsLt|||}|durd}n t|dkdd|df}||tt|||dS)aKCreate a Se2 group representing a random transformation. Args: batch_size: the batch size of the underlying data. device: device to place the result on. dtype: dtype of the result. Example: >>> s = Se2.random() >>> s = Se2.random(batch_size=3) N)rrrrrsrrP)rrandomrrr )rwrorQrRrr r$r$r%rbs z Se2.randomr^r_cCs^t|j|jkt||gt|jdkr|jdnd}t||j|j}||t||fdS)zConstruct a translation only Se2 instance. Args: x: the x-axis translation. y: the y-axis translation. rNrV) rr rr!rrvrQrRr )rwr^r_ror(r$r$r%transys  z Se2.transcCst|}|||S)z_Construct a x-axis translation. Args: x: the x-axis translation. rr)rwr^zsr$r$r%trans_x z Se2.trans_xcCst|}|||S)z_Construct a y-axis translation. Args: y: the y-axis translation. r)rwr_rr$r$r%trans_yrz Se2.trans_ycCs:|}t|jjd|jjd fd|ddddf<|S)aReturn the adjoint matrix of shape :math:`(B, 3, 3)`. Example: >>> s = Se2.identity() >>> s.adjoint() tensor([[1., -0., 0.], [0., 1., -0.], [0., 0., 1.]], grad_fn=) rUrTrV.rr)rxr rrc)r2ryr$r$r%adjoints .z Se2.adjoint)r(rr)r*rr)rr5)r8r9rr')r<r'rr')r<r rr )r<rErrE)rr)rrJ)rMr rr')rr )rMr rr )rmr rr )NNN)rorprQrqrRrrr')rxr rr')rr')r^r r_r rr')r^r rr')r_r rr') __name__ __module__ __qualname____doc__r,r7r;r@rrCpropertyr=rrr(r) staticmethodrWrbrjrn classmethodrvrxr{r|rrrrr __classcell__r$r$r3r%r'6sV "                     r'N)rrrr rr) __future__rtypingrr kornia.corerrrrr r r r r rrrkornia.core.checkrrrkornia.geometry.liegroup._utilsrrrkornia.geometry.liegroup.so2rkornia.geometry.vectorrr&r'r$r$r$r%s 8