
    3j!                    f    S SK Jr  S SKrS SKrS SKJrJrJrJr  S SK	J
r
  SS	S jjr " S S5      rg)
    )annotationsN)Se2Se3So2So3)
Quaternionc                B   / nUS:X  a	  SS/SS//nOUS:X  a  SS/SS//n[        U R                  5      S:  d5  [        U R                  5      S:  d  [        U R                  SS 5      U;  a2  [        U SUS	    S
US	    SUS    S
US    SU R                   35      eg)z"Verify matrix shape based on type.R      RT   Nz must be either r   xz or                  z, got )lenshapelist
ValueError)matrixmatrix_typetarget_shapess      N/home/wildlama/miniconda3/lib/python3.13/site-packages/kornia/geometry/pose.pycheck_matrix_shaper      s    McQ!Q(		Q!Q(
6<<1FLL 1A 5fll23>O9PXe9em+M!,<+=Q}Q?O>P QQ -"2!36&,,I
 	
 :f    c                     \ rS rSrSrSSS jjrSS jrSS jr\SS j5       r	\SS j5       r
\SS	 j5       r\SS
 j5       r\SS j5       r\  S         SS jj5       r\ S       SS jj5       rSS jrSS jrSrg)	NamedPose*   ap  Class to represent a named pose between two frames.

Internally represented by either Se2 or Se3.

Example:
    >>> b_from_a = NamedPose(Se3.identity(), frame_src="frame_a", frame_dst="frame_b")
    >>> b_from_a
    NamedPose(dst_from_src=rotation: tensor([1., 0., 0., 0.])
    translation: x: 0.0
    y: 0.0
    z: 0.0,
    frame_src: frame_a -> frame_dst: frame_b)

