
    3juk              	          S SK JrJrJr  S SKrS SKJr  S SKJr  S SK	J
r
Jr  S SKJrJr   " S S5      r " S	 S
\5      rSS\R"                  S\S\R"                  4S jjrSS\R"                  S\S\R"                  4S jjrS\R"                  S\R"                  S\R"                  4S jrS\R"                  S\R"                  4S jrS\R"                  S\R"                  S\R"                  4S jrS\R"                  S\R"                  S\R"                  S\R"                  4S jrSS\R"                  S\R"                  S\S\R"                  4S jjrg)     )IterableListUnionNKORNIA_CHECK_SAME_DEVICE)_torch_inverse_cast)convert_points_from_homogeneousconvert_points_to_homogeneous)inverse_transformationtransform_pointsc                      \ rS rSrSrS\R                  S\R                  S\R                  S\R                  SS4
S	 jr\S
\	\R                     S\
4S j5       r\S\R                  S\S\
4S j5       r\S\R                  S\S\
4S j5       r\S
\\R                     SS4S j5       rS\R"                  4S jr\S\R                  4S j5       r\S\R                  4S j5       r\S\4S j5       r\S\R                  4S j5       r\S\R                  4S j5       r\S\R                  4S j5       r\S\R                  4S j5       r\S\R                  4S j5       r\R8                  S\\R                  \4   SS4S j5       r\S\R                  4S j5       r\R8                  S\\R                  \4   SS4S j5       r\S\R                  4S j5       r \ R8                  S\\R                  \4   SS4S j5       r \S\R                  4S  j5       r!\S\R                  4S! j5       r"\S\R                  4S" j5       r#\S\R                  4S# j5       r$S:S$ jr%S\R                  4S% jr&S&\R                  SS 4S' jr'S&\\\R                  4   SS 4S( jr(S)\R                  S\R                  4S* jr)S+\R                  S,\R                  S\R                  4S- jr*\+S.\R                  S/\R                  S0\R                  S1\R                  S\S\S2\R                  S3\R                  S4\R                  S5\S6\\\R"                  S4   S7\RX                  SS 4S8 j5       r-S9r.g);PinholeCamera   aQ  Class that represents a Pinhole Camera model.

Args:
    intrinsics: torch.Tensor with shape :math:`(B, 4, 4)`
      containing the full 4x4 camera calibration matrix.
    extrinsics: torch.Tensor with shape :math:`(B, 4, 4)`
      containing the full 4x4 rotation-translation matrix.
    height: torch.Tensor with shape :math:`(B)` containing the image height.
    width: torch.Tensor with shape :math:`(B)` containing the image width.

.. note::
    We assume that the class attributes are in batch form in order to take
    advantage of PyTorch parallelism to boost computing performance.


