o
    ߥi0                     @   s`   d dl Z d dlmZ d dlZd dlZd dlZd dlmZ	 d dlm
Z
 d	ddZG dd dZdS )
    N)glob)Rotation)Slerpc           	      C   s  |d u r/t |   }t|dkr|dd  }dd dd |D D }t|tj }t	
|}|d }|d }|d }||d	  }td}||d d
d d
f< tjdtjd}| |d d
d d
f< |d d
 |d
  d d df |d d
d
f< ||fS )N      c                 S   s(   g | ]}|d  |d |d |d gqS )r   r          .0xr	   r	   e/home/ubuntu/.local/lib/python3.10/site-packages/modelscope/models/cv/surface_recon_common/dataset.py
<listcomp>   s     z$load_K_Rt_from_P.<locals>.<listcomp>c                 s   s    | ]}| d V  qdS ) N)splitr
   r	   r	   r   	<genexpr>   s    z#load_K_Rt_from_P.<locals>.<genexpr>r   r   )r   r   r   )dtype)openread
splitlineslennpasarrayastypefloat32squeezecvdecomposeProjectionMatrixeye	transpose)	filenamePlinesoutKRt
intrinsicsposer	   r	   r   load_K_Rt_from_P   s&   

,r)   c                       sl   e Zd Z fddZdddZdd Zddd	Zd
d Zdd ZdddZ	dddZ
dd Zdd Z  ZS )Datasetc                    s(  t t|   td || _|| _td| j ttj	
| jd  | _tttj	
| jd| _t| j| _td| j   fddt| jD | _ fddt| jD | _g | _g | _t| j| jD ]/\}}|| }|d d	d d
f }td |\}}| jt|  | jt|  qft| j| j| _t| j| _| jd d | _ t| j| j| _t!g d}t!g d}	ttj	
| jdd }
tj"#| jd |
 |d d d f  }tj"#| jd |
 |	d d d f  }	|d d	df | _$|	d d	df | _%td d S )NzLoad data: Beginz
data_dir: zcameras_sphere.npzzimage/*.pngzfound %d imagesc                        g | ]} d |   tjqS )zworld_mat_%dr   r   r   r   idxcamera_dictr	   r   r   :       z$Dataset.__init__.<locals>.<listcomp>c                    r+   )zscale_mat_%dr,   r-   r/   r	   r   r   >   r1   r   r   r   )r   r   ))\(r2   r2         ?))\(?r4   r4   r3   scale_mat_0zLoad data: End)&superr*   __init__printdevicedata_dirr   loadospathjoinr0   sortedr   
images_lisr   n_imagesrangeworld_mats_npscale_mats_npintrinsics_allpose_allzipr)   appendtorch
from_numpyfloatstacktoinverseintrinsics_all_invfocalarraylinalginvobject_bbox_minobject_bbox_max)selfr:   r9   	scale_mat	world_matr!   r'   r(   rT   rU   object_scale_mat	__class__r/   r   r7   +   s   

zDataset.__init__r   c              	   C   s2  |}t d| jd | j| }t d| jd | j| }t ||\}}t j||t |gdd}t | j|ddddddf |dddddddf 	 }|t j
j|dddd	 }	t | j|ddddddf |	dddddddf 	 }	| j|dddddf |	j}
|
dd|	ddfS 
z?
        Generate rays at world space from one camera.
        r   r   dimNr   r   Tordr_   keepdim)rI   linspaceWHmeshgridrL   	ones_likematmulrO   r   rR   normrF   expandshaper   )rV   img_idxresolution_levelleveltxtypixels_xpixels_yprays_vrays_or	   r	   r   gen_rays_ate   s4   zDataset.gen_rays_atc                 C   s   | j |dddf }|S )zA
        Generate rays_o at world space from one camera.
        Nr   )rF   )rV   rl   ru   r	   r	   r   gen_rays_o_aty   s   zDataset.gen_rays_o_atc              	   C   s"  |}t d| jd | j| }t d| jd | j| }t ||\}}t j||t |gdd}t | jdddddddf |dddddddf 	 }|t j
j|dddd	 }	t |ddddf |	dddddddf 	 }	|dddf |	j}
|
dd|	ddfS r\   )rI   rc   rd   re   rf   rL   rg   rh   rO   r   rR   ri   rj   rk   r   )rV   r(   rm   rn   ro   rp   rq   rr   rs   rt   ru   r	   r	   r   gen_rays_at_camera   s,   *zDataset.gen_rays_at_camerac                 C   b  t jd| j|gd}t jd| j|gd}| j| ||f }| j| ||f }| j| ||f }t j||t |gdd	 }t 
| j|dddddf |dddddf  }|t jj|dddd	 }	t 
| j|dddddf |	dddddf  }	| j|ddddf |	j}
t j|
 |	 ||dddd
f |dddf gdd S zF
        Generate random rays at world space from one camera.
        r   )lowhighsizer]   r^   Nr   r   Tr`   r   rI   randintrd   re   imagesmasksdepthsrL   rg   rK   rh   rO   r   rR   ri   rF   rj   rk   catcpucudarV   rl   
batch_sizerq   rr   colormaskdepthrs   rt   ru   r	   r	   r   gen_random_rays_at   J   (zDataset.gen_random_rays_atc                 C   ry   rz   r~   r   r	   r	   r   gen_random_rays_at_mask   r   zDataset.gen_random_rays_at_maskc              	   C     |}t d| jd | j| }t d| jd | j| }t ||\}}	t j||	t |	gdd}
t | jdddddddf |
dddddddf 	 }
|
t j
j|
dddd	 }| j|dddf d
|  | j|dddf |  }| j|    }| j|    }tj
|}tj
|}|ddddf }|ddddf }tt||g}ddg}t||}||}tg d}|tj}| |ddddf< d
| | ||  dddf |dddf< tj
|}t |ddddf  }t |dddf  }t |ddddddf |dddddddf 	 }|ddddf |j}|dd|dd|fS z7
        Interpolate pose between two cameras.
        r   r   r]   r^   Nr   r   Tr`   r3   )r3   r3   r3   r3   rI   rc   rd   re   rf   rL   rg   rh   rO   r   rR   ri   rF   detachr   numpyr   rS   Rotfrom_matrixr   diagr   r   	as_matrixrJ   r   rj   rk   r   rV   idx_0idx_1ratiorm   rn   ro   rp   rq   rr   rs   rt   transpose_0pose_1rot_0rot_1rots	key_timesslerprotr(   ru   r	   r	   r   gen_rays_between   X   
,zDataset.gen_rays_betweenc              	   C   r   r   r   r   r	   r	   r   gen_rays_across   r   zDataset.gen_rays_acrossc                 C   sR   t j|d ddd}dt j|| ddd }d|  | }|d }|d }||fS )Nr   r]   T)r_   rb   g       @g      ?r3   )rI   sum)rV   ru   rays_dabmidnearfarr	   r	   r   near_far_from_sphere  s   zDataset.near_far_from_spherec                 C   s4   t | j| }t || j| | j| fddS )Nr      )r   imreadr@   resizerd   re   clip)rV   r.   rm   imgr	   r	   r   image_at  s   zDataset.image_at)r   )__name__
__module____qualname__r7   rv   rw   rx   r   r   r   r   r   r   __classcell__r	   r	   rZ   r   r*   )   s    
:


&&r*   )N)r<   r   cv2r   r   r   rI   scipy.spatial.transformr   r   r   r)   r*   r	   r	   r	   r   <module>   s   
