o
    *j0                     @   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	   r/var/www/html/Deteccion_Ine/venv/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   )Zdtype)openread
splitlineslennpZasarrayastypefloat32squeezecvZ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   >   r.   r   r   r   )r   r   ))\(r/   r/         ?))\(?r1   r1   r0   Zscale_mat_0zLoad data: End)&superr'   __init__printdevicedata_dirr   loadospathjoinr-   sortedr   
images_lisr   Zn_imagesrangeZworld_mats_npZscale_mats_npZintrinsics_allpose_allzipr&   appendtorch
from_numpyfloatstacktoZinverseintrinsics_all_invZfocalarraylinalginvobject_bbox_minobject_bbox_max)selfr6   r5   Z	scale_matZ	world_matr   r$   r%   rJ   rK   Zobject_scale_mat	__class__r,   r   r3   +   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ordrR   keepdim)rA   linspaceWHmeshgridrD   	ones_likematmulrF   r   rH   normr>   expandshaper   )rL   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   )r>   )rL   r_   rh   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 rO   )rA   rV   rW   rX   rY   rD   rZ   r[   rF   r   rH   r\   r]   r^   r   )rL   r%   r`   ra   rb   rc   rd   re   rf   rg   rh   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sizerP   rQ   Nr   r   TrS   r   rA   randintrW   rX   ZimagesZmasksZdepthsrD   rZ   rC   r[   rF   r   rH   r\   r>   r]   r^   catcpucudarL   r_   Z
batch_sizerd   re   colormaskdepthrf   rg   rh   r	   r	   r   gen_random_rays_at   J   (zDataset.gen_random_rays_atc                 C   rl   rm   rq   rv   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   rP   rQ   Nr   r   TrS   r0   )r0   r0   r0   r0   rA   rV   rW   rX   rY   rD   rZ   r[   rF   r   rH   r\   r>   detachrt   numpyr   rI   RotZfrom_matrixr   Zdiagr   r   Z	as_matrixrB   ru   r]   r^   r   rL   Zidx_0Zidx_1ratior`   ra   rb   rc   rd   re   rf   rg   ZtransZpose_0Zpose_1Zrot_0Zrot_1ZrotsZ	key_timesZslerpZrotr%   rh   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   rP   T)rR   rU   g       @g      ?r0   )rA   sum)rL   rh   Zrays_dabmidZnearZ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   Zimreadr<   resizerW   rX   Zclip)rL   r+   r`   Zimgr	   r	   r   image_at  s   zDataset.image_at)r   )__name__
__module____qualname__r3   ri   rj   rk   rz   r|   r   r   r   r   __classcell__r	   r	   rM   r   r'   )   s    
:


&&r'   )N)r8   r   Zcv2r   r   r   rA   Zscipy.spatial.transformr   r   r   r&   r'   r	   r	   r	   r   <module>   s   
