o
    oi<                     @  s   d dl mZ d dlmZ d dlmZmZmZmZm	Z	m
Z
mZmZmZmZmZmZ d dlmZmZ d dlmZ d dlmZ d dlmZ d dlmZ G d	d
 d
eZdS )    )annotations)Optional)DeviceDtypeModule	ParameterTensorconcatenateeyepadstacktensorwhere
zeros_like)KORNIA_CHECKKORNIA_CHECK_SAME_DEVICES)So3)batched_dot_product)
Quaternion)Vector3c                      s  e Zd ZdZdX fdd	ZdYddZdZddZd[ddZd\ddZe	d]ddZ
e	d^ddZe	d]ddZe	d_dd Ze	d]d!d"Ze	d_d#d$Zed`d'd(Zdad)d*Zedbd+d,Zedcd.d/Zeddded7d8Zdad9d:Zedfd<d=Zedgd?d@ZdhdAdBZedddedCdDZedidFdGZedjdIdJZedkdLdMZedldNdOZedidPdQZ edjdRdSZ!edkdTdUZ"dadVdWZ#  Z$S )mSe3a  Base class to represent the Se3 group.

    The SE(3) is the group of rigid body transformations about the origin of three-dimensional Euclidean
    space :math:`R^3` under the operation of composition.
    See more: https://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf

    Example:
        >>> q = Quaternion.identity()
        >>> s = Se3(q, torch.ones(3))
        >>> s.r
        tensor([1., 0., 0., 0.])
        >>> s.t
        Parameter containing:
        tensor([1., 1., 1.], requires_grad=True)

    rotationQuaternion | So3translationVector3 | TensorreturnNonec                   s   t    t|ttfstdt| t|ttfs%tdt| |  |  t|tr4t	|| _
n|| _
t|trCt|| _dS || _dS )az  Construct the base class.

        Internally represented by a unit quaternion `q` and a translation 3-vector.

        Args:
            rotation: So3 group encompassing a rotation.
            translation: Vector3 or translation tensor with the shape of :math:`(B, 3)`.

        Example:
            >>> from kornia.geometry.quaternion import Quaternion
            >>> q = Quaternion.identity(batch_size=1)
            >>> s = Se3(q, torch.ones((1, 3)))
            >>> s.r
            tensor([[1., 0., 0., 0.]])
            >>> s.t
            Parameter containing:
            tensor([[1., 1., 1.]], requires_grad=True)

        zrotation type is ztranslation type is N)super__init__
isinstancer   r   	TypeErrortyper   r   r   _translation	_rotation)selfr   r   	__class__ P/home/ubuntu/.local/lib/python3.10/site-packages/kornia/geometry/liegroup/se3.pyr   ?   s   



zSe3.__init__strc                 C  s   d| j  d| j S )Nz
rotation: z
translation: )rtr$   r'   r'   r(   __repr__f   s   zSe3.__repr__idxint | slicec                 C  s   t | j| | j| S N)r   r#   r"   )r$   r.   r'   r'   r(   __getitem__i   s   zSe3.__getitem__rightc                 C  s(   | j |j  }| j| j |j  }t||S r0   )r*   r+   r   )r$   r2   _r_tr'   r'   r(   _mul_se3l   s   
zSe3._mul_se3Se3 | Vector3 | Tensorc                 C  sN   | j }| j}t|tr| |S t|ttfr|| |j S tdt	| )zCompose two Se3 transformations.

        Args:
            right: the other Se3 transformation.

        Return:
            The resulting Se3 transformation.

        zUnsupported type: )