intrinsics
extrinsicsheightwidthreturnNc                    U R                  XX4/5        U R                  US5        U R                  US5        U R                  US5        U R                  US5        U R                  XX4/5        X0l        X@l        Xl        X l        g )Nr   r   r   r   )_check_valid_check_valid_params_check_valid_shape_check_consistent_devicer   r   _intrinsics_extrinsics)selfr   r   r   r   s        X/home/wildlama/miniconda3/lib/python3.13/site-packages/kornia/geometry/camera/pinhole.py__init__PinholeCamera.__init__-   s     	:6AB  \:  \:1w/%%zv&MN$*#(
)3)3    	data_iterc                 H    [        S U  5       5      (       d  [        S5      eg)Nc              3   >   #    U  H  oR                   S    v   M     g7f)r   N)shape).0datas     r   	<genexpr>-PinholeCamera._check_valid.<locals>.<genexpr>?   s     7YT::a=Ys   zArguments shapes must matchT)all
ValueError)r!   s    r   r   PinholeCamera._check_valid=   s"    7Y777:;;r    r&   	data_namec                     [        U R                  5      S;  a.  U R                  SS  S:w  a  [        SU SU R                   35      eg)N)      r/   r/   	Argument z< shape must be in the following shape Bx4x4 or BxNx4x4. Got Tlenr$   r*   r&   r,   s     r   r   !PinholeCamera._check_valid_paramsC   sN    tzz?&(TZZ_-FI;&bcgcmcmbno  r    c                 l    [        U R                  5      S:X  d  [        SU SU R                   35      eg)N   r2   z- shape must be in the following shape B. Got Tr3   r5   s     r   r    PinholeCamera._check_valid_shapeK   s6    4::!#y3`aeakak`lmnnr    c                 6    U S   nU  H  n[        X!5        M     g )Nr   r   )r!   firstr&   s      r   r   &PinholeCamera._check_consistent_deviceQ   s    !D$T1 r    c                 .    U R                   R                  $ )zYReturn the device for camera buffers.

Returns:
    Union[str, torch.device, None] type

)r   devicer   s    r   r>   PinholeCamera.deviceW   s     &&&r    c                 h    U R                  U R                  S5      (       d  [        eU R                  $ )zXThe full 4x4 intrinsics matrix.

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

r   )r   r   AssertionErrorr?   s    r   r   PinholeCamera.intrinsics`   /     ''(8(8,GG  r    c                 h    U R                  U R                  S5      (       d  [        eU R                  $ )zXThe full 4x4 extrinsics matrix.

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

r   )r   r   rB   r?   s    r   r   PinholeCamera.extrinsicsl   rD   r    c                 4    U R                   R                  S   $ )zQReturn the batch size of the storage.

Returns:
    scalar with the batch size.

r   )r   r$   r?   s    r   
batch_sizePinholeCamera.batch_sizex   s     $$Q''r    c                      U R                   S   $ )z^Return the focal length in the x-direction.

Returns:
    torch.Tensor of shape :math:`(B)`.

.r   r   r   r?   s    r   fxPinholeCamera.fx        y))r    c                      U R                   S   $ )z^Return the focal length in the y-direction.

Returns:
    torch.Tensor of shape :math:`(B)`.

.r8   r8   rL   r?   s    r   fyPinholeCamera.fy   rO   r    c                      U R                   S   $ )zbReturn the x-coordinate of the principal point.

Returns:
    torch.Tensor of shape :math:`(B)`.

.r      rL   r?   s    r   cxPinholeCamera.cx   rO   r    c                      U R                   S   $ )zbReturn the y-coordinate of the principal point.

Returns:
    torch.Tensor of shape :math:`(B)`.

.r8   rV   rL   r?   s    r   cyPinholeCamera.cy   rO   r    c                      U R                   S   $ )zeReturn the x-coordinate of the translation vector.

Returns:
    torch.Tensor of shape :math:`(B)`.

.r   r   r?   s    r   txPinholeCamera.tx        z**r    valuec                      XR                   S'   g)zDSet the x-coordinate of the translation vector with the given value.r^   Nr`   r   rd   s     r   ra   rb           ',
#r    c                      U R                   S   $ )zeReturn the y-coordinate of the translation vector.

Returns:
    torch.Tensor of shape :math:`(B)`.

.r8   r_   r`   r?   s    r   tyPinholeCamera.ty   rc   r    c                      XR                   S'   g)DSet the y-coordinate of the translation vector with the given value.ri   Nr`   rf   s     r   rj   rk      rg   r    c                      U R                   S   $ )zfReturns the z-coordinate of the translation vector.

Returns:
    torch.Tensor of shape :math:`(B)`.

.rV   r_   r`   r?   s    r   tzPinholeCamera.tz   rc   r    c                      XR                   S'   g)rm   ro   Nr`   rf   s     r   rp   rq      rg   r    c                 .    U R                   SSS2SS24   $ )zdReturn the 3x4 rotation-translation matrix.

Returns:
    torch.Tensor of shape :math:`(B, 3, 4)`.

.Nr.   r/   r`   r?   s    r   	rt_matrixPinholeCamera.rt_matrix   !     sBQB{++r    c                 .    U R                   SSS2SS24   $ )zpReturn the 3x3 camera matrix containing the intrinsics.

Returns:
    torch.Tensor of shape :math:`(B, 3, 3)`.

.Nr.   rL   r?   s    r   camera_matrixPinholeCamera.camera_matrix   rv   r    c                 .    U R                   SSS2SS24   $ )zlReturn the 3x3 rotation matrix from the extrinsics.

Returns:
    torch.Tensor of shape :math:`(B, 3, 3)`.

.Nr.   r`   r?   s    r   rotation_matrixPinholeCamera.rotation_matrix   rv   r    c                 .    U R                   SSS2SS24   $ )zkReturn the translation vector from the extrinsics.

