
    ?iZ                        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dlmZ dZdZdZd	Z ed
      ZdZdZdZd Z ej0                  g dg dg dg dg dg      dz  ZdZee   Zd Z G d de      Zd Zd Z G d de      Z e!dk(  r$ e"d       	  edd       Z# e"d! e$e#       d"       e#d   Z% e"d#        e"d$e%d%   jL                           e"d&e%d%   jO                         d'd(e%d%   jQ                         d'd)        e"d*e%d+   jL                           e"d,e%d+   jS                  d-.               e"d/e%d0   jL                           e"d1e%d0   dd          e"d2e%d3   jL                           e"d4e%d5            e"d6e%d7            e"d9d:         e"d;       yy# e*$ r*Z+ e"d8e+        ddl,Z, e,jZ                          Y dZ+[+?dZ+[+ww xY w)<z-Dataset for real dense trajectory prediction.    N)Dataset)Path)Image          @   z/data/RTX/tracks  皙ɿ皙?c                 ^    | }|dkD  rd}|dkD  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xs     5/data/cameron/keygrip/point_track_pretraining/data.pyprocess_gripper_valuer      s7     	A3w3w {CQ/00    )     *@{GVg/@)g
ףp=%@gfffffXr   )r   r   g/)g(\1@g=
ףpTr   )g\(6@g=
ףpQr   g     @@   c                 t    t        j                  | d      }||z  }|d   dk  ry||dd z  }|dd |d   z  }|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_2ds          r   project_3d_to_2dr&   L   sZ     8S)Jj(I |q 2A&J"1~
1-HOr   c                   :    e Zd ZdZddedz  dedz  fdZd Zd Zy)	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.
    Nepisodemax_episodesc                    t        |      | _        || _        t        | _        | j                  j                         st        d| j                         t        | j                  j                         D cg c]"  }|j                         rd|j                  v r|$ c}      }|H|D cg c]  }|j                  |k(  s| }}t        |      dk(  rt        d| d| j                         ||d| }t        |      dk(  rt        d| j                         g | _        |D ]~  }t        |j                  d      D cg c]  }|j                  j                         s|! c}      }	|	D ]4  }
t!        |
j                        }| j                  j#                  ||f       6  t%        d	t        |       d
       t%        dt        | j                         d       yc c}w c c}w c c}w )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: r)   Nr   z	Episode 'z' not found in zNo episodes found in *.pngzLoaded z	 episodeszCreated z samples (one per frame))r   dataset_root
image_sizeN_WINDOWn_windowexists
ValueErrorsortediterdiris_dirnamelensamplesglobstemisdigitintr   print)selfr-   r.   r)   r*   depisode_dirsepisode_dirfframe_files
frame_file	frame_idxs               r   __init__zRealTrajectoryDataset.__init__r   s    !.$    '')<T=N=N<OPQQ$*;*;*C*C*E DQ !
yAFF/B  ! D E '3I!qvv7HAILI< A% 9WI_TEVEVDW!XYY#'6L|!4T5F5F4GHII ' 	>K [-=-=g-F![!&&..JZ!![\K * >

0	##[)$<=>	> 	L)*)45T\\*++CDE5D J "\s   6'G *G%?G%?G*
G*
c                 ,    t        | j                        S N)r7   r8   r>   s    r   __len__zRealTrajectoryDataset.__len__   s    4<<  r   c                    | j                   |   \  }}|d}|| dz  }t        j                  |      dddf   }|j                  dd \  }}t	        |j                  d      D 	cg c]  }	|	j                  j                         s|	! c}	      }
|
D 	cg c]  }	t        |	j                         }}	|j                  |      }g }g }g }t        | j                        D ]  }||z   t        |      k\  r n|||z      }|d}|| dz  }|j                         s nZt        j                  |      }|ddddf   }|dddf   }|t         z  |z   }|j#                  |       || d	z  }|j                         r@t        j                  |      }t%        |d
         }t'        |      }|j#                  |       n|j#                  d       || dz  }|| dz  }|j                         r|j                         s nyt        j                  |      }t        j                  |      }|j)                         }|dxx   |z  cc<   |dxx   |z  cc<   t+        |||      } |  n|j#                  |         t        |      dk(  rt        j                  || dz        }|ddddf   }|dddf   }|t         z  |z   }|g}|| d	z  }|j                         r2t        j                  |      }t%        |d
         }t'        |      }|g}ndg}t        j                  || dz        }t        j                  || dz        }|j)                         }|dxx   |z  cc<   |dxx   |z  cc<   t+        |||      } | | g}t        |      dk(  rt-        d|j.                   d|       t        j0                  |      }t        j0                  |      }t        j0                  |      }t        |      | j                  k  r|d