so3r+   r   r   r5   r   r   datar    r!   )r$   r2   r7   r+   r'   r'   r(   __mul__q   s   


zSe3.__mul__r   c                 C     | j S z$Return the underlying rotation(So3).r#   r,   r'   r'   r(   r7         zSe3.so3r   c                 C  s   | j jS )z+Return the underlying rotation(Quaternion).)r#   qr,   r'   r'   r(   
quaternion   s   zSe3.quaternionc                 C  r:   r;   r<   r,   r'   r'   r(   r*      r=   zSe3.rc                 C  r:   z@Return the underlying translation vector of shape :math:`(B,3)`.r"   r,   r'   r'   r(   r+      r=   zSe3.tc                 C  r:   )z&Return the underlying `rotation(So3)`.r<   r,   r'   r'   r(   r      r=   zSe3.rotationc                 C  r:   r@   rA   r,   r'   r'   r(   r      r=   zSe3.translationvr   c           	      C  s   | dddf }| dddf }t |}|| }t|| }t |}td| j| jdd|  |d  d |  ||	  |d  d |  }t
|d d	k|ddddf | d
|}t||S )a  Convert elements of lie algebra to elements of lie group.

        Args:
            v: vector of shape :math:`(B, 6)`.

        Example:
            >>> v = torch.zeros((1, 6))
            >>> s = Se3.exp(v)
            >>> s.r
            tensor([[1., 0., 0., 0.]])
            >>> s.t
            Parameter containing:
            tensor([[0., 0., 0.]], requires_grad=True)

        .N   devicedtype      .NN.N        )r   hatr   sqrtexpr
   rE   rF   cossinr   sumr   )	rB   upsilonomega	omega_hatomega_hat_sqthetaRVUr'   r'   r(   rO      s   

,
zSe3.expc                 C  s   | j  }t||d }| jj}t|}|| }t	d|j
|jdd|  d||d   d|d     |d d |  }t|d d	k|d
dddf | d|}t||fdS )a  Convert elements of lie group  to elements of lie algebra.

        Example:
            >>> from kornia.geometry.quaternion import Quaternion
            >>> q = Quaternion.identity()
            >>> Se3(q, torch.zeros(3)).log()
            tensor([0., 0., 0., 0., 0., 0.])

        g-q=rC   rD   g      ?rG   rH   rI   rJ   rK   .NrL   )r*   logr   	clamp_minrN   r+   r8   r   rM   r
   rE   rF   rP   rQ   powr   rR   r	   )r$   rT   rW   r+   rU   rV   V_invr'   r'   r(   r[      s   


4,zSe3.logc                 C  sD   | dddf | dddf }}t t||d fd}t|dS )a  Convert elements from vector space to lie algebra.

        Args:
            v: vector of shape :math:`(B, 6)`.

        Returns:
            matrix of shape :math:`(B, 4, 4)`.

        Example:
            >>> v = torch.ones((1, 6))
            >>> m = Se3.hat(v)
            >>> m
            tensor([[[ 0., -1.,  1.,  1.],
                     [ 1.,  0., -1.,  1.],
                     [-1.,  1.,  0.,  1.],
                     [ 0.,  0.,  0.,  0.]]])

        .NrC   rJ   rL   r   r   r   rG   )r	   r   rM   r   )rB   rS   rT   rtr'   r'   r(   rM      s   "
zSe3.hatrT   c                 C  s<   | ddddf }t | dddddf }t||fdS )a  Convert elements from lie algebra to vector space.

        Args:
            omega: 4x4-matrix representing lie algebra of shape :math:`(B,4,4)`.

        Returns:
            vector of shape :math:`(B,6)`.

        Example:
            >>> v = torch.ones((1, 6))
            >>> omega_hat = Se3.hat(v)
            >>> Se3.vee(omega_hat)
            tensor([[1., 1., 1., 1., 1., 1.]])

        .NrC   rL   )r   veer	   )rT   headtailr'   r'   r(   ra      s   zSe3.veeN
batch_sizeOptional[int]rE   Optional[Device]rF   r   c                 C  s>   t g d||d}|dur||d}| t|||t|S )a  Create a Se3 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 = Se3.identity()
            >>> s.r
            tensor([1., 0., 0., 0.])
            >>> s.t
            x: 0.0
            y: 0.0
            z: 0.0

        )rK   rK   rK   rD   NrG   )r   repeatr   identityr   )clsrd   rE   rF   r+   r'   r'   r(   rh   
  s   zSe3.identityc                 C  s2   t | j | jjd fd}t|d}d|d< |S )aC  Return the matrix representation of shape :math:`(B, 4, 4)`.

        Example:
            >>> s = Se3(So3.identity(), torch.ones(3))
            >>> s.matrix()
            tensor([[1., 0., 0., 1.],
                    [0., 1., 0., 1.],
                    [0., 0., 1., 1.],
                    [0., 0., 0., 1.]])

        rJ   rL   r_   g      ?).rL   rL   )r	   r*   matrixr+   r8   r   )r$   r`   rt_4x4r'   r'   r(   rj   #  s   
z
Se3.matrixrj   c                 C  s8   t |dddddf }|ddddf }| ||S )aY  Create a Se3 group from a matrix.

        Args:
            matrix: tensor of shape :math:`(B, 4, 4)`.

        Example:
            >>> s = Se3.from_matrix(torch.eye(4))
            >>> s.r
            tensor([1., 0., 0., 0.])
            >>> s.t
            Parameter containing:
            tensor([0., 0., 0.], requires_grad=True)

        .NrC   rL   )r   from_matrix)ri   rj   r*   r+   r'   r'   r(   rl   4  s   
zSe3.from_matrixqxyzc                 C  s6   |dddf |dddf }}| t |t|S )a  Create a Se3 group a quaternion and translation vector.

        Args:
            qxyz: tensor of shape :math:`(B, 7)`.

        Example:
            >>> qxyz = torch.tensor([1., 2., 3., 0., 0., 0., 1.])
            >>> s = Se3.from_qxyz(qxyz)
            >>> s.r
            tensor([1., 2., 3., 0.])
            >>> s.t
            x: 0.0
            y: 0.0
            z: 1.0

        .N   )r   	from_wxyzr   )ri   rm   r>   xyzr'   r'   r(   	from_qxyzI  s   "zSe3.from_qxyzc                 C  s4   | j  }d| j }t|trtdt||| S )aK  Return the inverse transformation.

        Example:
            >>> s = Se3(So3.identity(), torch.ones(3))
            >>> s_inv = s.inverse()
            >>> s_inv.r
            tensor([1., -0., -0., -0.])
            >>> s_inv.t
            Parameter containing:
            tensor([-1., -1., -1.], requires_grad=True)

        rL   z*Unexpected integer from `-1 * translation`)r*   inverser+   r   intr    r   )r$   r_invr4   r'   r'   r(   rr   _  s
   


zSe3.inversec                 C  sJ   |du rd}nt |dkdd |f}t|||}t|||}| ||S )aK  Create a Se3 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 = Se3.random()
            >>> s = Se3.random(batch_size=3)

        Nr'   rG   zbatch_size must be positive)msg)r   r   randomr   )ri   rd   rE   rF   shaper*   r+   r'   r'   r(   rv   s  s   
z
Se3.randomxc                 C  $   t |}| t|t|||fdS )z_Construct a x-axis rotation.

        Args:
            x: the x-axis rotation angle.

        rL   )r   r   rot_xr   ri   rx   zsr'   r'   r(   rz        z	Se3.rot_xyc                 C  ry   )z_Construct a y-axis rotation.

        Args:
            y: the y-axis rotation angle.

        rL   )r   r   rot_yr   ri   r~   r|   r'   r'   r(   r     r}   z	Se3.rot_yzc                 C  ry   )z_Construct a z-axis rotation.

        Args:
            z: the z-axis rotation angle.

        rL   )r   r   rot_zr   ri   r   r|   r'   r'   r(   r     r}   z	Se3.rot_zc                 C  sr   t |j|jk t |j|jk t|||g t|jdkr#|jd nd}t||j|j}| |t|||fdS )zConstruct a translation only Se3 instance.

        Args:
            x: the x-axis translation.
            y: the y-axis translation.
            z: the z-axis translation.

        r   NrL   )	r   rw   r   lenr   rh   rE   rF   r   )ri   rx   r~   r   rd   r   r'   r'   r(   trans  s   
z	Se3.transc                 C  s   t |}| |||S )z_Construct a x-axis translation.

        Args:
            x: the x-axis translation.

        r   r   r{   r'   r'   r(   trans_x     zSe3.trans_xc                 C  s   t |}| |||S )z_Construct a y-axis translation.

        Args:
            y: the y-axis translation.

        r   r   r'   r'   r(   trans_y  r   zSe3.trans_yc                 C  s   t |}| |||S )z_Construct a z-axis translation.

        Args:
            z: the z-axis translation.

        r   r   r'   r'   r(   trans_z  r   zSe3.trans_zc                 C  sH   | j  }t|}t|t| j| fd}t||fd}t||fdS )a  Return the adjoint matrix of shape :math:`(B, 6, 6)`.

        Example:
            >>> s = Se3.identity()
            >>> s.adjoint()
            tensor([[1., 0., 0., 0., 0., 0.],
                    [0., 1., 0., 0., 0., 0.],
                    [0., 0., 1., 0., 0., 0.],
                    [0., 0., 0., 1., 0., 0.],
                    [0., 0., 0., 0., 1., 0.],
                    [0., 0., 0., 0., 0., 1.]])

        rL   )r7   rj   r   r	   r   rM   r+   )r$   rX   r   row0row1r'   r'   r(   adjoint  s
   
zSe3.adjoint)r   r   r   r   r   r   )r   r)   )r.   r/   r   r   )r2   r   r   r   )r2   r   r   r6   )r   r   )r   r   )r   r   )rB   r   r   r   )r   r   )rB   r   r   r   )rT   r   r   r   )NNN)rd   re   rE   rf   rF   r   r   r   )rj   r   r   r   )rm   r   r   r   )r   r   )rx   r   r   r   )r~   r   r   r   )r   r   r   r   )rx   r   r~   r   r   r   r   r   )%__name__
__module____qualname____doc__r   r-   r1   r5   r9   propertyr7   r?   r*   r+   r   r   staticmethodrO   r[   rM   ra   classmethodrh   rj   rl   rq   rr   rv   rz   r   r   r   r   r   r   r   __classcell__r'   r'   r%   r(   r   -   sf    
'











r   N)
__future__r   typingr   kornia.corer   r   r   r   r   r	   r
   r   r   r   r   r   kornia.core.checkr   r   kornia.geometry.liegroup.so3r   kornia.geometry.linalgr   kornia.geometry.quaternionr   kornia.geometry.vectorr   r   r'   r'   r'   r(   <module>   s   8