
    3jb                       S r SSKJr  SSKJr  SSKrSSKJs  Jr	  SSKJr  SSK
JrJrJr  SSKJr  SSKJr  S	S
KJrJrJrJrJr  S	SKJrJr  S	SKJrJr   / SQ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" S           SS jjr# S           S S jjr$ " S S\RJ                  5      r& S!               S"S jjr'        S#S jr(g)$z;nn.Module containing operators to work on RGB-Depth images.    )annotations)OptionalN)nn)KORNIA_CHECKKORNIA_CHECK_IS_TENSORKORNIA_CHECK_SHAPE)spatial_gradient)create_meshgrid   )PinholeCamera	cam2pixel	pixel2camproject_pointsunproject_points)normalize_pixel_coordinates normalize_points_with_intrinsics)convert_points_to_homogeneoustransform_points)	DepthWarperdepth_from_disparitydepth_from_plane_equationdepth_to_3ddepth_to_3d_v2depth_to_normals
depth_warpunproject_meshgridwarp_frame_depthc                    [        U/ SQ5        [        XSXES9R                  5       nUSS2SS4   n[        Xg5      n[	        U5      n	U(       a  [
        R                  " U	SSS9n	U	$ )a  Compute a 3d point per pixel given its depth value and the camera intrinsics.

.. tip::

    This function should be used in conjunction with :py:func:`kornia.geometry.depth.depth_to_3d_v2` to cache
    the meshgrid computation when warping multiple frames with the same camera intrinsics.

Args:
    height: height of image.
    width: width of image.
    camera_matrix: tensor containing the camera intrinsics with shape :math:`(3, 3)`.
    normalize_points: whether to normalize the pointcloud. This must be set to `True` when the depth is
      represented as the Euclidean ray length from the camera position.
    device: device to place the result on.
    dtype: dtype of the result.

Return:
    tensor with a 3d point per pixel of the same resolution as the input :math:`(*, H, W, 3)`.

*3r!   Fnormalized_coordinatesdevicedtypeN   dimp)r   r
   squeezer   r   F	normalize)
heightwidthcamera_matrixnormalize_pointsr$   r%   	points_uvcamera_matrix_tmp	points_xy
points_xyzs
             O/home/wildlama/miniconda3/lib/python3.13/site-packages/kornia/geometry/depth.pyr   r   3   so    8 }o6 .eFgi 
 '4AtTM&B0NI /y9J[[q9
    c                    [        U / SQ5        [        U/ SQ5        U R                  SS u  pEUb  UO![        XEXU R                  U R                  5      n[        U/ SQ5        X`S   -  $ )a{  Compute a 3d point per pixel given its depth value and the camera intrinsics.

.. note::

    This is an alternative implementation of :py:func:`kornia.geometry.depth.depth_to_3d`
    that does not require the creation of a meshgrid.

Args:
    depth: image tensor containing a depth value per pixel with shape :math:`(*, H, W)`.
    camera_matrix: tensor containing the camera intrinsics with shape :math:`(*, 3, 3)`.
    normalize_points: whether to normalise the pointcloud. This must be set to `True` when the depth is
      represented as the Euclidean ray length from the camera position.
    xyz_grid: explicit xyz point values.

Return:
    tensor with a 3d point per pixel of the same resolution as the input :math:`(*, H, W, 3)`.

Example:
    >>> depth = torch.rand(4, 4)
    >>> K = torch.eye(3).repeat(2,1,1)
    >>> depth_to_3d_v2(depth, K).shape
    torch.Size([2, 4, 4, 3])

r    HWr   N)r    r:   r;   r!   ).N)r   shaper   r$   r%   )depthr0   r1   xyz_gridr.   r/   r5   s          r6   r   r   d   sv    @ uo.}o6 KK$MF  	}PUP\P\^c^i^ij  z#78i(((r7   c                >   [        U 5        [        U5        [        U / SQ5        [        U/ SQ5        U R                  u    p4n[        XESU R                  U R
                  S9nU R                  SSSS5      nUS	S	2S	S	4   n[        XgXS
9n	U	R                  SSSS5      $ )af  Compute a 3d point per pixel given its depth value and the camera intrinsics.

.. note::

    This is an alternative implementation of `depth_to_3d` that does not require the creation of a meshgrid.
    In future, we will support only this implementation.

Args:
    depth: image tensor containing a depth value per pixel with shape :math:`(B, 1, H, W)`.
    camera_matrix: tensor containing the camera intrinsics with shape :math:`(B, 3, 3)`.
    normalize_points: whether to normalise the pointcloud. This must be set to `True` when the depth is
      represented as the Euclidean ray length from the camera position.

Return:
    tensor with a 3d point per pixel of the same resolution as the input :math:`(B, 3, H, W)`.

Example:
    >>> depth = torch.rand(1, 1, 4, 4)
    >>> K = torch.eye(3)[None]
    >>> depth_to_3d(depth, K).shape
    torch.Size([1, 3, 4, 4])

B1r:   r;   rB   r!   r!   Fr"   r   r'      r   N)r-   )r   r   r=   r
   r$   r%   permuter   )
r>   r0   r1   _r.   r/   	points_2dpoints_depthr3   	points_3ds
             r6   r   r      s    0 5!=)u23}o6  ++Aq%-eELLPUP[P[I
 "'q!Q!:L '4AtTM&B.!2I Q1a((r7   c                   [        U 5        [        U5        [        U / SQ5        [        U/ SQ5        [        U R                  S5      X5      R	                  SSSS5      n[        U5      nUSS2SS2S4   R	                  SSSS5      R                  5       nUSS2SS2S4   R	                  SSSS5      R                  5       n[        R                  R                  XVSS	9n[        R                  " USSS
9R	                  SSSS5      $ )a  Compute the normal surface per pixel.

Args:
    depth: image tensor containing a depth value per pixel with shape :math:`(B, 1, H, W)`.
    camera_matrix: tensor containing the camera intrinsics with shape :math:`(B, 3, 3)`.
    normalize_points: whether to normalize the pointcloud. This must be set to `True` when the depth is
    represented as the Euclidean ray length from the camera position.

Return:
    tensor with a normal surface vector per pixel of the same resolution as the input :math:`(B, 3, H, W)`.

Example:
    >>> depth = torch.rand(1, 1, 4, 4)
    >>> K = torch.eye(3)[None]
    >>> depth_to_normals(depth, K).shape
    torch.Size([1, 3, 4, 4])

rA   rD   r   r   rE   r'   Nr&   r)   r(   )r   r   r   r+   rF   r	   
contiguoustorchlinalgcrossr,   r-   )r>   r0   r1   xyz	gradientsabnormalss           r6   r   r      s    & 5!=)u23}o6 'u}}Q'7YaabcefhiklmC /s3I
  1a(00Aq!<GGIA1a(00Aq!<GGIA!LL..q.<G;;wB!,44Q1a@@r7   c                   [        U SS/5        [        USS/5        [        U/ SQ5        [        U/ SQ5        [        X#5      n[        U5      nU R                  S5      n[        R
                  " Xg-  SS9n[        R                  " U5      n	X:  n
[        R                  " X[        R                  " U5      -  U5      nX-  nU$ )	a  Compute depth values from plane equations and pixel coordinates.

Args:
    plane_normals (torch.Tensor): Plane normal vectors of shape (B, 3).
    plane_offsets (torch.Tensor): Plane offsets of shape (B, 1).
    points_uv (torch.Tensor): Pixel coordinates of shape (B, N, 2).
    camera_matrix (torch.Tensor): Camera intrinsic matrix of shape (B, 3, 3).
    eps: epsilon for numerical stability.

Returns:
    torch.Tensor: Computed depth values at the given pixels, shape (B, N).

rB   r!   rC   )rB   N2rD   r   r&   rL   )	r   r   r   	unsqueezerN   sumabswheresign)plane_normalsplane_offsetsr2   r0   epsr4   raysplane_normals_expdenom	denom_abs	zero_maskr>   s               r6   r   r      s    ( }sCj1}sCj1y/2}o6 1JI(3D &//2 IId.B7E		% IIKK	E):#:EBE !ELr7   c                \   [        U / SQ5        [        U/ SQ5        [        U/ SQ5        [        U/ SQ5        [        UR                  S5      X45      n[        USS2S4   U5      nUSS2SS4   n[	        Xg5      nUR
                  SS u  p[        XU
5      n[        R                  " XSS	9$ )
a4  Warp a tensor from a source to destination frame by the depth in the destination.

Compute 3d points from the depth, transform them using given transformation, then project the point cloud to an
image plane.

Args:
    image_src: image tensor in the source frame with shape :math:`(B,D,H,W)`.
    depth_dst: depth tensor in the destination frame with shape :math:`(B,1,H,W)`.
    src_trans_dst: transformation matrix from destination to source with shape :math:`(B,4,4)`.
    camera_matrix: tensor containing the camera intrinsics with shape :math:`(B,3,3)`.
    normalize_points: whether to normalize the pointcloud. This must be set to ``True`` when the depth
       is represented as the Euclidean ray length from the camera position.

Return:
    the warped tensor in the source frame with shape :math:`(B,3,H,W)`.

)rB   Dr:   r;   rA   )rB   4rh   rD   r   Nr<   Talign_corners)	r   r   r+   r   r   r=   r   r,   grid_sample)	image_src	depth_dstsrc_trans_dstr0   r1   points_3d_dstpoints_3d_srcr3   points_2d_srcr.   r/   points_2d_src_norms               r6   r   r     s    0 y"67y"67}o6}o6 #11B1B11E}"gM %]1d7%;]KM '4AtTM&B"0"RM OOBC(MF'B=Z_'`==dKKr7   c                     ^  \ rS rSrSr   S             SU 4S jjjr\SS j5       rSS jrSS jr	SS jr
SS jrSS	 jrS
rU =r$ )r   iE  a  Warp a patch by depth.

.. math::
    P_{src}^{\{dst\}} = K_{dst} * T_{src}^{\{dst\}}

    I_{src} = \\omega(I_{dst}, P_{src}^{\{dst\}}, D_{src})

Args:
    pinholes_dst: the pinhole models for the destination frame.
    height: the height of the image to warp.
    width: the width of the image to warp.
    mode: interpolation mode to calculate output values ``'bilinear'`` | ``'nearest'``.
    padding_mode: padding mode for outside grid values ``'zeros'`` | ``'border'`` | ``'reflection'``.
    align_corners: interpolation flag.

c                  > [         TU ]  5         X0l        X l        X@l        XPl        SU l        X`l        [        U[        5      (       d  [        S[        U5       35      eXl        S U l        S U l        U R                  X#5      U l        g )Ngư>z+Expected pinhole_dst as PinholeCamera, got )super__init__r/   r.   modepadding_moder`   rj   
isinstancer   	TypeErrortype_pinhole_dst_pinhole_src_dst_proj_src_create_meshgridgrid)selfpinhole_dstr.   r/   rw   rx   rj   	__class__s          r6   rv   DepthWarper.__init__X  s     	
!	!-#0 +}55I${J[I\]^^+62626 #'"7"7"F	r7   c                ,    [        XSS9n[        U5      $ )NF)r#   )r
   r   )r.   r/   r   s      r6   r   DepthWarper._create_meshgridt  s    ,VSXY,T22r7   c                   [        U R                  5      [        La!  [        S[        U R                  5       35      e[        U5      [        La  [        S[        U5       35      eUR                  R
                  SS nUR                  R                  nUR                  R                  n[        R                  " SX4S9R                  " / UQSPSP76 R                  5       n[        R                  " SX4S9R                  " / UQSPSP76 R                  5       nUR                  SSS2SS24   nUR                  SSS2SS24   n[        R                  " US	S5      n	[        R                  " U	* U5      n
XSSS2SS24'   XSSS2SS24'   U R                  R                  SSS2SS24   nU R                  R                  SSS2SS24   n[        R                  " X5      n[        R                  " X5      U-   nXSSS2SS24'   XSSS2SS24'   [        R                  " U R                  R                  U5      nXl        Xl        U $ )
zCCompute the projection matrix from the source to destination frame.zDMember self._pinhole_dst expected to be of class PinholeCamera. Got z@Argument pinhole_src expected to be of class PinholeCamera. Got Nr<      r$   r%   .rE   r&   )r{   r|   r   rz   
extrinsicsr=   r$   r%   rN   eyeexpandrM   	transposematmul
intrinsicsr}   r~   )r   pinhole_srcbatch_shaper$   r%   inv_extrdst_trans_srcsrc_rmatsrc_tvecinv_rmatinv_tvecdst_rmatdst_tveccomposed_rmatcomposed_tvecdst_proj_srcs                   r6   compute_projection_matrix%DepthWarper.compute_projection_matrixy  sV    !!"-7VW[\`\m\mWnVop  M1^_cdo_p^qrss!,,223B7''..&&,, 99Qv;BBVKVQRVTUVaac		!F@GG[[VW[YZ[ffh ))#rr2A2+6))#rr12+6??8R4<<	84 !)bqb"1" (bqb!" $$//RaR!<$$//RaR<X8X88C%2c2A2rrk"%2c2A2qrk" ||D$5$5$@$@-P')r7   c                   U R                   b  U R                  c  [        S5      e[        R                  " U/U/U/S///U R                   R
                  U R                   R                  S9n[        R                  " U R                   U5      nSUS S 2S4   -  nUS S 2S4   U-  nUS S 2S4   U-  n[        R                  " Xx/S5      $ )N'Please, call compute_projection_matrix.      ?r   r'   r   r   )	r~   r}   
ValueErrorrN   tensorr$   r%   r   cat)	r   xyinvdpointflowz_x_ys	            r6   _compute_projectionDepthWarper._compute_projection  s    %):):)BFGGcA3&'0B0B0I0IQUQcQcQiQi
 ||D..6$q!t*!Q$Z!^!Q$Z!^yy"1%%r7   c                   U R                   c  [        S5      eSnU R                  S-  nU R                  S-  nSU-
  SU-   4n[        R
                  " X#US   S/X#US   S//U R                   R                  U R                   R                  S9R                  SS5      R                  S5      nU R                   n[        R                  " Xe5      nSUSS2S4   -  nUSS2S4   U-  n	USS2S4   U-  n
[        R                  " X4S	S
9n[        R                  " USS2S4   USS2S4   -
  SSS9S-  nX-  n[        R                  " SU-  5      $ )a  Compute the inverse depth step for sub pixel accurate sampling of the depth cost volume, per camera.

Szeliski, Richard, and Daniel Scharstein. "Symmetric sub-pixel stereo matching." European Conference on Computer
Vision. Springer Berlin Heidelberg, 2002.
NzCExpected torch.Tensor, but got None Type from the projection matrixg{Gz?r'   r   r   r   )r%   r$   r&   rL   )r*   r)   g       @g      ?)r~   RuntimeErrorr/   r.   rN   r   r%   r$   r   rY   r   stacknormmin)r   delta_dcenter_xcenter_yinvdspointsprojr   zsxsysxysdxydxdds                 r6   compute_subpixel_step!DepthWarper.compute_subpixel_step  s]    %dee::>;;? wg. LLeAh4x5QR8UX6YZ((..))00
 Yq!_Yq\ 	 !!||D)41:!Q$Z"_!Q$Z"_kk2(+jjQTSAY.!;cA}yyt$$r7   c                @   U R                   b  U R                  c  [        S5      e[        UR                  5      S:w  a  [        SUR                   35      eUR                  u  n    nUR
                  nUR                  nU R                  R                  XES9R                  USSS5      n[        XR                  R                  5       R                  XES9U5      n[        XpR                   R                  XES95      n[        XR                  U R                  5      n	U	$ )a4  Compute a grid for warping a given the depth from the reference pinhole camera.

The function `compute_projection_matrix` has to be called beforehand in order to have precomputed the relative
projection matrices encoding the relative pose and the intrinsics between the reference and a non reference
camera.
r   r   z7Input depth_src has to be in the shape of Bx1xHxW. Got r   r&   )r~   r}   r   lenr=   r$   r%   r   tor   r   intrinsics_inverser   r   r.   r/   )
r   	depth_src
batch_sizerG   r$   r%   pixel_coordscam_coords_srcpixel_coords_srcpixel_coords_src_norms
             r6   	warp_gridDepthWarper.warp_grid  s    %):):)BFGGy1$VW`WfWfVghii (oo
Aq!(//&__ &*YY\\\%M%T%TU_acegik%l (1((;;=@@@\^j(

 *3..111M*

 /JJZ\g\gimisis.t$$r7   c                    [         R                  " UU R                  U5      U R                  U R                  U R
                  S9$ )aw  Warp a tensor from destination frame to reference given the depth in the reference frame.

Args:
    depth_src: the depth in the reference frame. The tensor must have a shape :math:`(B, 1, H, W)`.
    patch_dst: the patch in the destination frame. The tensor must have a shape :math:`(B, C, H, W)`.

Return:
    the warped patch from destination frame to reference.

Shape:
    - Output: :math:`(N, C, H, W)` where C = number of channels.

Example:
    >>> # pinholes camera models
    >>> pinhole_dst = PinholeCamera(torch.randn(1, 4, 4), torch.randn(1, 4, 4),
    ... torch.tensor([32]), torch.tensor([32]))
    >>> pinhole_src = PinholeCamera(torch.randn(1, 4, 4), torch.randn(1, 4, 4),
    ... torch.tensor([32]), torch.tensor([32]))
    >>> # create the depth warper, compute the projection matrix
    >>> warper = DepthWarper(pinhole_dst, 32, 32)
    >>> _ = warper.compute_projection_matrix(pinhole_src)
    >>> # warp the destination frame to reference by depth
    >>> depth_src = torch.ones(1, 1, 32, 32)  # Nx1xHxW
    >>> image_dst = torch.rand(1, 3, 32, 32)  # NxCxHxW
    >>> image_src = warper(depth_src, image_dst)  # NxCxHxW

)rw   rx   rj   )r,   rk   r   rw   rx   rj   )r   r   	patch_dsts      r6   forwardDepthWarper.forward  s=    8 }}NN9%**,,
 	
r7   )
r~   r|   r}   rj   r`   r   r.   rw   rx   r/   )bilinearzerosT)r   r   r.   intr/   r   rw   strrx   r   rj   boolreturnNone)r.   r   r/   r   r   torch.Tensor)r   r   r   r   )r   floatr   r   r   r   r   r   )r   r   )r   r   r   r   )r   r   r   r   r   r   )__name__
__module____qualname____firstlineno____doc__rv   staticmethodr   r   r   r   r   r   __static_attributes____classcell__)r   s   @r6   r   r   E  s    . #"G"G G 	G
 G G G 
G G8 3 3*X&#%J"%H"
 "
r7   r   c                H    [        XXVS9nUR                  U5        U" X#5      $ )a  Warp a tensor from destination frame to reference given the depth in the reference frame.

See :class:`~kornia.geometry.warp.DepthWarper` for details.

Example:
    >>> # pinholes camera models
    >>> pinhole_dst = PinholeCamera(torch.randn(1, 4, 4), torch.randn(1, 4, 4),
    ... torch.tensor([32]), torch.tensor([32]))
    >>> pinhole_src = PinholeCamera(torch.randn(1, 4, 4), torch.randn(1, 4, 4),
    ... torch.tensor([32]), torch.tensor([32]))
    >>> # warp the destination frame to reference by depth
    >>> depth_src = torch.ones(1, 1, 32, 32)  # Nx1xHxW
    >>> image_dst = torch.rand(1, 3, 32, 32)  # NxCxHxW
    >>> image_src = depth_warp(pinhole_dst, pinhole_src, depth_src, image_dst, 32, 32)  # NxCxHxW

ri   )r   r   )r   r   r   r   r.   r/   rj   warpers           r6   r   r      s)    8 eQF
$$[1)''r7   c                   [        U S[        U 5       S35        [        U / SQ5        [        [	        U[
        [        R                  45      S[        U5       35        [        [	        U[
        [        R                  45      S[        U5       35        [	        U[        R                  5      (       a  [        US/5        [	        U[        R                  5      (       a  [        US/5        X-  U S-   -  $ )a  Compute depth from disparity.

Args:
    disparity: Disparity tensor of shape :math:`(*, H, W)`.
    baseline: float/tensor containing the distance between the two lenses.
    focal: float/tensor containing the focal length.

Return:
    Depth map of the shape :math:`(*, H, W)`.

Example:
    >>> disparity = torch.rand(4, 1, 4, 4)
    >>> baseline = torch.rand(1)
    >>> focal = torch.rand(1)
    >>> depth_from_disparity(disparity, baseline, focal).shape
    torch.Size([4, 1, 4, 4])

z0Input disparity type is not a torch.Tensor. Got .r9   z=Input baseline should be either a float or torch.Tensor. Got z:Input focal should be either a float or torch.Tensor. Got rC   :0yE>)r   r{   r   r   ry   r   rN   Tensor)	disparitybaselinefocals      r6   r   r   C  s    * 9(XY]^gYhXiij&kly/28eU\\23
GXGWX 55%,,/0
DT%[MR
 (ELL))8cU+%&&53%(y4/00r7   )FNN)r.   r   r/   r   r0   r   r1   r   r$   zOptional[torch.device]r%   zOptional[torch.dtype]r   r   )FN)
r>   r   r0   r   r1   r   r?   zOptional[torch.Tensor]r   r   )F)r>   r   r0   r   r1   r   r   r   )r   )r^   r   r_   r   r2   r   r0   r   r`   r   r   r   )rl   r   rm   r   rn   r   r0   r   r1   r   r   r   )T)r   r   r   r   r   r   r   r   r.   r   r/   r   rj   r   r   r   )r   r   r   float | torch.Tensorr   r   r   r   ))r   
__future__r   typingr   rN   torch.nn.functionalr   
functionalr,   kornia.core.checkr   r   r   kornia.filters.sobelr	   kornia.geometry.gridr
   camerar   r   r   r   r   conversionsr   r   rO   r   r   __all__r   r   r   r   r   r   Moduler   r   r    r7   r6   <module>r      s  $ B "      V V 1 0 Y Y V C A
" #%)#'...  . 	.
 #. !. .h #'+	-)-)-) -) %	-)
 -)`,)^%AZ ))) )  	)
 
) )b #+L+L+L  +L  	+L
 +L +L\X
")) X
D  ( ( (  ( 	 (
  (  (  (  (F&1&1';&1DX&1&1r7   