d }!|d
d }"|d
d }#| j                  t        |      z
  }$t        j2                  |t        j4                  |!|$df      gd      }t        j2                  |t        j4                  |"|$df      gd      }t        j2                  |t        j4                  |#|$f      gd      }nEt        |      | j                  kD  r-|d| j                   }|d| j                   }|d| j                   }|d
   }%t        j                  || dz        }t        j                  || dz        }|| j6                  k7  s|| j6                  k7  rtt9        j:                  || j6                  | j6                  ft8        j<                        }| j6                  |z  }&| j6                  |z  }'|t        j0                  |&|'g      z  }g }(t        | j                        D ]  })||)   }*t        j0                  t        j>                  |*d   d| j6                  dz
        t        j>                  |*d   d| j6                  dz
        g      }*t        j@                  | j6                  | j6                  ft        jB                        }+t        tE        |*d               },t        tE        |*d               }-d|,cxk  r| j6                  k  r"n nd|-cxk  r| j6                  k  r
n nd|+|-|,f<   |(j#                  |+        t        j0                  |(      }(tG        jH                  |      jK                  ddd      j%                         }.|.jM                         dkD  r|.dz  }.tG        jN                  g d      jQ                  ddd      }/tG        jN                  g d      jQ                  ddd      }0|.|/z
  |0z  }.tG        jH                  |(      j%                         }1tG        jH                  |      j%                         }2tG        jH                  |      j%                         }3tG        jH                  |      j%                         }4tG        jH                  |%      j%                         }5tG        jH                  |      j%                         }6tG        jH                  |      j%                         }7|| d	z  }8d}9|8j                         r	 t        j                  |8      }9|dz  }:d};|:j                         r	 t        j                  |:      };|98tG        jH                  t        jT                  |9t        jB                              nd}<|;8tG        jH                  t        jT                  |;t        jB                              nd}=|.|1|2|3|4|5|6|7|j.                   d| |j.                  |tW        |      |<|=dS c c}	w c c}	w # tR        $ r d}9Y w xY w# tR        $ r d};Y w xY w)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   r,   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.npy_frame_)rgbheatmap_targettrajectory_2dtrajectory_3dtrajectory_gripper	target_3dr    
cam_K_norm
episode_idepisode_namerE   rA   joint_stateepisode_start_joint_state),r8   pltimreadshaper3   r9   r:   r;   r<   indexranger0   r7   r1   r   loadkp_localr   floatr   copyr&   r2   r6   arrayconcatenatetiler.   cv2resizeINTER_LINEARclipzerosfloat32roundtorch
from_numpypermuter   tensorview	Exceptionasarraystr)>r>   idxrA   rE   	frame_strrgb_pathrX   H_origW_origrB   rC   frame_indicesstart_frame_idxrZ   r[   r\   inext_frame_idxnext_frame_strgripper_pose_pathgripper_posegripper_rotgripper_poskp_3djoint_state_pathra   r   cam_K_norm_pathcamera_pose_pathr^   r    r!   kp_2dlast_point_2dlast_point_3dlast_grippern_padr]   scale_xscale_yheatmap_targetst	target_2drY   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_tensors>                                                                 r   __getitem__z!RealTrajectoryDataset.__getitem__   s	    "&c!2Y o	 I;d!33jj"37+2A )9)9')BWAaffnnFVaWX.9:QVV::'--i8  t}}% 3	(A"c-&88*?Q+>?N .s3N !,0@@Q.R R$++-77#45L&rr2A2v.K&rr1u-K  (*[8E  '  +/?t-DD&&( gg&67 %k"o 6 5m D"))-8 #))#. *yk,DDO*	{:J-KK#**,1A1H1H1J1J''"23K OO%E!HH!HH %UK?E}  'g3	(j }"77;I;>O1P#PQL&rr2A2v.K&rr1u-K(*[8E"GM  +	{$-??&&( gg&67 %k"o 6 5m D&3_"&)U")J/G!GHJ''+9+=M0N"NOKOO%E!HH!HH$UK?E !&}"@AQAQ@RRYZcYdeff//XX&89 }-)"#.M)"#.M-bc2LMMC$66ENNM277=SXZ[R\;]+^efgMNNM277=SXZ[R\;]+^efgM!#1CRWW\\a[cEd0elm!n$--/).4==9M).4==9M!3NT]]!C ""%	 ggkyk9I,JJKWW[i[
+CCD
 T__$$//(A**S4??DOO"D),)9)9;C oo.Goo.G)BHHgw5G,HHM t}}% 	3A%a(I 	!a1)<=	!a1)<=" I  XXt&HPRPZPZ[N51./H51./HH.t.13R4??3R58x12"">2!	3$ ((?3 %%c*221a;AAC
