o
    5*je                      @   sx   d Z 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ZdZdZd	d
 Zdd ZG dd de	ZdS )a#  Two-view libero dataset reader.

Reads the dual-camera cache produced by prerender_dataset_2view.py. Per-sample yields:
  rgb_bev, rgb_wrist : (3, H, W) ImageNet-normalised
  trajectory_2d_bev  : (T, 2) in 448-image pixel coords
  trajectory_3d, _gripper, _quat, _euler  : (T, ...)
  start_pix_bev, _wrist                   : (2,)
  wrist_world_to_cam                      : (T, 4, 4) per-step
  bev_K_norm                              : (3, 3)
  bev_world_to_cam                        : (4, 4)
  wrist_K_norm                            : (3, 3)
    N)Path)Dataset)Rotation)g
ףp=
?gv/?gCl?)gZd;O?gy&1?g?z /data/libero/parsed_libero_2viewc                 C   s   |   }|d  t|9  < |d  t|9  < tjdtjd}|tj|ddddf< tj||tj }tj|}|tjS )uU  Robosuite's `get_camera_transform_matrix` returns `K_extended @ world_to_cam_extrinsic`,
    so to recover the pure extrinsic (cam→world) we need to undo the K multiplication.
    K_norm:           (3, 3) normalised
    world_to_cam_full: (4, 4) = K @ world_to_cam_extrinsic  (K on rows 0..2)
    Returns extrinsic (4, 4) cam→world.
    r         dtypeN   )	copyfloatnpeyefloat64astypelinalginvfloat32)ZK_normZworld_to_cam_full
image_sizeKZK_extZw2c_ext	extrinsic r   ./data/cameron/para/libero/data_libero_2view.py!_derive_extrinsic_from_projection   s   (,r   c                 C   s^   t t| }t |t jtjd }|jd |ks"|jd |kr-t j	|||ft j
d}|S )Ng     o@r   r   )interpolation)cv2ZimreadstrZcvtColorZCOLOR_BGR2RGBr   r   r   shaperesizeZINTER_LINEAR)pr   Zbgrrgbr   r   r   	_load_rgb*   s
   r    c                   @   s4   e Zd ZeddddddfddZd	d
 Zdd ZdS )CachedTrajectory2ViewDatasetZlibero_spatial)r   i     r	   r   c                    s*  || _ || _|| _t||    std  t fdd|D }g | _g | _|D ]}	|	 s4q-d}
t|		dD ]}|dkrI|
|krI nt|d 	d}t|d 	d}|ret
|t
|krfq=t
|}||t|d	 t|d
 t|d t|d t|d t|d t|d t|d t|d t|d tt|d |d}|d tj|d< t|d }t|D ]}t|d |d | |||< q|tj|d< | j| t|D ]}| jt
| jd |f q|
d7 }
q=q-tdt
| j dt
| j d d S )NzCache not found: c                    s   g | ]	} d |  qS )Ztask_r   ).0tidZ
bench_rootr   r   
<listcomp>?   s    z9CachedTrajectory2ViewDataset.__init__.<locals>.<listcomp>r   zdemo_*Z
frames_bevz*.pngZframes_wristzeef_pos.npyzeef_quat.npyzgripper.npyzpix_uv_bev.npyzpix_uv_wrist.npyzbev_K_norm.npyzbev_extrinsic.npyzbev_world_to_cam.npyzwrist_K_norm.npyzwrist_world_to_cam.npyz
base_z.npy)
bev_frameswrist_frameseef_poseef_quatgripper
pix_uv_bevZpix_uv_wrist
bev_K_normbev_extrinsicZbev_world_to_camwrist_K_normwrist_world_to_camZbase_zTr.   r0   r/   wrist_extrinsicr   zCachedTrajectory2ViewDataset: z demos, z samples)r   n_windowframe_strider   existsFileNotFoundErrorsorteddemossamplesgloblenr   loadr   r   r   
zeros_likeranger   appendprint)selfZ
cache_rootZbenchmark_nametask_idsr   r3   r4   	max_demosZ	task_dirsZtask_dirZtask_demo_countZdemo_dirr'   r(   r1   demoZ	wrist_exttr   r%   r   __init__3   sb   

&z%CachedTrajectory2ViewDataset.__init__c                 C   s
   t | jS )N)r;   r9   )rA   r   r   r   __len__p   s   
z$CachedTrajectory2ViewDataset.__len__c                 C   s  | j | \}}| j| }|d }g g g g f\}}}}	d  }
}d }t| jD ]c}t||| j  |d }|dkrQt|d | | j}
t|d | | j}|d | }||d | 	t
j |	|d | 	t
j |tt
|d	 | d
d ||d | 	t
j q%t
j|dd}t
j|dd	t
j}t
j|t
jd}t
j|	dd	t
j}	t
jdd |	D dd	t
j}t
jtt
jdddd}t
jtt
jdddd}t|
ddd	t
j}t|ddd	t
j}|t| t| }|t| t| }||t|t|t|t|	t|t|t|d t|d t|d tj|tjdtj|tjddS )Nr1   r   r   r'   r(   r2   r)   r*   r+   g      g      ?r,   )axisr   c                 S   s   g | ]
}t |d qS )xyz)ScipyRZ	from_quatZas_euler)r#   qr   r   r   r&      s    z<CachedTrajectory2ViewDataset.__getitem__.<locals>.<listcomp>r	      r-   r.   r/   )Zrgb_bevZ	rgb_wristZtrajectory_2d_bevZtrajectory_3dZtrajectory_gripperZtrajectory_quatZtrajectory_eulerr2   r-   r.   r/   demo_idxstart_t)r9   r8   r>   r3   minr4   r    r   r?   r   r   r   r   clipr   stackarrayIMAGENET_MEANreshapeIMAGENET_STDtorch
from_numpy	transposetensorlong)rA   idxrM   rN   rD   r1   Ztraj_2d_bevZtraj_3dZ	traj_gripZ	traj_quatZ
rgb_bev_t0Zrgb_wrist_t0Zwrist_extrinsic_t0krE   Z
traj_eulermeanstdZ	rgb_bev_tZrgb_wrist_tr   r   r   __getitem__s   sZ   
 z(CachedTrajectory2ViewDataset.__getitem__N)__name__
__module____qualname__DEFAULT_CACHErF   rG   r_   r   r   r   r   r!   2   s    
=r!   )__doc__ospathlibr   r   numpyr   rV   torch.utils.datar   Zscipy.spatial.transformr   rJ   rS   rU   rc   r   r    r!   r   r   r   r   <module>   s    