o
    )j                     @   s   d Z ddlZddl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 ejdd ddlmZmZ dZddlZd	Zd
Zdd ZG dd deZdS )u  All-in-memory data loader for DA3 pixel-aligned training (smith300 robot data).

Per sample:
  rgb:           (3, 504, 504) float32 in [0, 1] — DA3 expects [0, 1] (not ImageNet-normed)
  gt_pix_504:    (N_WINDOW, 2) GT EEF pixel coords in 504-space (model trains heatmap at 288 res
                  — we scale at loss-time)
  gt_pix_valid:  (N_WINDOW,) bool — False for clamped (off-episode) future steps
  da3_depth:     (504, 504) float32 — frozen DA3 depth, used as distillation target
    N)Path)Dataset)Rotationz/data/cameron/para/para_mac)EEF_BODY_NAME_scale_K_toz-/data/cameron/para/libero/example_twolink.xmli     c           
      C   s   t | jd df}t j| |gdd}||j jddddf }t |dddf dd}|ddddf |dddf  }t j|t |jd dfgdd}||j jddddf }	|	|dddf fS )	u>   world_pts: (N, 3) → (N, 2) (u, v) pixel + (N,) depth. Numpy.r      axisN      gMbP?)nponesshapeconcatenateTclip)
Z	world_ptsZworld_to_cameraKr   Zpts_hZcamznormZhomogpix r   +/data/cameron/para/libero/data_da3_pixel.pyproject   s   $"r   c                   @   s2   e Zd ZdeededfddZdd Zdd	 Zd
S )Smith300DA3Datasetz8/data/cameron/mac_robot_datasets/first_mobile_collectionr   	da3_depthc           1         sf  || _ || _|| _|| _t|}tdd | D }|s$td| tdt	| d t
j|}	t
|	}
t
|	t
jjt}|	j}g | _g | _g | _g | _g | _t|D ]\  d }| sfqXtt|}|d \}}tj|d tjd	}tj|d
 tjd	}tj|d tjd	}|| }t ||f||}t! fdddD d }|d u rqXtt|d }t d }tj"|d tjd	}|j#d }|j#d }t$ }|D ]}t%t&|d t't&|d |d d D ]}|(| qqi }t|D ]}tj)|tjd	} ||d |f | d t'||< | |
j*d |< t
+|	|
 |
j,| - }!t.|!/dd||\}"}#|"d 0tj1}"d|"d   krK|k r\n nd|"d   kr[|k s]n q d|dd }$ | j  d|dd }%|$ r||% s}qt23t4|$}&|&d u rqt25|&t2j6}'t2j7|'||ft2j8d}'|'0tj1d }'|'9ddd}(t|%0tj1})t	| j}*|*||< | j:|( | j:|) | j:|" | j: q|D ]@}g }+t%t&|d t't&|d |d d D ]}||v r|+:||  qt	|+dk r	q| j:dtj"|+tj;d	i qtd j< d t=fd!d| jD  d" qXt>?tj@| jdd#| _t>?tj@| jdd#| _t>?tj@| jdd#| _g | _At| jD ]\},}t%t	|d d D ]}-| jA:|,|-f qjq\t	| j}.| jB | jC  d$ }/| jB | jC  d$ }0td%t	| j d&|. d't	| jA d(|/d)d*|0d)d+ d S ),Nc                 S   s   g | ]}|  r|qS r   )is_dir).0dr   r   r   
<listcomp>4   s    z/Smith300DA3Dataset.__init__.<locals>.<listcomp>zNo sessions under zSmith300DA3Dataset: loading z	 sessionsz	meta.jsonZimage_size_whr   dtypeT_camera_arucoBaseZT_W_baseBody_inv_aruco_offsetc                 3   s$    | ]} |   r | V  qd S N)exists)r   p)sessr   r   	<genexpr>R   s    
z.Smith300DA3Dataset.__init__.<locals>.<genexpr>)zrgb_overlay/episodes.jsonzepisodes.jsonepisodesz
joints.npzZq_motorsr   r   startendr   Zrgb_Z06dz.jpgz.npy)interpolationg     o@r   framesz  z: c                 3   s    | ]	}| krd V  qdS )r   Nr   )r   s)sess_idxr   r   r(      s    z frames loadedr
   g    eAzSmith300DA3Dataset ready: z eps, z	 frames, z samples, rgb=z.2fz GB, depth=z GB)Ddepth_subdir
image_sizen_windowr.   r   sortediterdirFileNotFoundErrorprintlenmujocoZMjModelZfrom_xml_pathZMjDataZ
mj_name2idZmjtObjZ
mjOBJ_BODYr   nqr)   rgb_tdepth_tpix_tZsession_idx	enumerater%   jsonloadopenr   arrayfloat64r   nextasarrayr   setrangeintminaddzerosZqposZ
mj_forwardxposcopyr   reshapeastypefloat32cv2imreadstrcvtColorCOLOR_BGR2RGBresizeINTER_LINEAR	transposeappendint64namesumtorch
from_numpystacksampleselement_sizenumel)1selfroot_dirr1   r2   frame_strideZ
mujoco_xmlr0   rootsessionsZmj_modelZmj_dataZeef_idZn_qpos	meta_pathmetaZIMG_WZIMG_HZK_origr#   ZT_W_baseBodyZT_CAM_WORLDZK_targetZep_pathZsess_episodesZjointsZq_motors_allZn_motorsn_framesZneededepfZlocal_to_globalqZeef_posr   _Zimg_pathZ
depth_pathZbgrrgbZrgb_chwdepthZg_idxfsep_idxtnZgb_rgbZgb_dr   )r'   r/   r   __init__+   s   


,>
,
 .
zSmith300DA3Dataset.__init__c                 C   s
   t | jS r$   )r7   r_   )rb   r   r   r   __len__   s   
zSmith300DA3Dataset.__len__c              	      s   | j | \}| j| d }t|}| jt| }|d  fddt| jD } fdd|D }tj fdd|D tj	d}|| }	| j
|	 }
| j| }| j| }||
||tj|tjdtjtjddS )	Nr-   r   c                    s   g | ]
}|d     qS )r   r   r   i)r.   rr   r   r   r       s    z2Smith300DA3Dataset.__getitem__.<locals>.<listcomp>c                    s   g | ]}t | qS r   )rH   rv   	last_realr   r   r       s    c                    s   g | ]}| kqS r   r   )r   rawrx   r   r   r       s    r!   )rn   
gt_pix_504gt_pix_validr   rq   start_t)r_   r)   r7   r.   rG   rF   r2   r\   tensorboolr<   r:   r;   long)rb   idxrq   r-   LZcur_gZfuture_local_rawZfuture_localvalidZfuture_globalgt_pixrn   ro   r   )ry   r.   rr   r   __getitem__   s(   


zSmith300DA3Dataset.__getitem__N)	__name__
__module____qualname__	DA3_INPUTN_WINDOWDEFAULT_SMITH300_XMLrt   ru   r   r   r   r   r   r   *   s    
rr   )__doc__ossysr>   pathlibr   rP   numpyr   r\   torch.utils.datar   Zscipy.spatial.transformr   ZScipyRpathinsertZdata_smith300_parar   r   r   r8   r   r   r   r   r   r   r   r   <module>   s     	