o
    ]Wief                  
   @   s  d 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l	m
Z ddlZddlZddlmZ dZdZdZd	Zed
Zd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dCddZ dd 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.d0 e%d1e(d2 j)  e%d3e(d2 j,d4d5  e%d6e(d7 j)  e%d8e(d7 dd   e%d9e(d: j)  e%d;e(d<   e%d=e(d>   W n" e-y1 Z. ze%d?e.  ddl/Z/e/0  W Y dZ.[.ndZ.[.ww e%d@dA  e%dB dS dS )Dz-Dataset for real dense trajectory prediction.    N)Dataset)Path)Image          @   z/data/RTX/tracks  U   皙ɿ皙?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   5/data/cameron/keygrip/point_track_pretraining/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)Zpoint_3dcamera_posecam_KZ
point_3d_hZ	point_camZ
point_2d_hZpoint_2dr   r   r   project_3d_to_2dO   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/r	   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#   Zepisode_dirsepisode_dirframe_filesZ
frame_file	frame_idxr   r$   r   __init__u   s.   
	

zRealTrajectoryDataset.__init__c                 C   
   t | jS N)r;   r<   r@   r   r   r   __len__      
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)
        Z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dtype     o@g
ףp=
?gv/?gCl?gZd;O?gy&1?g?z
000000.npyZ_frame_)rgbheatmap_targettrajectory_2dtrajectory_3dtrajectory_gripper	target_3dr   
cam_K_norm
episode_idZepisode_namerC   rA   joint_stateZepisode_start_joint_state)*r<   pltZ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@   idxrA   rC   Z	frame_strZrgb_pathrU   H_origW_origrB   frame_indicesZstart_frame_idxrW   rX   rY   iZnext_frame_idxZnext_frame_strZgripper_pose_pathZgripper_poseZgripper_rotZgripper_posZkp_3dZjoint_state_pathr]   r   Zcam_K_norm_pathZcamera_pose_pathr[   r   r   Zkp_2dZlast_point_2dZlast_point_3dZlast_gripperZn_padrZ   Zscale_xZscale_yZheatmap_targetstZ	target_2drV   Ztarget_xZtarget_y
rgb_tensormeanstdZheatmap_tensorZtrajectory_2d_tensorZtrajectory_3d_tensorZtrajectory_gripper_tensorZtarget_3d_tensorZcamera_pose_tensorZcam_K_norm_tensorZjoint_state_path_curZjoint_state_curZjoint_state_path_ep0Zjoint_state_ep0Zjoint_state_cur_tensorZjoint_state_ep0_tensorr   r   r   __getitem__   s<  














<




z!RealTrajectoryDataset.__getitem__)r!   r	   NN)	__name__
__module____qualname____doc__rw   r>   rD   rH   r   r   r   r   r   r    h   s
    -r    c                 C   s   | j \}}}tj|tjd}t|D ]3}t|d D ]*}|||f rD||d |f rDtj| |d |f | ||f  }	||  |	7  < qq|dksQt|dkrVt|S t	||}
||
k}t
|d }t|dkro|S t|S )zReturn indices of tracks in the top (100 - percentile)% by total distance traveled.
    So percentile=85 means top 15% (most motion). tracks (T, N, 2), visibility (T, N).rP   rK   r   )r_   r   rm   float64ra   linalgnormallarange
percentilewherer;   )tracks
visibilityr   TN_Z
total_distnr}   r)   Zthreshmaskindicesr   r   r   _top_motion_track_indices  s   $
r   c                 C   sL   t | }|t| t|dW  d   S 1 sw   Y  dS )z2Load a single frame from a GIF as (H, W, 3) uint8.RGBN)r   openseekr>   r   rf   convert)pathrC   imr   r   r   load_gif_frame  s   $r   c              	   C   sf   g }t | }|D ]}|t| |t|d q
W d   n1 s)w   Y  t|S )zHLoad GIF frames at given indices. Returns (len(indices), H, W, 3) uint8.r   N)	r   r   r   r>   r   r   rf   r   stack)r   r   outr   r|   r   r   r   load_gif_frames_at_indices  s   
r   c                   @   s:   e Zd ZdZdeeeededfddZ	dd Z
dd	 ZdS )
RTXPointTrackDatasetzLoad .pt track files from /data/RTX/tracks; first frame as input, predict next N_WINDOW heatmaps for N_QUERY_POINTS.
    Optionally filter to top motion tracks only (motion_percentile=85 -> top 15%% by total distance).N*   c	           
      C   s   t |pt| _|| _|| _|| _|| _|| _t| j	d| _