Returns:
    torch.Tensor of shape :math:`(B, 3, 1)`.

.Nr.   r_   r`   r?   s    r   translation_vector PinholeCamera.translation_vector   s     sBQB|,,r    c                     U R                   R                  5       nU R                  R                  5       nU R                  R                  5       nU R                  R                  5       n[        X4X5      $ )z2Return a deep copy of the current object instance.)r   cloner   r   r   r   )r   r   r   r   r   s        r   r   PinholeCamera.clone   sW    #{{002"jj..0#'??#8#8#:
#'??#8#8#:
ZVCCr    c                 6    U R                   R                  5       $ )ziReturn the inverse of the 4x4 instrisics matrix.

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

)r   inverser?   s    r   intrinsics_inverse PinholeCamera.intrinsics_inverse  s     &&((r    scale_factorc                 :   U R                   R                  5       nUS==   U-  ss'   US==   U-  ss'   US==   U-  ss'   US==   U-  ss'   XR                  R                  5       -  nXR                  R                  5       -  n[	        X R
                  X45      $ )zScale the pinhole model.

Args:
    scale_factor: a torch.Tensor with the scale factor. It has
      to be broadcastable with class members. The expected shape is
      :math:`(B)` or :math:`(1)`.

Returns:
    the camera model with scaled parameters.

rK   rQ   rU   rZ   )r   r   r   r   r   r   )r   r   r   r   r   s        r   scalePinholeCamera.scale  s     $(??#8#8#:
9-9-9-9-+kk.?.?.AA*ZZ-=-=-??Z&HHr    c                    U R                   S==   U-  ss'   U R                   S==   U-  ss'   U R                   S==   U-  ss'   U R                   S==   U-  ss'   U =R                  U-  sl        U =R                  U-  sl        U $ )a  Scale the pinhole model in-place.

Args:
    scale_factor: a torch.Tensor with the scale factor. It has
      to be broadcastable with class members. The expected shape is
      :math:`(B)` or :math:`(1)`.

Returns:
    the camera model with scaled parameters.

rK   rQ   rU   rZ   )r   r   r   )r   r   s     r   scale_PinholeCamera.scale_'  su     		"l2"	"l2"	"l2"	"l2"|#

l"
r    point_3dc                 \    U R                   U R                  -  n[        [        X!5      5      $ )ag  Project a 3d point in world coordinates onto the 2d camera plane.

Args:
    point_3d: torch.Tensor containing the 3d points to be projected
        to the camera plane. The shape of the torch.Tensor can be :math:`(*, 3)`.

Returns:
    torch.Tensor of (u, v) cam coordinates with shape :math:`(*, 2)`.

Example:
    >>> _ = torch.manual_seed(0)
    >>> X = torch.rand(1, 3)
    >>> K = torch.eye(4)[None]
    >>> E = torch.eye(4)[None]
    >>> h = torch.ones(1)
    >>> w = torch.ones(1)
    >>> pinhole = kornia.geometry.camera.PinholeCamera(K, E, h, w)
    >>> pinhole.project(X)
    tensor([[5.6088, 8.6827]])

)r   r   r	   r   )r   r   Ps      r   projectPinholeCamera.project=  s'    , OOdoo-./?/LMMr    point_2ddepthc                 z    U R                   U R                  -  n[        U5      n[        U[	        U5      U-  5      $ )a  Unproject a 2d point in 3d.

Transform coordinates in the pixel frame to the world frame.

Args:
    point_2d: torch.Tensor containing the 2d to be projected to
        world coordinates. The shape of the torch.Tensor can be :math:`(*, 2)`.
    depth: torch.Tensor containing the depth value of each 2d
        points. The torch.Tensor shape must be equal to point2d :math:`(*, 1)`.
    normalize: whether to F.normalize the pointcloud. This
        must be set to `True` when the depth is represented as the Euclidean
        ray length from the camera position.

Returns:
    torch.Tensor of (x, y, z) world coordinates with shape :math:`(*, 3)`.

Example:
    >>> _ = torch.manual_seed(0)
    >>> x = torch.rand(1, 2)
    >>> depth = torch.ones(1, 1)
    >>> K = torch.eye(4)[None]
    >>> E = torch.eye(4)[None]
    >>> h = torch.ones(1)
    >>> w = torch.ones(1)
    >>> pinhole = kornia.geometry.camera.PinholeCamera(K, E, h, w)
    >>> pinhole.unproject(x, depth)
    tensor([[0.4963, 0.7682, 1.0000]])

)r   r   r   r   r
   )r   r   r   r   P_invs        r   	unprojectPinholeCamera.unprojectV  s9    < OOdoo-#A&'DX'NQV'VWWr    rM   rR   rW   r[   ra   rj   rp   rH   r>   dtypec                    [         R                  " U
SSXS9nUS==   U-  ss'   US==   U-  ss'   US==   U-  ss'   US==   U-  ss'   US==   S-  ss'   US	==   S-  ss'   [         R                  " SXS9R                  U
S
S
5      nUS==   U-  ss'   US==   U-  ss'   US==   U	-  ss'   [         R                  " XUS9nUS==   U-  ss'   [         R                  " XUS9nUS==   U-  ss'   U " XUU5      $ )Nr/   r>   r   rK   rQ   rU   rZ   ).rV   rV         ?).r.   r.   r8   r^   ri   ro   .r   )torchzeroseyerepeat)r   rM   rR   rW   r[   r   r   ra   rj   rp   rH   r>   r   r   r   
height_tmp	width_tmps                    r   from_parametersPinholeCamera.from_parametersy  s   " [[Q&N
9#9#9#9#9$9$YYq=DDZQRTUV
:"$:"$:"$[[%H
6f$KK
G	&U"JJ	BBr    r   r   r   r   )r   r   )/__name__
__module____qualname____firstlineno____doc__r   Tensorr   staticmethodr   boolr   strr   r   r   r   r>   propertyr   r   intrH   rM   rR   rW   r[   ra   setterr   floatrj   rp   rt   rx   r{   r~   r   r   r   r   r   r   classmethodr   r   __static_attributes__ r    r   r   r      s    4,,449LL4JO,,4_d_k_k4	4   6 4  
 %,, 3 4    # $  
 2D,> 24 2 2
' ' 	 ELL 	  	  	 ELL 	  	  (C ( ( *ELL * * *ELL * * *ELL * * *ELL * * +ELL + + YY,ellE12 ,t , , +ELL + + YY,ellE12 ,t , , +ELL + + YY,ellE12 ,t , , ,5<< , , ,u|| , , , , , -ELL - -D)ELL )I%,, I? I.5)<#= / ,N N N2 X%,,  Xu||  X  XF !CLL!C LL!C LL	!C
 LL!C !C !C LL!C LL!C LL!C !C c5<<-.!C {{!C 
!C !Cr    r   c                   r    \ rS rSrSrS\\   SS4S jrS\\   SS 4S jr\	S\
4S	 j5       rS
\
S\4S jrSrg)PinholeCamerasListi  aQ  Class that represents a list of pinhole cameras.

The class inherits from :class:`~kornia.PinholeCamera` meaning that
it will keep the same class properties but with an extra dimension.

.. note::
    The underlying data torch.Tensor will be stacked in the first dimension.
    That's it, given a list of two camera instances, the intrinsics torch.Tensor
    will have a shape :math:`(B, N, 4, 4)` where :math:`B` is the batch
    size and :math:`N` is the numbers of cameras (in this case two).

Args:
    pinholes_list: a python tuple or list containing a set of `PinholeCamera` instances.

pinholes_listr   Nc                 &    U R                  U5        g )N)_initialize_parameters)r   r   s     r   r   PinholeCamerasList.__init__  s    ##M2r    pinholesc                    [        U[        [        45      (       d  [        S[	        U5       35      e/ / p2/ / pTU H  n[        U[
        5      (       d  [        S[	        U5       35      eUR                  UR                  5        UR                  UR                  5        UR                  UR                  5        UR                  UR                  5        M     [        R                  " USS9U l        [        R                  " USS9U l        [        R                  " USS9U l        [        R                  " USS9U l        U $ )z5Initialise the class attributes given a cameras list.z(pinhole must of type list or tuple. Got z6Argument pinhole must be from type PinholeCamera. Got r8   dim)
isinstancelisttuple	TypeErrortyper   appendr   r   r   r   r   stackr   r   )r   r   r   r   r   r   pinholes          r   r   )PinholeCamerasList._initialize_parameters  s   (T5M22FtH~FVWXXB!#RJGg}55"XY]^eYfXg hiiMM'..)LL'g001g001   %*KKA$>#(;;u!#<
).ZQ)G).ZQ)Gr    c                 h    SnU R                   b"  [        U R                   R                  S   5      nU$ )z0Return the number of pinholes cameras per batch.r_   r8   )r   r   r$   )r   num_camerass     r   r   PinholeCamerasList.num_cameras  s2     ??&doo33A67Kr    idxc                     U R                   SU4   nU R                  SU4   nU R                  SS2U4   nU R                  SS2U4   n[	        XEX#5      $ )z<Return a PinholeCamera object with parameters such as Bx4x4..N)r   r   r   r   r   )r   r   r   r   r   r   s         r   get_pinholePinholeCamerasList.get_pinhole  sW    #{{384"jjc2#'??1c6#:
#'??1c6#:
ZVCCr    r   )r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r    r   r   r     si     3h}&= 3$ 3x/F K_ ( S  Ds D} Dr    r   r   epsr   c                    [        U R                  5      S:X  a  U R                  S   S:X  d  [        U R                  5      e[        R                  " U SSS24   SSS9u  p#pE[        R
                  " SU R                  U R                  S9U-   nUR                  SSS5      R                  U R                  S	   SS5      nX&SS	S	S24'   XFSS	SS
24'   X6SSSS24'   XVSSSS
24'   U$ )a  Return the pinhole matrix from a pinhole model.

.. note::
    This method is going to be deprecated in version 0.2 in favour of
    :attr:`kornia.PinholeCamera.camera_matrix`.

Args:
    pinholes: torch.Tensor of pinhole models.
    eps: epsilon for numerical stability.

Returns:
    torch.Tensor of pinhole matrices.

Shape:
    - Input: :math:`(N, 12)`
    - Output: :math:`(N, 4, 4)`

Example:
    >>> rng = torch.manual_seed(0)
    >>> pinhole = torch.rand(1, 12)    # Nx12
    >>> pinhole_matrix(pinhole)  # Nx4x4
    tensor([[[4.9626e-01, 1.0000e-06, 8.8477e-02, 1.0000e-06],
             [1.0000e-06, 7.6822e-01, 1.3203e-01, 1.0000e-06],
             [1.0000e-06, 1.0000e-06, 1.0000e+00, 1.0000e-06],
             [1.0000e-06, 1.0000e-06, 1.0000e-06, 1.0000e+00]]])

rV   r8      .Nr/   r   r   r   r.   
r4   r$   rB   r   chunkr   r>   r   viewr   )r   r   rM   rR   rW   r[   ks          r   pinhole_matrixr     s    8 1$):b)@X^^,,[[#rr'!2A1=NBB		!HOO8>>BSHA	q!Qx~~a0!Q7Ac1ackNc1ackNc1ackNc1ackNHr    r   c                    [        U R                  5      S:X  a  U R                  S   S:X  d  [        U R                  5      e[        R                  " U SSS24   SSS9u  p#pE[        R
                  " SU R                  U R                  S9nUR                  SSS5      R                  U R                  S	   SS5      nS
X!-   -  USS	S	S24'   S
X1-   -  USSSS24'   SU-  X!-   -  USS	SS24'   SU-  X1-   -  USSSS24'   U$ )a  Return the inverted pinhole matrix from a pinhole model.

.. note::
    This method is going to be deprecated in version 0.2 in favour of
    :attr:`kornia.PinholeCamera.intrinsics_inverse()`.

Args:
    pinhole: torch.Tensor with pinhole models.
    eps: epsilon for numerical stability.

Returns:
    torch.Tensor of inverted pinhole matrices.

Shape:
    - Input: :math:`(N, 12)`
    - Output: :math:`(N, 4, 4)`

Example:
    >>> rng = torch.manual_seed(0)
    >>> pinhole = torch.rand(1, 12)  # Nx12
    >>> inverse_pinhole_matrix(pinhole)  # Nx4x4
    tensor([[[ 2.0151,  0.0000, -0.1783,  0.0000],
             [ 0.0000,  1.3017, -0.1719,  0.0000],
             [ 0.0000,  0.0000,  1.0000,  0.0000],
             [ 0.0000,  0.0000,  0.0000,  1.0000]]])

rV   r8   r   .Nr/   r   r   r   r   g      r.   r   )r   r   rM   rR   rW   r[   r   s          r   inverse_pinhole_matrixr     s   8 !#a(8B(>W]]++[[bqb!11!<NBB		!GNN'--@A	q!Qw}}Q/A6ABH%Ac1ackNBH%Ac1ackNBY"(+Ac1ackNBY"(+Ac1ackNHr    r   c                 H   [        U R                  5      S:X  a  U R                  S   S:X  d  [        U R                  5      e[        UR                  5      S:w  a  [        UR                  5      eU R                  5       nU SSS24   UR	                  S5      -  USSS24'   U$ )a  Scale the pinhole matrix for each pinhole model.

.. note::
    This method is going to be deprecated in version 0.2 in favour of
    :attr:`kornia.PinholeCamera.scale()`.

Args:
    pinholes: torch.Tensor with the pinhole model.
    scale: torch.Tensor of scales.

Returns:
    torch.Tensor of scaled pinholes.

Shape:
    - Input: :math:`(N, 12)` and :math:`(N, 1)`
    - Output: :math:`(N, 12)`

Example:
    >>> rng = torch.manual_seed(0)
    >>> pinhole_i = torch.rand(1, 12)  # Nx12
    >>> scales = 2.0 * torch.ones(1)   # N
    >>> scale_pinhole(pinhole_i, scales)  # Nx12
    tensor([[0.9925, 1.5364, 0.1770, 0.2641, 0.6148, 1.2682, 0.4901, 0.8964, 0.4556,
             0.6323, 0.3489, 0.4017]])

rV   r8   r   .N   r_   )r4   r$   rB   r   	unsqueeze)r   r   pinholes_scaleds      r   scale_pinholer   -  s    6 1$):b)@X^^,,
5;;1U[[))nn&O'RaR05??23FFOC!Gr    c                     [        U R                  5      S:X  a  U R                  S   S:X  d  [        U R                  5      e[        e)a  Compute extrinsic transformation matrices for pinholes.

Args:
    pinholes: torch.Tensor of form [fx fy cx cy h w rx ry rz tx ty tz]
                       of size (N, 12).

Returns:
    torch.Tensor of extrinsic transformation matrices of size (N, 4, 4).

rV   r8   r   )r4   r$   rB   NotImplementedError)r   s    r   get_optical_pose_baser   Q  s:     1$):b)@X^^,,
r    	pinhole_ipinhole_refc           	         [        U R                  5      S:X  a  U R                  S   S:X  d  [        U R                  5      eU R                  UR                  :w  a  [        UR                  5      e[        U 5      n[        U5      n[        R
                  " U[        U5      5      n[        R
                  " [        U 5      [        R
                  " U[        U5      5      5      $ )a  Homography from reference to ith pinhole.

.. note::
    The pinhole model is represented in a single vector as follows:

    .. math::
        pinhole = (f_x, f_y, c_x, c_y, height, width,
        r_x, r_y, r_z, t_x, t_y, t_z)

    torch.where:
        :math:`(r_x, r_y, r_z)` is the rotation vector in angle-axis
        convention.

        :math:`(t_x, t_y, t_z)` is the translation vector.

.. math::

    H_{ref}^{i} = K_{i} * T_{ref}^{i} * K_{ref}^{-1}

Args:
    pinhole_i: torch.Tensor with pinhole model for ith frame.
    pinhole_ref: torch.Tensor with pinhole model for reference frame.

Returns:
    tensors that convert depth points (u, v, d) from pinhole_ref to pinhole_i.

Shape:
    - Input: :math:`(N, 12)` and :math:`(N, 12)`
    - Output: :math:`(N, 4, 4)`

Example:
    pinhole_i = torch.rand(1, 12)    # Nx12
    pinhole_ref = torch.rand(1, 12)  # Nx12
    homography_i_H_ref(pinhole_i, pinhole_ref)  # Nx4x4

rV   r8   r   )	r4   r$   rB   r   r   matmulr   r   r   )r   r   i_pose_baseref_pose_base
i_pose_refs        r   homography_i_H_refr   g  s    L 	 A%)//!*<*BY__--++++[..//'	2K)+6Mk+A-+PQJ<<y15<<
LbcnLo3pqqr    r   intrinsics_invpixel_coordsc                    [        U R                  5      S:X  d+  U R                  S   S:X  a  [        SU R                   35      e[        UR                  5      S:X  d  [        SUR                   35      e[        UR                  5      S:X  d+  UR                  S   S:X  a  [        SUR                   35      e[        USS2S4   U5      nX0R	                  SS	SS5      -  $ )
an  Transform coordinates in the pixel frame to the camera frame.

Args:
    depth: the source depth maps. Shape must be Bx1xHxW.
    intrinsics_inv: the inverse intrinsics camera matrix. Shape must be Bx4x4.
    pixel_coords: the grid with (u, v, 1) pixel coordinates. Shape must be BxHxWx3.

Returns:
    torch.Tensor of shape BxHxWx3 with (x, y, z) cam coordinates.

r/   r8   z3Input depth has to be in the shape of Bx1xHxW. Got r.   z:Input intrinsics_inv has to be in the shape of Bx4x4. Got z:Input pixel_coords has to be in the shape of BxHxWx3. Got Nr   rV   )r4   r$   r*   r   permute)r   r   r   
cam_coordss       r   	pixel2camr     s     u{{q U[[^q%8Nu{{m\]]~##$)UVdVjVjUklmm|!!"a'L,>,>q,AQ,FUVdVjVjUklmm/q$w0GVJaAq111r    cam_coords_srcdst_proj_srcc                    [        U R                  5      S:X  d+  U R                  S   S:X  a  [        SU R                   35      e[        UR                  5      S:X  d+  UR                  SS S:X  a  [        SUR                   35      e[        USS2S4   U 5      nUS   nUS	   nUS
   nXFU-   -  nXVU-   -  n[        R
                  " Xx/SS9n	U	$ )a  Transform coordinates in the camera frame to the pixel frame.

Args:
    cam_coords_src: (x, y, z) coordinates defined in the first camera coordinates system. Shape must be BxHxWx3.
    dst_proj_src: the projection matrix between the
      reference and the non reference camera frame. Shape must be Bx4x4.
    eps: small value to avoid division by zero error.

Returns:
    torch.Tensor of shape BxHxWx2 with (u, v) pixel coordinates.

r/   r.   z<Input cam_coords_src has to be in the shape of BxHxWx3. Got r0   Nr1   z8Input dst_proj_src has to be in the shape of Bx4x4. Got r   ).r8   ).rV   r_   r   )r4   r$   r*   r   r   r   )
r   r   r   point_coordsx_coordy_coordz_coordu_coordv_coordpixel_coords_dsts
             r   	cam2pixelr     s     ~##$)n.B.B1.E.JWXfXlXlWmnoo|!!"a'L,>,>rs,Cv,MST`TfTfSghii!1,q$w2G!XL(0G(0G(0G $}5G#}5G &+[['1C%Lr    )gư>)g-q=)typingr   r   r   r   kornia.core.checkr   kornia.core.utilsr   kornia.geometry.conversionsr	   r
   kornia.geometry.linalgr   r   r   r   r   r   r   r   r   r   r   r   r   r   r    r   <module>r     sW  $ ) (  6 1 f KC CD6D 6Dr(U\\ ( ( (V(ELL (u ( (V!ELL ! !%,, !HELL U\\ ,-r%,, -rU\\ -rell -rh2U\\ 25<< 2u|| 2`e`l`l 24ell %,, U _d_k_k B10r    