
    3j#,                        S SK Jr  S SKrS SKJrJr  S SKJrJr  / SQr	SS jr
SS jrSS jrSS	 jrSSS
 jjrSSS jjrSSS jjrSSS jjr\rg)    )annotationsN)KORNIA_CHECK_IS_TENSORKORNIA_CHECK_SHAPE)convert_points_from_homogeneousconvert_points_to_homogeneous)	batched_dot_productbatched_squared_normcompose_transformationseuclidean_distanceinverse_transformationpoint_line_distancerelative_transformationsquared_normtransform_pointsc                   [        U 5        [        U5        U R                  5       S;   a  U R                  SS S:X  d  [        SU R                   35      eUR                  5       S;   a  UR                  SS S:X  d  [        SUR                   35      eU R                  5       UR                  5       :w  a-  [        SU R                  5        SUR                  5        35      eU S	SS
2SS
24   nUS	SS
2SS
24   nU S	SS
2S
S24   nUS	SS
2S
S24   n[        R
                  " X#5      n[        R
                  " X%5      U-   nU R                  U R                  5      nXhS	SS
2SS
24'   XxS	SS
2S
S24'   SUS'   U$ )a(  Compose two homogeneous transformations.

.. math::
    T_0^{2} = \begin{bmatrix} R_0^1 R_1^{2} & R_0^{1} t_1^{2} + t_0^{1} \
    \\mathbf{0} & 1\end{bmatrix}

Args:
    trans_01: tensor with the homogeneous transformation from
      a reference frame 1 respect to a frame 0. The tensor has must have a
      shape of :math:`(N, 4, 4)` or :math:`(4, 4)`.
    trans_12: tensor with the homogeneous transformation from
      a reference frame 2 respect to a frame 1. The tensor has must have a
      shape of :math:`(N, 4, 4)` or :math:`(4, 4)`.

Returns:
    the transformation between the two frames with shape :math:`(N, 4, 4)` or :math:`(4, 4)`.

Example::
    >>> trans_01 = torch.eye(4)  # 4x4
    >>> trans_12 = torch.eye(4)  # 4x4
    >>> trans_02 = compose_transformations(trans_01, trans_12)  # 4x4

      N   r   z8Input trans_01 must be a of the shape Nx4x4 or 4x4. Got z8Input trans_12 must be a of the shape Nx4x4 or 4x4. Got %Input number of dims must match. Got  and .r         ?.r   r   )r   dimshape
ValueErrortorchmatmul	new_zeros)	trans_01trans_12rmat_01rmat_12tvec_01tvec_12rmat_02tvec_02trans_02s	            P/home/wildlama/miniconda3/lib/python3.13/site-packages/kornia/geometry/linalg.pyr
   r
   &   s   0 8$8$\\^v%HNN23,?6,IST\TbTbScdee\\^v%HNN23,?6,IST\TbTbScdee||~'@@PPUV^VbVbVdUefgg sBQB{#GsBQB{#GsBQB{#GsBQB{#G ll7,Gll7,w6G!!(..1H#S"1"bqb[#S"1"ab[HYO    c                   [        U 5        U R                  5       S;   a  U R                  SS S:X  d  [        SU R                   35      eU SSS2SS24   nU SSS2SS24   nUR	                  S	S5      n[
        R                  " U* U5      nU R                  U R                  5      nUSSS2SS24   R                  U5        USSS2SS24   R                  U5        S
US'   U$ )aP  Invert a 4x4 homogeneous transformation.

 :math:`T_1^{2} = \begin{bmatrix} R_1 & t_1 \\ \mathbf{0} & 1 \end{bmatrix}`

The inverse transformation is computed as follows:

.. math::

    T_2^{1} = (T_1^{2})^{-1} = \begin{bmatrix} R_1^T & -R_1^T t_1 \\
    \mathbf{0} & 1\end{bmatrix}

Args:
    trans_12: transformation tensor of shape :math:`(N, 4, 4)` or :math:`(4, 4)`.

Returns:
    tensor with inverted transformations with shape :math:`(N, 4, 4)` or :math:`(4, 4)`.

Example:
    >>> trans_12 = torch.rand(1, 4, 4)  # Nx4x4
    >>> trans_21 = inverse_transformation(trans_12)  # Nx4x4

r   r   Nr   z'Input size must be a Nx4x4 or 4x4. Got .r   r   r   r   )	r   r   r   r   	transposer   r    r!   copy_)r#   r%   r'   rmat_21tvec_21trans_21s         r+   r   r   [   s   . 8$\\^v%HNN23,?6,IB8>>BRSTTsBQB{#GsBQB!|$G B'GllG8W-G !!(..1HS"1"bqb[(S"1"ac\  )HYOr,   c                   [        U 5        [        U5        U R                  5       S;   a  U R                  SS S:X  d  [        SU R                   35      eUR                  5       S;   a  UR                  SS S:X  d  [        SUR                   35      eU R                  5       UR                  5       :X  d-  [        SU R                  5        SUR                  5        35      eU SSS	2SS	24   nU SSS	2S	S
24   nUSSS	2SS	24   nUSSS	2S	S
24   nUR	                  SS5      n[
        R                  " Xd5      n[
        R                  " XeU-
  5      n[
        R                  " U 5      n	XySSS	2SS	24'   XSSS	2S	S
24'   SU	S'   U	$ )a=  Compute the relative homogeneous transformation from a reference transformation.

:math:`T_1^{0} = \begin{bmatrix} R_1 & t_1 \\ \mathbf{0} & 1 \end{bmatrix}` to destination :math:`T_2^{0} =
\begin{bmatrix} R_2 & t_2 \\ \mathbf{0} & 1 \end{bmatrix}`.

The relative transformation is computed as follows:

.. math::

    T_1^{2} = (T_0^{1})^{-1} \cdot T_0^{2}

Args:
    trans_01: reference transformation tensor of shape :math:`(N, 4, 4)` or :math:`(4, 4)`.
    trans_02: destination transformation tensor of shape :math:`(N, 4, 4)` or :math:`(4, 4)`.

Returns:
    the relative transformation between the transformations with shape :math:`(N, 4, 4)` or :math:`(4, 4)`.

Example::
    >>> trans_01 = torch.eye(4)  # 4x4
    >>> trans_02 = torch.eye(4)  # 4x4
    >>> trans_12 = relative_transformation(trans_01, trans_02)  # 4x4

r   r   Nr   z/Input must be a of the shape Nx4x4 or 4x4. Got r   r   .r   r   r.   r   r   )r   r   r   r   r/   r   r    
zeros_like)
r"   r*   r$   r&   r(   r)   rmat_10r%   r'   r#   s
             r+   r   r      s   2 8$8$\\^v%HNN23,?6,IJ8>>JZ[\\\\^v%HNN23,?6,IJ8>>JZ[\\<<>X\\^+@@PPUV^VbVbVdUefggsBQB{#GsBQB!|$GsBQB{#GsBQB!|$GB'Gll7,Gll7g$56G)H#S"1"bqb[$S"1"ac\HYOr,   c                   [        U 5        [        U5        U R                  S   UR                  S   :X  d8  U R                  S   S:w  a%  [        SU R                   SUR                   35      eU R                  S   UR                  S   S-   :X  d  [        SU  SU 35      e[        UR                  5      nUR	                  SUR                  S   UR                  S   5      nU R	                  SU R                  S   U R                  S   5      n [
        R                  " U [        UR                  S   U R                  S   -  5      SS9n [        U5      nUR                  n[
        R                  " UR                  U R                  5      U R                  SS	S5      5      n[        U5      nUR                  S   US'   UR                  S   US'   UR	                  U5      nUR                  U5      $ )
a  Apply transformations to a set of points.

Args:
    trans_01: tensor for transformations of shape
      :math:`(B, D+1, D+1)`.
    points_1: tensor of points of shape :math:`(B, N, D)`.

Returns:
    a tensor of N-dimensional points.

Shape:
    - Output: :math:`(B, N, D)`

Examples:
    >>> points_1 = torch.rand(2, 4, 3)  # BxNx3
    >>> trans_01 = torch.eye(4).view(1, 4, 4)  # Bx4x4
    >>> points_0 = transform_points(trans_01, points_1)  # BxNx3

r      z=Input batch size must be the same for both tensors or 1. Got r   r.   z1Last input dimensions must differ by one unit Gotr   )repeatsr   r   )r   r   r   listreshaper   repeat_interleaveintr   dtypebmmtopermuter   )r"   points_1	shape_inp
points_1_hpoints_dtype
points_0_hpoints_0s          r+   r   r      s   ( 8$8$>>!q 11hnnQ6G16LKHNNK[[`aiaoao`pq
 	
 >>"(.."4q"89LXJV[\d[efgg X^^$IHNN2$6r8JKHHNN2$6r8JKH &&xX^^A=NRZR`R`abRc=c9djklH.x8J ##L:==8(:J:J1aQR:STJ.z:HNN2&IbMNN2&IbM	*H;;|$$r,   c                   [        U 5        [        U5        U R                  S   S;  a  [        SU R                   35      eUR                  S   S:w  a  [        SUR                   35      eUS   U S   -  nX1S   U S   -  -  nX1S   -  nUR                  5         US   R	                  5       US   R	                  5       -   R                  5       nX4U-   -  $ )	aB  Return the distance from points to lines.

Args:
   point: (possibly homogeneous) points :math:`(*, N, 2 or 3)`.
   line: lines coefficients :math:`(a, b, c)` with shape :math:`(*, N, 3)`, where :math:`ax + by + c = 0`.
   eps: Small constant for safe sqrt.

Returns:
    the computed distance with shape :math:`(*, N)`.

r.   r   z&pts must be a (*, 2 or 3) tensor. Got r   z#lines must be a (*, 3) tensor. Got ).r   ).r8   ).r   )r   r   r   abs_squaresqrt)pointlineeps	numerator
denom_norms        r+   r   r      s     5!4 {{2f$A%++OPPzz"~>tzzlKLL VuV},Iff--IfINN v,%%'$v,*=*=*??EEGJS())r,   c                b    [        U SS/5        [        USS/5        X-  R                  SU5      $ )z#Return a batched version of .dot().*Nr.   )r   sum)xykeepdims      r+   r   r     s1    q3*%q3*%E;;r7##r,   c                    [        X U5      $ )z$Return the squared norm of a vector.)r   )rU   rW   s     r+   r	   r	     s    qW--r,   c                    [        U SS/5        [        USS/5        X-
  R                  S5      R                  SUS9R                  U5      R	                  5       $ )a_  Compute the Euclidean distance between two set of n-dimensional points.

More: https://en.wikipedia.org/wiki/Euclidean_distance

Args:
    x: first set of points of shape :math:`(*, N)`.
    y: second set of points of shape :math:`(*, N)`.
    keepdim: whether to keep the dimension after reduction.
    eps: small value to have numerical stability.

rR   rS   r   r.   )r   rW   )r   powrT   add_sqrt_)rU   rV   rW   rN   s       r+   r   r     sT     q3*%q3*%E;;q>"g6;;C@FFHHr,   )r"   torch.Tensorr#   r]   returnr]   )r#   r]   r^   r]   )r"   r]   r*   r]   r^   r]   )r"   r]   rB   r]   r^   r]   )g&.>)rL   r]   rM   r]   rN   floatr^   r]   )F)rU   r]   rV   r]   rW   boolr^   r]   )rU   r]   rW   r`   r^   r]   )Fgư>)
rU   r]   rV   r]   rW   r`   rN   r_   r^   r]   )
__future__r   r   kornia.core.checkr   r   kornia.geometry.conversionsr   r   __all__r
   r   r   r   r   r   r	   r   r    r,   r+   <module>rf      sO   $ #  H f
2j(V.b2%j*B$.
I& $r,   