>>c!#e+J ||12771a@ll0166q!Q? 4'3.
))/:@@B$//>DDF$//>DDF$)$4$45G$H$N$N$P! ++I6<<>"--k:@@B!,,Z8>>@  +	{$-??&&('"$''*>"?  +\9&&('"$''*>"? * RZZrzzJK 	 * RZZrzzJK 	 ,11";)-+(--.gi[A',,"{+1)?
 	
O X:`  '"&'  '"&'s6   %f3f3f8f= 3g =g
ggg)zscratch/r
   NN)	__name__
__module____qualname____doc__r}   r<   rF   rJ   r    r   r   r(   r(   e   s4    
+Ft +Fjmptjt +FZ!m
r   r(   c                     t        j                  |       5 }|j                  t        |             t	        j
                  |j                  d            cddd       S # 1 sw Y   yxY w)z2Load a single frame from a GIF as (H, W, 3) uint8.RGBN)r   openseekr<   r   rl   convert)pathrE   ims      r   load_gif_framer     sJ    	D	 +R
Ixx

5)*+ + +s   >AA'c           	      .   g }t        j                  |       5 }|D ]O  }|j                  t        |             |j	                  t        j                  |j                  d                   Q 	 ddd       t        j                  |      S # 1 sw Y   xY w)zHLoad GIF frames at given indices. Returns (len(indices), H, W, 3) uint8.r   N)	r   r   r   r<   r   r   rl   r   stack)r   indicesoutr   r   s        r   load_gif_frames_at_indicesr     sv    
C	D	 4R 	4AGGCFOJJrxx

5 123	44 88C=	4 4s   ABBc                   0    e Zd ZdZdeeeedfdZd Z	d Z
y)RTXPointTrackDatasetztLoad .pt track files from /data/RTX/tracks; first frame as input, predict next N_WINDOW heatmaps for N_QUERY_POINTS.Nc                    t        |xs t              | _        || _        || _        || _        || _        t        | j                  j                  d            | _	        | j                  st        d| j                         || j                  d | | _	        t        dt        | j                         d| j                          y )Nz*.ptzNo .pt files in zRTXPointTrackDataset: z .pt files from )r   TRACKS_ROOTtracks_rootr.   r0   n_queryheatmap_sizer3   r9   pt_pathsFileNotFoundErrorr=   r7   )r>   r   r.   r0   r   r   max_sampless          r   rF   zRTXPointTrackDataset.__init__  s     :{;$ (t//44V<=}}#&6t7G7G6H$IJJ" MM,;7DM&s4=='9&::J4K[K[J\]^r   c                 ,    t        | j                        S rH   )r7   r   rI   s    r   rJ   zRTXPointTrackDataset.__len__  s    4==!!r   c           
         | j                   |   }t        j                  |d      }|d   j                         }|d   j                         j	                         }|j
                  dk(  r9t        j                  |d d d f   |j                  d   |j                  d   f      }|d   j                         }t        |d         }t        |d	         }t        |d
         }	|j                  \  }
}}|
| j                  dz   k  rt        j                  |dd  | j                  dz   |
z
  ddf      }t        j                  ||gd      }t        j                  |dd  | j                  dz   |
z
  |f      }t        j                  ||gd      }|j                  d   }
t        j                  j                  |t!        | j"                  |      || j"                  k        }t%        |      | j"                  k  rLt        j                  |t        j                  j                  || j"                  t%        |      z
        g      }|d | j"                   }t'        ||d         }t)        j*                  || j,                  | j,                  ft(        j.                        }t        j0                  |      j3                  ddd      j5                         }|j7                         dkD  r|dz  }t        j8                  g d      j;                  ddd      }t        j8                  g d      j;                  ddd      }||z
  |z  }| j,                  |	z  }| j,                  |z  }|d|f   t        j<                  ||g      z  }t        j0                  |      j5                         }| j>                  |	z  }| j>                  |z  }g }g }tA        | j"                        D ]  }g }g }tA        d| j                  dz         D ]  }||||   df   ||||   df   }!} t        jB                  t        tE        | |z              d| j>                  dz
        }"t        jB                  t        tE        |!|z              d| j>                  dz
        }#|jG                  |#| j>                  z  |"z          |jG                  ||||   f   dkD          |jG                  |       |jG                  |        t        j0                  t        j<                  |            jI                         }$t        j0                  t        j<                  |            jK                         }%|d | j                  dz    }&tM        ||&      }'|d | j                  dz   |f   }(|||$|%t        j0                  |'      t        j0                  |(      j5                         |jN                  dS )NF)weights_onlytracks
visibilityrN   r   r   gif_pathheightwidthrM   rO   )sizereplace)r   rQ   r   r   rT   rU   r   rV   g      ?)rX   query_start_2dtarget_heatmap_indicesr   
frames_vis
tracks_vispt_name)(r   rv   rh   numpysqueezendimr   broadcast_tore   r   r<   r0   rn   rm   randomchoicer   r   r7   r   ro   rp   r.   rq   rw   rx   rj   r   ry   rz   rl   r   rg   rr   ru   r   longboolr   r:   ))r>   r~   pt_pathpayloadr   r   r   r   r   r   TN_lastvis_last	query_idxfirst_framer   r   r   scale_448_xscale_448_yquery_startr   
scale_64_x
scale_64_ytarget_indicesvis_maskqtivmr   r   yx64y64r   visibility_maskframe_inds_visr   r   s)                                            r   r   z RTXPointTrackDataset.__getitem__  s&   --$**W59"((*\*002::<
??aAtG)<v||APVP\P\]^P_>`aJ0668
+,WX&'WW%&,,1at}}q  776"#;):Q)>1(EFD^^VTN;Fz"#9JQ9NPQ8RSHX(>QGJQAII$$QSq-AAPTP\P\L\$^	y>DLL(	2993C3CADLL[^_h[iLi3C3j'klIn-	$X}Q/?@jjt.P`c`p`pq%%k2::1aCIIK
>>c!#e+J||12771a@ll0166q!Q? 4'3.
oo.oo.Q	\*RXX{K6P-QQ))+6<<>&&/
&&/
t||$ 
	 ABB1dmma/0 =a1q016!Yq\1:L3M1ggc%J"78!T=N=NQR=RSggc%J"78!T=N=NQR=RS		# 1 11C78		*Q	!_5;<= !!"%OOB
	  "'!1!1"((>2J!K!P!P!R**288H+=>CCE&':):;/.I
/dmma//:;
,&<)**:6**:6<<>||
 	
r   )r   r   r   r   IMAGE_SIZE_PTN_WINDOW_POINT_TRACKN_QUERY_POINTSHEATMAP_SIZErF   rJ   r   r   r   r   r   r     s/    ~#'ML`jx  HT  bf _"D
r   r   __main__z Testing RealTrajectoryDataset...z&scratch/parsed_moredata_pickplace_home)r.   r-   u   ✓ Loaded z samplesz

