o oizj @s6ddlmZmZmZddlZddlmZmZmZm Z m Z ddl m Z ddl mZmZddlmZmZddlmZGdd d ZGd d d eZd(d ededefddZd(dededefddZd ededefddZd edefddZdededefddZdeded edefd!d"Zd)d$ed%ededefd&d'ZdS)*)IterableListUnionN)DeviceTensoreyestackzerosKORNIA_CHECK_SAME_DEVICE)convert_points_from_homogeneousconvert_points_to_homogeneous)inverse_transformationtransform_points)_torch_inverse_castc@seZdZdZdededededdf dd Zed eedefd d Z ed ede defddZ ed ede defddZ ed e eddfddZdejfddZedefddZedefddZedefddZedefddZedefdd Zedefd!d"Zedefd#d$Zedefd%d&Zejd'eeefddfd(d&Zedefd)d*Zejd'eeefddfd+d*Zedefd,d-Zejd'eeefddfd.d-Zedefd/d0Z edefd1d2Z!edefd3d4Z"edefd5d6Z#dSd7d8Z$defd9d:Z%d;eddfdd?Z'd@edefdAdBZ(dCedDedefdEdFZ)e*dGedHedIedJedededKedLedMedNedOe+dPej,ddfdQdRZ-dS)T PinholeCameraaeClass that represents a Pinhole Camera model. Args: intrinsics: tensor with shape :math:`(B, 4, 4)` containing the full 4x4 camera calibration matrix. extrinsics: tensor with shape :math:`(B, 4, 4)` containing the full 4x4 rotation-translation matrix. height: tensor with shape :math:`(B)` containing the image height. width: 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 extrinsicsheightwidthreturnNcCsp|||||g||d||d||d||d|||||g||_||_||_||_dS)Nrrrr) _check_valid_check_valid_params_check_valid_shape_check_consistent_devicerr _intrinsics _extrinsics)selfrrrrrR/home/ubuntu/.local/lib/python3.10/site-packages/kornia/geometry/camera/pinhole.py__init__.s     zPinholeCamera.__init__ data_itercCstdd|Ds tddS)Ncss|]}|jdVqdS)rN)shape).0datarrr >sz-PinholeCamera._check_valid..zArguments shapes must matchT)all ValueError)r!rrrr<szPinholeCamera._check_validr$ data_namecCs:t|jdvr|jdddkrtd|d|jdS)N)r*r* Argument z< shape must be in the following shape Bx4x4 or BxNx4x4. Got Tlenr"r'r$r(rrrrBs z!PinholeCamera._check_valid_paramscCs(t|jdkstd|d|jdS)Nr-z- shape must be in the following shape B. Got Tr.r0rrrrJsz PinholeCamera._check_valid_shapecCs |d}|D]}t||qdS)Nrr )r!firstr$rrrrPs z&PinholeCamera._check_consistent_devicecCs|jjS)zYReturn the device for camera buffers. Returns: Device type )rdevicerrrrr3VszPinholeCamera.devicecC||jds t|jS)zjThe full 4x4 intrinsics matrix. Returns: tensor of shape :math:`(B, 4, 4)`. r)rrAssertionErrorr4rrrr_zPinholeCamera.intrinsicscCr5)zjThe full 4x4 extrinsics matrix. Returns: tensor of shape :math:`(B, 4, 4)`. r)rrr6r4rrrrkr7zPinholeCamera.extrinsicscCs |jjdS)ziReturn the batch size of the storage. Returns: scalar with the batch size. r)rr"r4rrr batch_sizews zPinholeCamera.batch_sizecC |jdS)zpReturn the focal length in the x-direction. Returns: tensor of shape :math:`(B)`. .rrrr4rrrfx zPinholeCamera.fxcCr9)zpReturn the focal length in the y-direction. Returns: tensor of shape :math:`(B)`. .r1r1r;r4rrrfyr=zPinholeCamera.fycCr9)ztReturn the x-coordinate of the principal point. Returns: tensor of shape :math:`(B)`. .rr;r4rrrcxr=zPinholeCamera.cxcCr9)ztReturn the y-coordinate of the principal point. Returns: tensor of shape :math:`(B)`. .r1rAr;r4rrrcyr=zPinholeCamera.cycCr9)zwReturn the x-coordinate of the translation vector. Returns: tensor of shape :math:`(B)`. .rrr4rrrtxr=zPinholeCamera.txvaluecC||jd<|S)zDSet the x-coordinate of the translation vector with the given value.rErGrrIrrrrH cCr9)zwReturn the y-coordinate of the translation vector. Returns: tensor of shape :math:`(B)`. .r1rFrGr4rrrtyr=zPinholeCamera.tycCrJ)DSet the y-coordinate of the translation vector with the given value.rMrGrKrrrrNrLcCr9)zxReturns the z-coordinate of the translation vector. Returns: tensor of shape :math:`(B)`. .rArFrGr4rrrtzr=zPinholeCamera.tzcCrJ)rOrPrGrKrrrrQrLcCs|jdddddfS)zvReturn the 3x4 rotation-translation matrix. Returns: tensor of shape :math:`(B, 3, 4)`. .Nr)r*rGr4rrr rt_matrixzPinholeCamera.rt_matrixcC|jdddddfS)zReturn the 3x3 camera matrix containing the intrinsics. Returns: tensor of shape :math:`(B, 3, 3)`. .Nr)r;r4rrr camera_matrixrSzPinholeCamera.camera_matrixcCrT)z~Return the 3x3 rotation matrix from the extrinsics. Returns: tensor of shape :math:`(B, 3, 3)`. .Nr)rGr4rrrrotation_matrixrSzPinholeCamera.rotation_matrixcCs|jdddddfS)z}Return the translation vector from the extrinsics. Returns: tensor of shape :math:`(B, 3, 1)`. .Nr)rFrGr4rrrtranslation_vectorrSz PinholeCamera.translation_vectorcCs6|j}|j}|j}|j}t||||S)z2Return a deep copy of the current object instance.)rclonerrrr)rrrrrrrrrXs    zPinholeCamera.clonecCs |jS)z{Return the inverse of the 4x4 instrisics matrix. Returns: tensor of shape :math:`(B, 4, 4)`. )rinverser4rrrintrinsics_inverse s z PinholeCamera.intrinsics_inverse scale_factorcCsv|j}|d|9<|d|9<|d|9<|d|9<||j}||j}t||j||S)a/Scale the pinhole model. Args: scale_factor: a 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. r:r>r@rC)rrXrrrr)rr[rrrrrrscales zPinholeCamera.scalecCsh|jd|9<|jd|9<|jd|9<|jd|9<|j|9_|j|9_|S)a8Scale the pinhole model in-place. Args: scale_factor: a 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. r:r>r@rC)rrr)rr[rrrscale_)s zPinholeCamera.scale_point_3dcCs|j|j}tt||S)aProject a 3d point in world coordinates onto the 2d camera plane. Args: point_3d: tensor containing the 3d points to be projected to the camera plane. The shape of the tensor can be :math:`(*, 3)`. Returns: 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]]) )rrr r)rr^Prrrproject?s zPinholeCamera.projectpoint_2ddepthcCs&|j|j}t|}t|t||S)aUnproject a 2d point in 3d. Transform coordinates in the pixel frame to the world frame. Args: point_2d: tensor containing the 2d to be projected to world coordinates. The shape of the tensor can be :math:`(*, 2)`. depth: tensor containing the depth value of each 2d points. The tensor shape must be equal to point2d :math:`(*, 1)`. normalize: 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. Returns: 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]]) )rrrrr )rrarbr_P_invrrr unprojectXs zPinholeCamera.unprojectr<r?rBrDrHrNrQr8r3dtypec Cst| dd| | d} | d|7<| d|7<| d|7<| d|7<| dd7<| d d7<td| | d| d d }|d |7<|d |7<|d | 7<t| | | d}|d|7<t| | | d}|d|7<|| |||S)Nr*r3rer:r>r@rC).rArA?).r)r)r1rErMrP.r)r rrepeat)rr<r?rBrDrrrHrNrQr8r3rerr height_tmp width_tmprrrfrom_parameters{s zPinholeCamera.from_parameters)rr).__name__ __module__ __qualname____doc__rr staticmethodrboolrstrrrrrtorchr3propertyrrintr8r<r?rBrDrHsetterrfloatrNrQrRrUrVrWrXrZr\r]r`rd classmethodrrerlrrrrrs                #     rc@s`eZdZdZdeeddfddZdeeddfdd Zede fd d Z d e defd dZ dS)PinholeCamerasListamClass 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 tensor will be stacked in the first dimension. That's it, given a list of two camera instances, the intrinsics 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_listrNcCs||dS)N)_initialize_parameters)rr{rrrr szPinholeCamerasList.__init__pinholescCst|ttfstdt|gg}}gg}}|D](}t|ts,tdt|||j||j||j ||j qt |dd|_t |dd|_t |dd|_ t |dd|_ |S)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 r1dim) isinstancelisttuple TypeErrortyperappendrrrrrrr)rr}rrrrpinholerrrr|s       z)PinholeCamerasList._initialize_parameterscCs"d}|jdurt|jjd}|S)z0Return the number of pinholes cameras per batch.rFNr1)rrvr")r num_camerasrrrrs zPinholeCamerasList.num_camerasidxcCsN|jd|f}|jd|f}|jdd|f}|jdd|f}t||||S)zr}epsrcCst|jdkr|jddkst|jtj|dddfddd\}}}}td|j|jd|}|ddd |jd dd}||dd d df<||dd dd f<||ddddf<||dddd f<|S) aLReturn 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: tensor of pinhole models. eps: epsilon for numerical stability. Returns: 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]]]) rAr1 .Nr*r~rfrr) r/r"r6rtchunkrr3reviewri)r}rr<r?rBrDkrrrpinhole_matrixs $rrcCst|jdkr|jddkst|jtj|dddfddd\}}}}td|j|jd}|ddd |jd dd}d |||dd d df<d |||ddddf<d ||||dd dd f<d ||||dddd f<|S) 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: tensor with pinhole models. eps: epsilon for numerical stability. Returns: 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]]]) rAr1r.Nr*r~rfrrggr)r)rrr<r?rBrDrrrrinverse_pinhole_matrixs $rr\cCspt|jdkr|jddkst|jt|jdkrt|j|}|dddf|d|dddf<|S)aScale 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: tensor with the pinhole model. scale: tensor of scales. Returns: 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]]) rAr1r.NrF)r/r"r6rX unsqueeze)r}r\pinholes_scaledrrr scale_pinhole5s  &rcCs*t|jdkr|jddkst|jt)aCompute extrinsic transformation matrices for pinholes. Args: pinholes: tensor of form [fx fy cx cy h w rx ry rz tx ty tz] of size (N, 12). Returns: tensor of extrinsic transformation matrices of size (N, 4, 4). rAr1r)r/r"r6NotImplementedError)r}rrrget_optical_pose_base\s r pinhole_i pinhole_refcCsxt|jdkr|jddkst|j|j|jkrt|jt|}t|}t|t|}tt|t|t|S)aHomography 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) 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: tensor with pinhole model for ith frame. pinhole_ref: 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 rAr1r) r/r"r6rrtmatmulrrr)rr i_pose_base ref_pose_base i_pose_refrrrhomography_i_H_refrs&   rrbintrinsics_inv pixel_coordscCst|jdks|jddkrtd|jt|jdks%td|jt|jdks;|jddkr;td|jt|dddf|}||dd ddS) aTransform 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: tensor of shape BxHxWx3 with (x, y, z) cam coordinates. r*r1z3Input 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 NrrA)r/r"r'rpermute)rbrr cam_coordsrrr pixel2cams r-q=cam_coords_src dst_proj_srcc Cst|jdks|jddkrtd|jt|jdks.|jdddkr.td|jt|dddf|}|d}|d }|d }|||}|||}t||gd d } | S) aTransform 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: tensor of shape BxHxWx2 with (u, v) pixel coordinates. r*r)zs(  9..'4!