o oi0@sddlmZddlZddlmZmZmZddlmZddl m Z Gddde Z Gdd d Z d ed dfd d Zded dfddZd eded efddZdS))AnyN)Tensorstackzeros)transform_points)create_meshgridcs.eZdZdedededdffdd ZZS)StereoExceptionmsgargskwargsreturnNcs*d}||}tj|g|Ri|dS)aqConstruct custom exception for the :module:`~kornia.geometry.camera.stereo` module. Adds a general helper module redirecting the user to the proper documentation site. Args: msg: Custom message to add to the general message. *args: Additional argument passthrough **kwargs: Additional argument passthrough z Please check documents here: https://kornia.readthedocs.io/en/latest/geometry.camera.stereo.html for further information and examples.N)super__init__)selfr r r doc_help final_msg __class__Q/home/ubuntu/.local/lib/python3.10/site-packages/kornia/geometry/camera/stereo.pyrs zStereoException.__init__)__name__ __module__ __qualname__strrr __classcell__rrrrrs&rc@seZdZdededdfddZedededdfddZedefd d Z edefd d Z edefd dZ edefddZ edefddZ edefddZedefddZedefddZdefddZdedefddZdS) StereoCamerarectified_left_camerarectified_right_camerar NcCs:|||||_||_|jj|_|jj|_||_dS)a?Class representing a horizontal stereo camera setup. Args: rectified_left_camera: The rectified left camera projection matrix of shape :math:`(B, 3, 4)` rectified_right_camera: The rectified right camera projection matrix of shape :math:`(B, 3, 4)` N)_check_stereo_camerarrdevicedtype_init_Q_matrix _Q_matrix)rrrrrrr2s   zStereoCamera.__init__c Cst|jdkrtd|jdt|jdkr td|jd|jdddkr6td|jddd|jdddkrLtd |jddd|j|jkr_td |jd |jd|j|jkrrtd |jd |jdtt|d ddddf|d ddddfstd|d ddddfd |d ddddfd|d}tt|drtd|ddS)a9Ensure user specified correct camera matrices. Args: rectified_left_camera: The rectified left camera projection matrix of shape :math:`(B, 3, 4)` rectified_right_camera: The rectified right camera projection matrix of shape :math:`(B, 3, 4)` z;Expected 'rectified_left_camera' to have 3 dimensions. Got .z;Expected 'rectified_right_camera' to have 3 dimension. Got N)r#z@Expected each 'rectified_left_camera' to be of shape (3, 4).Got zAExpected each 'rectified_right_camera' to be of shape (3, 4).Got z\Expected 'rectified_left_camera' and 'rectified_right_camera' to be on the same devices.Got z and zTExpected 'rectified_left_camera' and 'rectified_right_camera' tohave same dtype.Got .ztExpected 'left_rectified_camera' and 'rectified_right_camera' to havesame parameters except for the last column.Got .rr#rz/Expected :math:`T_x * f_x` to be negative. Got ) lenshaperrr torchalleqgt)rrtx_fxrrrrEsX     6z!StereoCamera._check_stereo_cameracCs |jjdS)zgReturn the batch size of the storage. Returns: scalar with the batch size r)rr)rrrr batch_sizes zStereoCamera.batch_sizecC |jdS)zReturn the focal length in the x-direction. Note that the focal lengths of the rectified left and right camera are assumed to be equal. Returns: tensor of shape :math:`(B)` ).rrrr/rrrfx zStereoCamera.fxcCr1)zReturns the focal length in the y-direction. Note that the focal lengths of the rectified left and right camera are assumed to be equal. Returns: tensor of shape :math:`(B)` ).r%r%r2r/rrrfyr4zStereoCamera.fycCr1)zReturn the x-coordinate of the principal point for the left camera. Returns: tensor of shape :math:`(B)` .rr2r/rrrcx_left zStereoCamera.cx_leftcCr1)zReturn the x-coordinate of the principal point for the right camera. Returns: tensor of shape :math:`(B)` r6)rr/rrrcx_rightr9zStereoCamera.cx_rightcCr1)zReturn the y-coordinate of the principal point. Note that the y-coordinate of the principal points is assumed to be equal for the left and right camera. Returns: tensor of shape :math:`(B)` ).r%r7r2r/rrrcyr4zStereoCamera.cycCs|jd |jS)ztThe horizontal baseline between the two cameras. Returns: Tensor of shape :math:`(B)` r')rr3r/rrrtxszStereoCamera.txcCs|jS)aQThe Q matrix of the horizontal stereo setup. This matrix is used for reprojecting a disparity tensor to the corresponding point cloud. Note that this is in a general form that allows different focal lengths in the x and y direction. Return: The Q matrix of shape :math:`(B, 4, 4)`. )r"r/rrrQs zStereoCamera.QcCst|jddf|j|jd}|j }|j||ddddf<|j |j||ddddf<|j||ddddf<|j |j||ddddf<|j|j||ddddf<|j |ddddf<|j|j|j |ddddf<|S)zInitialize the Q matrix of the horizontal stereo setup. See the Q property. Returns: The Q matrix of shape :math:`(B, 4, 4)`. r&)rr Nrr#r%r7) rr0rr r<r5r8r3r;r:)rr=baselinerrrr!s   zStereoCamera._init_Q_matrixdisparity_tensorcCs t||jS)zReproject the disparity tensor to a 3D point cloud. Args: disparity_tensor: Disparity tensor of shape :math:`(B, 1, H, W)`. Returns: The 3D point cloud of shape :math:`(B, H, W, 3)` )reproject_disparity_to_3Dr=)rr?rrrr@s z&StereoCamera.reproject_disparity_to_3D)rrrrr staticmethodrpropertyintr0r3r5r8r:r;r<r=r!r@rrrrr1s,<        rr?r cCst|tstdt|dt|jdkrtd|jd|jddkr/td|jd|jtjtj tj fvrBtd|jd S) zEnsure correct user provided correct disparity tensor. Args: disparity_tensor: The disparity tensor of shape :math:`(B, 1, H, W)`. z@Expected 'disparity_tensor' to be an instance of Tensor but got r$r&z6Expected 'disparity_tensor' to have 4 dimensions. Got r%z]Expected dimension 1 of 'disparity_tensor' to be 1 for as single channeled disparity map.Got z\Expected 'disparity_tensor' to have dtype torch.float16, torch.float32 or torch.float64.Got N isinstancerrtyper(r)r r*float16float32float64)r?rrr_check_disparity_tensors& rKQ_matrixcCst|tstdt|dt|jdkstd|j|jdddks/td|j|jtjtj tj fvrBtd |jdS) zEnsure Q matrix is of correct form. Args: Q_matrix: The Q matrix for reprojecting disparity to a point cloud of shape :math:`(B, 4, 4)` z8Expected 'Q_matrix' to be an instance of Tensor but got r$r#z.Expected 'Q_matrix' to have 3 dimensions. Got r%N)r&r&zFExpected last two dimensions of 'Q_matrix' to be of shape (4, 4). Got zUExpected 'Q_matrix' to be of type torch.float16, torch.float32 or torch.float64. Got rE)rLrrr_check_Q_matrixs  rMc Cst|t||j\}}}}|j}|j}t||d||d}||ddd}tj|dd\} } t | dt | d} } t | | |fd |dd ddd} t ||  |||d} | j|||dfksrtd |||dfd | jd | S) aReproject the disparity tensor to a 3D point cloud. Args: disparity_tensor: Disparity tensor of shape :math:`(B, H, W, 1)`. Q_matrix: Tensor of Q matrices of shapes :math:`(B, 4, 4)`. Returns: The 3D point cloud of shape :math:`(B, H, W, 3)` F)normalized_coordinatesrr rD)dimr%r#rr7z]Something went wrong in `reproject_disparity_to_3D`. Expected the final outputto be of shape z(.But the computed point cloud had shape zO. Please ensure input are correct. If this is an error, please submit an issue.)rMrKr)r rrexpandr*unbind unsqueezerreshapepermuterr) r?rLr0rowscols_r ruvvuuvdpointsrrrr@0s( $ r@)typingrr* kornia.corerrrkornia.geometry.linalgrkornia.utils.gridr ExceptionrrrKrMr@rrrrs   N