o
    "i
E                  
   @   s(  d Z ddlZddlZddlmZ ddlmZ ddlm	Z
 ddlZddlZdZdZdZdd	 Zeg d
g dg dg dg dgd ZdZee Zdd ZG dd deZedkred zedddZedee d ed Zed eded j  eded  d d!ed  d d" ed#ed$ j  ed%ed$ jd&d'  ed(ed) j  ed*ed) dd   ed+ed, j  ed-ed.   ed/ed0   W n" e y Z! zed1e!  ddl"Z"e"#  W Y dZ![!ndZ![!ww ed2d3  ed4 dS dS )5z-Dataset for real dense trajectory prediction.    N)Dataset)Path   皙ɿ皙?c                 C   s,   | }|dkrd}|dkrd}t ttt|S )a|  Process gripper value: fix wrapping and clamp to range.
    
    Args:
        gripper_value: scalar gripper joint value
    
    Returns:
        Processed gripper value in [MIN_GRIPPER, MAX_GRIPPER] = [-0.2, 0.8]
        Processing steps:
        1. Map values > 4.0 to -0.2 (wraparound fix)
        2. Clamp values > 0.8 to 0.8
        3. Ensure final range is [-0.2, 0.8]
    g      @r   r   )maxMIN_GRIPPERminMAX_GRIPPER)gripper_valuex r   3/data/cameron/keygrip/volume_tracks_smolvla/data.pyprocess_gripper_value   s   r   )     *@{GVg/@)g
ףp=%@gfffffXr   )r   r   g/)g(\1@g=
ףpTr   )g\(6@g=
ףpQr   g     @@   c                 C   sL   t | d}|| }|d dkrdS ||dd  }|dd |d  }|S )a0  Project 3D point to 2D pixel coordinates.
    
    Args:
        point_3d: (3,) 3D point in world coordinates
        camera_pose: (4, 4) camera pose matrix (world-to-camera)
        cam_K: (3, 3) camera intrinsics
    
    Returns:
        (2,) 2D pixel coordinates [x, y], or None if behind camera
          ?   r   Nr   )npappend)point_3dcamera_posecam_K
point_3d_h	point_cam
point_2d_hpoint_2dr   r   r   project_3d_to_2dD   s   r   c                   @   s<   e Zd ZdZddedB dedB fddZd	d
 Zdd ZdS )RealTrajectoryDataseta  Dataset for real dense trajectory prediction.
    
    Each sample contains:
        - RGB image (from any frame)
        - Dense 2D trajectory (N_WINDOW waypoints starting from that frame)
        - Camera parameters (pose and intrinsics)
        - Ground truth heatmaps for each timestep
    
    For each episode, creates samples from every frame, with trajectories
    padded with the last observed keypoint if there aren't enough subsequent frames.
    scratch/  Nepisodemax_episodesc           
         s.  t || _|| _t| _| j std| j tdd | j D } durB fdd|D }t	|dkrBtd  d| j |durL|d| }t	|dkrZtd	| j g | _
|D ] }td
d |dD }|D ]}t|j}	| j
||	f qoq_tdt	| d tdt	| j
 d dS )an  Initialize dataset.
        
        Args:
            dataset_root: Root directory containing episodes
            image_size: Size to resize images to (will be square, default 448)
            episode: Optional episode directory name (e.g. "episode_001") to load only.
            max_episodes: Optional limit on how many episodes to load (after sorting).
        zDataset directory not found: c                 S   s"   g | ]}|  rd |jv r|qS r"   )is_dirname.0dr   r   r   
<listcomp>{   s    
z2RealTrajectoryDataset.__init__.<locals>.<listcomp>Nc                    s   g | ]	}|j  kr|qS r   )r&   r'   r$   r   r   r*          r   z	Episode 'z' not found in zNo episodes found in c                 S      g | ]	}|j  r|qS r   stemisdigitr(   fr   r   r   r*      r+   *.pngzLoaded z	 episodeszCreated z samples (one per frame))r   dataset_root
image_sizeN_WINDOWn_windowexists
ValueErrorsortediterdirlensamplesglobintr.   r   print)
selfr3   r4   r"   r#   episode_dirsepisode_dirframe_files
frame_file	frame_idxr   r$   r   __init__j   s.   
	

zRealTrajectoryDataset.__init__c                 C   s
   t | jS )N)r;   r<   )r@   r   r   r   __len__   s   