Nc                    Xl         U=(       d    [        R                  " 5       R                  U l        U=(       d    [        R                  " 5       R                  U l        g)zConstruct NamedPose.

Args:
    dst_from_src: Pose from source frame to destination frame.
    frame_src: Name of frame a.
    frame_dst: Name of frame b.

N)_dst_from_srcuuiduuid4hex
_frame_src
_frame_dst)selfdst_from_src	frame_src	frame_dsts       r   __init__NamedPose.__init__:   s:     *#7tzz|'7'7#7tzz|'7'7r   c                V    SU R                    SU R                   SU R                   S3$ )NzNamedPose(dst_from_src=z,
frame_src: z -> frame_dst: ))r    r$   r%   r&   s    r   __repr__NamedPose.__repr__G   s7    %d&8&8%9 ://*/$//9J!M	
r   c                &   U R                   UR                  :w  a  [        SU  SU 35      e[        UR                  [
        5      (       aD  [        U R                  R                  UR                  5      UR                   U R                  5      $ [        UR                  [        5      (       aD  [        U R                  R                  UR                  5      UR                   U R                  5      $ [        S[        U R                  5       35      e)a  Compose two NamedPoses.

Args:
    other: NamedPose to compose with.

Returns:
    Composed NamedPose.

Example:
    >>> b_from_a = NamedPose(Se3.identity(), frame_src="frame_a", frame_dst="frame_b")
    >>> c_from_b = NamedPose(Se3.identity(), frame_src="frame_b", frame_dst="frame_c")
    >>> c_from_b * b_from_a
    NamedPose(dst_from_src=rotation: tensor([1., 0., 0., 0.])
    translation: x: 0.0
    y: 0.0
    z: 0.0,
    frame_src: frame_a -> frame_dst: frame_c)

zCannot compose z with z$Pose must be either Se2 or Se3, got )r$   r%   r   
isinstanceposer   r   r    _mul_se2r   _mul_se3type)r&   others     r   __mul__NamedPose.__mul__M   s    ( ??e...tfF5'BCCejj#&&T//88DeFVFVX\XgXghh

C((T//88DeFVFVX\XgXghhCDI[I[D\C]^__r   c                    U R                   $ )z-Pose from source frame to destination frame .r    r.   s    r   r3   NamedPose.posej   s     !!!r   c                .    U R                   R                  $ )zRotation part of the pose.)r    rotationr.   s    r   r>   NamedPose.rotationo   s     !!***r   c                .    U R                   R                  $ )zTranslation part of the pose.)r    translationr.   s    r   rA   NamedPose.translationt   s     !!---r   c                    U R                   $ )zName of the source frame.)r$   r.   s    r   r(   NamedPose.frame_srcy        r   c                    U R                   $ )zName of the destination frame.)r%   r.   s    r   r)   NamedPose.frame_dst~   rE   r   c                \   [        U[        [        45      (       a  U " [        X5      X45      $ [        U[        5      (       a  U " [        X5      X45      $ [        U[        R                  5      (       a  [        U5        UR                  S   n[        R                  " US-   UR                  UR                  S9nXSSU2SU24'   X&SSU2U4'   US:X  a  U " [
        R                  " U5      X45      $ US:X  a  U " [        R                  " U5      X45      $  g[        S[        U5       35      e)	a  Construct NamedPose from rotation and translation.

Args:
    rotation: Rotation part of the pose.
    translation: Translation part of the pose.
    frame_src: Name of the source frame.
    frame_dst: Name of the destination frame.

Returns:
    NamedPose constructed from rotation and translation.

Example:
    >>> b_from_a_rot = So3.identity()
    >>> b_from_a_trans = torch.tensor([1., 2., 3.])
    >>> b_from_a = NamedPose.from_rt(b_from_a_rot, b_from_a_trans, frame_src="frame_a", frame_dst="frame_b")
    >>> b_from_a
    NamedPose(dst_from_src=rotation: tensor([1., 0., 0., 0.])
    translation: Parameter containing:
    tensor([1., 2., 3.], requires_grad=True),
    frame_src: frame_a -> frame_dst: frame_b)

r   )devicedtype.Nr   r   z6R must be either So2, So3, Quaternion, or Tensor, got )r2   r   r   r   r   r   torchTensorr   r   eyerJ   rK   from_matrixr   r6   )clsr>   rA   r(   r)   dimr   s          r   from_rtNamedPose.from_rt   s   < hj 122s819HH#&&s819HH%,,//x(..$C378??(..QB"*sDSD$3$!,sDSD#~ax3??2.	EE3??2.	EE   UVZ[cVdUefggr   c                    [        USS9  UR                  S   nUS:X  a  U " [        R                  " U5      X#5      $ US:X  a  U " [        R                  " U5      X#5      $ g)aV  Construct NamedPose from a matrix.

Args:
    matrix: Matrix representation of the pose.
    frame_src: Name of the source frame.
    frame_dst: Name of the destination frame.

Returns:
    NamedPose constructed from a matrix.

Example:
    >>> b_from_a_matrix = Se3.identity().matrix()
    >>> b_from_a = NamedPose.from_matrix(b_from_a_matrix, frame_src="frame_a", frame_dst="frame_b")
    >>> b_from_a
    NamedPose(dst_from_src=rotation: tensor([1., 0., 0., 0.])
    translation: Parameter containing:
    tensor([0., 0., 0.], requires_grad=True),
    frame_src: frame_a -> frame_dst: frame_b)

r   )r   rI   r   r   N)r   r   r   rO   r   )rP   r   r(   r)   rQ   s        r   rO   NamedPose.from_matrix   sY    0 	6t4ll2!8sv.	EEAXsv.	EEr   c                t    [        U R                  R                  5       U R                  U R                  5      $ )a]  Inverse of the NamedPose.

Returns:
    Inverse of the NamedPose.

Example:
    >>> b_from_a = NamedPose(Se3.identity(), frame_src="frame_a", frame_dst="frame_b")
    >>> b_from_a.inverse()
    NamedPose(dst_from_src=rotation: tensor([1., -0., -0., -0.])
    translation: x: 0.0
    y: 0.0
    z: 0.0,
    frame_src: frame_b -> frame_dst: frame_a)

)r   r    inverser%   r$   r.   s    r   rW   NamedPose.inverse   s*      ++335tXXr   c                     U R                   U-  $ )aO  Transform points from source frame to destination frame.

Args:
    points_in_src: Points in source frame.

Returns:
    Points in destination frame.

Example:
    >>> b_from_a = NamedPose(Se3.identity(), frame_src="frame_a", frame_dst="frame_b")
    >>> b_from_a.transform_points(torch.tensor([1., 2., 3.]))
    tensor([1., 2., 3.])

r;   )r&   points_in_srcs     r   transform_pointsNamedPose.transform_points   s     !!M11r   )r    r%   r$   )NN)r'   	Se2 | Se3r(   
str | Noner)   r^   returnNone)r_   str)r7   r   r_   r   )r_   r]   )r_   z	So3 | So2)r_   torch.Tensor)
r>   z%So3 | So2 | torch.Tensor | QuaternionrA   rb   r(   r^   r)   r^   r_   NamedPose | None)r   rb   r(   r^   r)   r^   r_   rc   )r_   r   )rZ   rb   r_   rb   )__name__
__module____qualname____firstlineno____doc__r*   r/   r8   propertyr3   r>   rA   r(   r)   classmethodrR   rO   rW   r[   __static_attributes__ r   r   r   r   *   s
   8
`: " " + + . .     
 !% $-7- "- 	-
 - 
- -^ Y]!.8LV	 >Y$2r   r   )r
   )r   rb   r   ra   r_   r`   )
__future__r   r!   rL   kornia.geometry.liegroupr   r   r   r   kornia.geometry.quaternionr   r   r   rl   r   r   <module>rp      s*   $ #   7 7 1
J2 J2r   