| j
s*td| j |d ur9t|}	|	| j
 |d urE| j
d | | _
tdt| j
 d| j |d ur]dd|  dnd |d ured	nd  d S )
Nz*.ptzNo .pt files in zRTXPointTrackDataset: z .pt files from z (motion filter: top d   z%%) z (paths shuffled))r   TRACKS_ROOTtracks_rootr4   r6   n_queryheatmap_sizemotion_percentiler9   r=   pt_pathsFileNotFoundErrorrandomRandomshuffler?   r;   )
r@   r   r4   r6   r   r   max_samplesr   Zshuffle_seedrngr   r   r   rD     s   
LzRTXPointTrackDataset.__init__c                 C   rE   rF   )r;   r   rG   r   r   r   rH     rI   zRTXPointTrackDataset.__len__c           +      C   s  | j | }tj|dd}|d  }|jd |jd }}d|v r*|d   }n
tj||ftjd}|j	dkrnt
||krLt|dd||f}nht
||kr_t|dd||f}nUt|d d d f ||f}nF|j	d	kr|j||fkr{n9|j||fkr|j}n.|j|dfkrt|||f }n|jd|fkrt|||f }n
tj||ftjd}|d
  }t|d }	t|d }
t|d }|j\}}}|| jd k rt|dd  | jd | ddf}tj||gdd}t|dd  | jd | |f}tj||gdd}|jd }| jd ur"t||| j}nt|}t
|}|dkr7t|}|}tjj|t| j||| jk d}t
|| jk rat|tjj|| jt
| dg}|d | j }t|	|d }tj|| j| jftjd}t |!d	dd" }|# dkr|d }t$g d%ddd}t$g d%ddd}|| | }| j| }| j|
 }|d|f t&||g }t |" }| j'| }| j'|
 }g }g }t(| jD ]g}g }g } t(d| jd D ]M}!||!|| df ||!|| df }"}#t)tt*|"| d| j'd }$t)tt*|#| d| j'd }%|+|%| j' |$  | +||!|| f dk q|+| |+|  qt t&|, }&t t&|- }'|d | jd  }(t.|	|(})|d | jd |f }*|||&|'t |)t |*" |j/dS )NF)weights_onlyr   r   rK   r   rP   rJ   r   r{   gif_pathheightwidthrL   )sizereplace)r   rN   r   rR   rS   r   rT   g      ?)rU   query_start_2dtarget_heatmap_indicesr   
frames_vis
tracks_vispt_name)0r   rp   rb   numpyr_   squeezer   onesrn   ndimr;   broadcast_toreshaper   re   r   r>   r6   rh   rg   r   r   r   r   choicer   r   r   ri   rj   r4   rk   rq   rr   rd   r   rs   rt   rf   r   ra   rl   ro   r   longboolr   r.   )+r@   rx   Zpt_pathpayloadr   r   r   r   r{   r   ry   rz   r   lastZvis_lastZcandidate_indicesZn_candidatesZ	query_idxZfirst_framer~   r   r   Zscale_448_xZscale_448_yZquery_startr   Z
scale_64_xZ
scale_64_ytarget_indicesZvis_maskqtivmr}   r   yx64Zy64r   Zvisibility_maskZframe_inds_visr   r   r   r   r   r     s   


$"



 $



&  

z RTXPointTrackDataset.__getitem__)r   r   r   r   IMAGE_SIZE_PTN_WINDOW_POINT_TRACKN_QUERY_POINTSHEATMAP_SIZEMOTION_PERCENTILErD   rH   r   r   r   r   r   r     s
    r   __main__z Testing RealTrajectoryDataset...z&scratch/parsed_moredata_pickplace_home)r4   r3   u   ✓ Loaded z samplesz

Sample 0:z  RGB shape: rU   z  RGB range: [z.3fz, ]z  Heatmap shape: rV   z  Heatmap sum per timestep: )rK   r   )dimz  Trajectory 2D shape: rW   z  Trajectory 2D (first 3): z  Trajectory 3D shape: rX   z  Target 3D: rZ   z  Episode ID: r\   u   ✗ Error: 
z<============================================================u   ✓ Dataset test complete!)r
   )1r   r   rp   r   r   torch.utils.datar   pathlibr   matplotlib.pyplotpyplotr^   ri   mathPILr   r5   r   r   r   r   r   r   r   r   r   rf   ZKEYPOINTS_LOCAL_M_ALLZKP_INDEXrc   r   r    r   r   r   r   r   r?   datasetr;   sampler_   r   r   sumru   e	traceback	print_excr   r   r   r   <module>   sx    .  
/

~*