Sample 0:z  RGB shape: rX   z  RGB range: [z.3fz, ]z  Heatmap shape: rY   z  Heatmap sum per timestep: )rN   r   )dimz  Trajectory 2D shape: rZ   z  Trajectory 2D (first 3): z  Trajectory 3D shape: r[   z  Target 3D: r]   z  Episode ID: r_   u   ✗ Error: 
z<============================================================u   ✓ Dataset test complete!).r   rv   r   r   torch.utils.datar   pathlibr   matplotlib.pyplotpyplotrc   ro   mathPILr   r/   r   r   r   r   r   r   r   r   rl   KEYPOINTS_LOCAL_M_ALLKP_INDEXri   r&   r(   r   r   r   r   r=   datasetr7   samplere   r   r   sumr{   e	traceback	print_excr   r   r   <module>r     s|   3   $   
     %& 1\ !"79JLb#57I"K LNTU  *2j
G j
Z	+W
7 W
t z	
,-'3EmnCL>23 fUm11234ve}00237r&-:K:K:Mc9RRSTU!&)9":"@"@!ABC,V4D-E-I-Ie-I-T,UVW'(?(E(E'FGH+F?,CBQ,G+HIJ'(?(E(E'FGHf[1234vl3456 
Bvh-	
&'7 *  A3 	s   DF9 9G(> G##G(