zRealTrajectoryDataset.__len__c           =      C   sP  | j | \}}|d}|| d }t|dddf }|jdd \}}tdd |d	D }	d
d |	D }
|
|}g }g }g }t| jD ]}|| t	|
krU n|
||  }|d}|| d }|
 sl nt|}|ddddf }|dddf }|t | }|| || d }|
 rt|}t|d }t|}|| n|d || d }|| d }|
 r|
 s n0t|}t|}| }|d  |9  < |d  |9  < t|||}|du r n|| qIt	|dkrt|| d }|ddddf }|dddf }|t | }|g}|| d }|
 rFt|}t|d }t|}|g}ndg}t|| d }t|| d }| }|d  |9  < |d  |9  < t|||}|dur|g}t	|dkrtd|j d| t|}t|}t|}t	|| jk r|dd } |dd }!|dd }"| jt	| }#tj|t| |#dfgdd}tj|t|!|#dfgdd}tj|t|"|#fgdd}nt	|| jkr|d| j }|d| j }|d| j }|d }$t|| d }t|| d }|| jks/|| jkrOtj|| j| jftjd}| j| }%| j| }&|t|%|&g }g }'t| jD ]g}(||( })tt|)d d| jd t|)d d| jd g})tj| j| jftjd}*tt |)d }+tt |)d },d|+  kr| jk rn nd|,  kr| jk rn nd|*|,|+f< |'|* qVt|'}'t!"|#ddd }-|-$ dkr|-d }-t!%g d&ddd}.t!%g d&ddd}/|-|. |/ }-t!"|' }0t!"| }1t!"| }2t!"| }3t!"|$ }4t!"| }5t!"| }6|| d }7d}8|7
 rKzt|7}8W n t'yJ   d}8Y nw |d }9d}:|9
 rjzt|9}:W n t'yi   d}:Y nw |8durzt!"tj(|8tjdnd};|:durt!"tj(|:tjdnd}<|-|0|1|2|3|4|5|6|j d| |j|t)||;|<dS )a  Get a single sample.
        
        Returns:
            dict with keys:
                - rgb: (3, H, W) normalized RGB image
                - heatmap_target: (N_WINDOW, H, W) one-hot heatmaps for each timestep
                - trajectory_2d: (N_WINDOW, 2) 2D pixel locations for each timestep
                - trajectory_3d: (N_WINDOW, 3) 3D world positions for each timestep
                - trajectory_gripper: (N_WINDOW,) gripper values for each timestep
                - target_3d: (3,) final target 3D world position (last waypoint)
                - camera_pose: (4, 4) camera pose matrix (from first frame)
                - cam_K_norm: (3, 3) normalized intrinsics (from first frame)
        06dz.png.Nr   r   c                 S   r,   r   r-   r0   r   r   r   r*      r+   z5RealTrajectoryDataset.__getitem__.<locals>.<listcomp>r2   c                 S   s   g | ]}t |jqS r   )r>   r.   r0   r   r   r   r*      s    z_gripper_pose.npyz.npyr   z
_cam_K.npyz_camera_pose.npyr      z!Could not compute trajectory for z frame )axis)interpolation)dtypeg     o@)g
ףp=
?gv/?gCl?)gZd;O?gy&1?g?z
000000.npy_frame_)rgbheatmap_targettrajectory_2dtrajectory_3dtrajectory_gripper	target_3dr   
cam_K_norm
episode_idepisode_namerE   rB   joint_stateepisode_start_joint_state)*r<   pltimreadshaper9   r=   indexranger6   r;   r7   r   loadkp_localr   floatr   copyr   r8   r&   arrayconcatenatetiler4   cv2resizeINTER_LINEARclipzerosfloat32r>   roundtorch
from_numpypermuter   tensorview	Exceptionasarraystr)=r@   idxrB   rE   	frame_strrgb_pathrO   H_origW_origrC   frame_indicesstart_frame_idxrQ   rR   rS   inext_frame_idxnext_frame_strgripper_pose_pathgripper_posegripper_rotgripper_poskp_3djoint_state_pathrX   r   cam_K_norm_pathcamera_pose_pathrU   r   r   kp_2dlast_point_2dlast_point_3dlast_grippern_padrT   scale_xscale_yheatmap_targetst	target_2drP   target_xtarget_y
rgb_tensormeanstdheatmap_tensortrajectory_2d_tensortrajectory_3d_tensortrajectory_gripper_tensortarget_3d_tensorcamera_pose_tensorcam_K_norm_tensorjoint_state_path_curjoint_state_curjoint_state_path_ep0joint_state_ep0joint_state_cur_tensorjoint_state_ep0_tensorr   r   r   __getitem__   s<  














<




z!RealTrajectoryDataset.__getitem__)r    r!   NN)	__name__
__module____qualname____doc__rt   r>   rF   rG   r   r   r   r   r   r   ]   s
    -r   __main__z Testing RealTrajectoryDataset...r!   z&scratch/parsed_moredata_pickplace_home)r4   r3   u   ✓ Loaded z samplesz

Sample 0:z  RGB shape: rO   z  RGB range: [z.3fz, ]z  Heatmap shape: rP   z  Heatmap sum per timestep: )rJ   r   )dimz  Trajectory 2D shape: rQ   z  Trajectory 2D (first 3): z  Trajectory 3D shape: rR   z  Target 3D: rT   z  Episode ID: rV   u   ✗ Error: 
z<============================================================u   ✓ Dataset test complete!)$r   rm   numpyr   torch.utils.datar   pathlibr   matplotlib.pyplotpyplotrZ   rf   mathr5   r   r
   r   rc   KEYPOINTS_LOCAL_M_ALLKP_INDEXr`   r   r   r   r?   datasetr;   sampler\   r	   r   sumrr   e	traceback	print_excr   r   r   r   <module>   s`    .  
/*