o oiS_@s6dZddlmZddlmZddlZddlmZddlm Z m Z m Z ddl m Z mZmZddlmZddlmZd d lmZmZmZmZmZd d lmZmZd d lmZmZ gd Z   dDdEddZ! dFdGd d!Z"dHdId"d#Z#dHdId$d%Z$ &dJdKd,d-Z% dHdLd1d2Z&Gd3d4d4e Z' 5dMdNd>> 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+HWr*N)r+r@rAr,).N)r shaperr%r')r<r"r#r=rr!r9r:r:r;rds rc Cst|t|t|gdt|gd|j\}}}}t||dd}||j|j}|dddd}|d d d d f}t||||d } | ddddS) aCompute 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]) B1r@rArEr,r,Fr-rr/r N)r5) rr rCr tor%r'permuter) r<r"r#_rr! points_2d points_depthr7 points_3dr:r:r;rsrcCst|t|t|gdt|gdt|||}t|}|dddddf|dddddf}}tjj||dd}tj|dddS) aCompute 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]) rDrGNrr r1r/r0) rr rr torchlinalgcrossr4r5)r<r"r#xyz gradientsabnormalsr:r:r;rs .r:0yE> plane_normals plane_offsetsr6epsfloatc Cst|ddgt|ddgt|gdt|gdt||}t|}|d}tj||dd}t|} | |k} t| |t||}||} | S) aCompute depth values from plane equations and pixel coordinates. Args: plane_normals (Tensor): Plane normal vectors of shape (B, 3). plane_offsets (Tensor): Plane offsets of shape (B, 1). points_uv (Tensor): Pixel coordinates of shape (B, N, 2). camera_matrix (Tensor): Camera intrinsic matrix of shape (B, 3, 3). eps: epsilon for numerical stability. Returns: Tensor: Computed depth values at the given pixels, shape (B, N). rEr,rF)rEN2rGr r.rP) r rr unsqueezerQsumabswheresign) rZr[r6r"r\r8raysplane_normals_expdenom denom_abs zero_maskr<r:r:r;rs   r image_src depth_dst src_trans_dstc Cst|gdt|gdt|gdt|gdt|||}|dddd}t|d d d f|}|d d d d f}t||}|jd d \} } t|| | } tj|| d d S) adWarp 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)`. )rEDr@rArD)rE4rnrGrr/rIr NrBT align_corners) r rrKrrrCrr4map_coordinates) rjrkrlr"r# points_3d_dst points_3d_srcr7 points_2d_srcrr!points_2d_src_normr:r:r;r s   rcsjeZdZdZ   d'd(fdd Zed)ddZd*ddZd+ddZd,dd Z d-d"d#Z d.d%d&Z Z S)/raCWarp 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. bilinearzerosT pinhole_dstr rr r!modestr padding_moderpr$r)Nonecsnt||_||_||_||_d|_||_t|t s%t dt |||_ d|_ d|_||||_dS)Ngư>z+Expected pinhole_dst as PinholeCamera, got )super__init__r!rryr{r\rp isinstancer TypeErrortype _pinhole_dst _pinhole_src _dst_proj_src_create_meshgridgrid)selfrxrr!ryr{rp __class__r:r;r~Js  zDepthWarper.__init__rcCst||dd}t|S)NFrH)r r)rr!rr:r:r;rfszDepthWarper._create_meshgrid pinhole_srccCst|jturtdt|jt|tur tdt||jjdd}|jj}|jj}tj d||dj g|ddR }tj d||dj g|ddR }|jdddddf}|jdddddf}t |d d} t | |} | |dddddf<| |dddddf<|jjdddddf} |jjdddddf} t | | } t | | | }| |dddddf<||dddddf<t |jj|}||_||_|S) 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 NrBr%r'.rIr.)rrr r extrinsicsrCr%r'rQeyeexpand contiguous transposematmul intrinsicsrr)rr batch_shaper%r'inv_extr dst_trans_srcsrc_rmatsrc_tvecinv_rmatinv_tvecdst_rmatdst_tvec composed_rmat composed_tvec dst_proj_srcr:r:r;compute_projection_matrixks6 (( z%DepthWarper.compute_projection_matrixxr]yinvdc Cs|jdus |jdurtdt|g|g|gdggg|jj|jjd}t|j|}d|dddf}|dddf|}|dddf|}t ||gdS)N'Please, call compute_projection_matrix.?rr/rr ) rr ValueErrorrr%r'rQrr4 concatenate) rrrrpointflowz_x_yr:r:r;_compute_projections(zDepthWarper._compute_projectioncCs"|jdur tdd}|jd}|jd}d|d|f}tj|||ddg|||ddgg|jj|jjddd d}|j}t ||}d|dddf}|dddf|} |dddf|} tj | | fd d } tj | dddf| dddfddd d } | |} t d | S)aCompute 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. Nz=Expected Tensor, but got None Type from the projection matrixg{Gz?r/rrr )r'r%r.rP)r2r1g@g?)r RuntimeErrorr!rrQrr'r%rr`rstacknormmin)rdelta_dcenter_xcenter_yinvdspointsprojrzsxsysxysdxydxddr:r:r;compute_subpixel_steps.    0z!DepthWarper.compute_subpixel_step depth_srcc Cs|jdus |jdurtdt|jdkrtd|j|j\}}}}|j}|j}|jj||d |ddd}t ||j j||d|}t ||jj||d}t ||j|j} | S)aTCompute 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. Nrrz7Input depth_src has to be in the shape of Bx1xHxW. Got rr.)rrrlenrCr%r'rrJrrintrinsics_inverserrrr!) rr batch_sizerLr%r' pixel_coordscam_coords_srcpixel_coords_srcpixel_coords_src_normr:r:r; warp_grids zDepthWarper.warp_grid patch_dstcCs tj||||j|j|jdS)aWarp 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 )ryr{rp)r4rqrryr{rp)rrrr:r:r;forwardszDepthWarper.forward)rvrwT)rxr rr r!r ryrzr{rzrpr$r)r|)rr r!r r)r)rr r)r)rr]rr]rr]r)r)r)r)rrr)r)rrrrr)r) __name__ __module__ __qualname____doc__r~ staticmethodrrrrrr __classcell__r:r:rr;r7s   , %$rTrxr rrrrpcCs$t||||d}|||||S)aWarp 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 ro)rr)rxrrrrr!rpwarperr:r:r;rs  r disparitybaselinefloat | TensorfocalcCst|dt|dt|gdtt|ttfdt|tt|ttfdt|t|tr;t|dgt|trFt|dg|||dS)aCompute 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]) z*Input disparity type is not a Tensor. Got .r?z7Input baseline should be either a float or Tensor. Got z4Input focal should be either a float or Tensor. Got rFrY)rrr rrr]r)rrrr:r:r;r3s      r)FNN)rr r!r r"rr#r$r%r&r'r(r)r)FN) r<rr"rr#r$r=r>r)r)F)r<rr"rr#r$r)r)rY) rZrr[rr6rr"rr\r]r)r) rjrrkrrlrr"rr#r$r)r)T)rxr rr rrrrrr r!r rpr$r)r)rrrrrrr)r)*r __future__rtypingrrQ kornia.corecorer4rrrkornia.core.checkrrr kornia.filters.sobelr kornia.utilsr camerar rrrr conversionsrrrRrr__all__rrrrrrrrrr:r:r:r;s>      2 + .& ) -a #