o
    Qi                     @   s    d Z ddlZdd Zdd ZdS )z1Lightweight geometry utils used by PARA training.    Nc                 C   s   | du rdS t j| d | d dgt jd}t j|}|| }|tt j|d }t j|dddf t jd}t j|ddddf t jd| }|tt j|d }||fS )	z/Unproject a 2D pixel to world-space camera ray.N)NNr      g      ?)dtypeg-q=   )nparrayfloat64linalginvmaxnormasarray)kp_2d_imagecamera_posecam_kZkp_hZk_invZray_camcam_posZ	ray_world r   '/data/cameron/para_videopolicy/utils.py_unproject_2d_to_ray   s   $r   c                 C   sx   | du s|du r
dS t | ||\}}|du s"|du s"t|d dk r$dS t||d  |d  }|dk r6dS |||  S )z2Recover 3D point from 2D pixel and world-z height.N   gư>r   )r   absfloat)r   heightr   r   r   Zray_directiontr   r   r   *recover_3d_from_direct_keypoint_and_height   s    r   )__doc__numpyr   r   r   r   r   r   r   <module>   s    