
    {i                          S r SSKrSSKJr  SSKJr  SSKrSSK	r	SSK
r
SSKrSSKJr  SSKJr  SSKJr  SSKrS rSS jrS	 rS
 rS rS rS rS rSS jrS rSS jr  SS jr SS jr g)zMGeneral utilities for token selection model: geometry, visualization, and IK.    N)GridSpec)Path)	make_grid)Rotationc                    [         R                  " U SSS9n[        U[        R                  5      (       a  [         R
                  " U5      nUR                  5       nS=p#UR                  5       S:X  a$  UR                  S   S:X  a  UR                  S5      nUR                  5       S:X  aU  UR                  S   UR                  S	   :  a  UR                  SS
S5      nUR                  u  p#nUR                  S	U5      nOyUR                  5       S
:X  aF  UR                  S   UR                  S   :  a%  UR                  S   S::  a  UR                  SS5      nOUR                  UR                  S   S	5      nXU4$ )a  Load and process DINO features from file.

Args:
    path: Path to DINO features file (.pt)

Returns:
    feats: (num_patches, dino_feat_dim) tensor of features
    H_patches: Height in patches (or None if not determinable)
    W_patches: Width in patches (or None if not determinable)
cpuF)map_locationweights_onlyN   r               )torchload
isinstancenpndarray
from_numpyfloatdimshapesqueezepermutereshape	transpose)pathfeats	H_patches	W_patchesDs        S/Users/cameronsmith/Projects/robotics_testing/3dkeygrip/volume_dino_tracks/utils.pyload_dino_featuresr$      s4    JJt%eDE%$$  'KKME  Iyy{aEKKNa/a yy{a;;q>EKKO+MM!Q*E"'++	ab!$		;;q>EKKN*u{{1~/DOOAq)Eekk!nb1Y&&    c                    Ub  Uc:  [        [        R                  " U 5      5      nX-  S:w  a  US-  nX-  S:w  a  M  X-  n[        R                  " [        R                  " U5      [        R                  " U5      SS9u  p4[        R
                  " UR                  5       UR                  5       /SS9R                  [        R                  5      nXQU4$ )a<  Build patch position coordinates.

Args:
    num_patches: Total number of patches
    H_patches: Height in patches (if known)
    W_patches: Width in patches (if known)

Returns:
    patch_positions: (num_patches, 2) array of [x, y] patch coordinates
    H_patches: Height in patches
    W_patches: Width in patches
r   r   ij)indexingaxis)	intr   sqrtmeshgridarangestackflattenastypefloat32)num_patchesr    r!   y_coordsx_coordspatch_positionss         r#   build_patch_positionsr7   .   s     I-,-	%*NI %*,	RYYy%9299Y;OZ^_Hhh 0 0 2H4D4D4FGaPWWXZXbXbcOy00r%   c                    [        UR                  5      S nU SU S3-  nU SU S3-  nUR                  5       (       aA  UR                  5       (       a,  [        R                  " U5      [        R                  " U5      4$ U S-  nU S-  nUR                  5       (       aA  UR                  5       (       a,  [        R                  " U5      [        R                  " U5      4$ g)a  Load camera pose and intrinsics for a frame.

Args:
    episode_dir: Path to episode directory
    frame_file: Path to frame file

Returns:
    camera_pose: 4x4 camera pose matrix (or None if not found)
    cam_K: 3x3 camera intrinsics (or None if not found)
06drobot_camera_pose_z.npycam_K_zrobot_camera_pose.npyz	cam_K.npyNN)r+   stemexistsr   r   )episode_dir
frame_file	frame_strcam_pose_path
cam_K_pathcam_pose_staticcam_K_statics          r#   load_cam_datarF   D   s     z',I$6yk"FFM	{$77J*"3"3"5"5ww}%rwwz':::!$;;O,LL$7$7$9$9ww')>>>r%   c                 p    [         R                  " U S5      nX-  SS nUS   S::  a  gX$-  nUSS US   -  $ )z)Project 3D point to 2D image coordinates.      ?Nr   r   r   )r   append)point_3dcamera_posecam_K
point_3d_h	point_cam
point_2d_hs         r#   project_3d_to_2drP   \   sM    8S)J)2A.I|q"Jbq>JqM))r%   c                 4   U c  g[         R                  " U [         R                  S9n U R                  S:X  a  U $ U R                  S:X  a  U R                  SS5      n SnOSnXB-  nX1-  n[         R                  " U S   U-  U S	   U-  /SS
9nU(       a  US   $ U$ )a1  Rescale 2D coordinates from original image size to new size.

Args:
    coords: Can be None, empty, 1D array (2,), or 2D array (N, 2)
    H_orig, W_orig: Original image dimensions
    H_new, W_new: New image dimensions

Returns:
    Rescaled coordinates in same shape as input (or None if input was None)
Ndtyper   r   r   TF).r   ).r   r)   )r   asarrayr2   sizendimr   r/   )	coordsH_origW_origH_newW_newwas_1dscale_xscale_ycoords_rescaleds	            r#   rescale_coordsr`   e   s     ~ZZbjj1F{{a{{a2&nGnGhh	'	!6&>G#;<2O q!!r%   c                 >   [         R                  R                  U5      nUSS2S4   nUSS2SS24   nUS   US   pvUS   US   pU S   U-
  U-  n
U S   U	-
  U-  nS	n[         R                  " XU/5      nU[         R                  R	                  U5      -  nX]-  nXN4$ )
a4  Unproject 2D point to a ray in robot frame.

Args:
    point_2d: 2D point in image coordinates
    camera_pose: 4x4 transformation matrix from robot frame to camera frame
    cam_K: 3x3 camera intrinsics

Returns:
    cam_pos_robot: Camera position in robot frame
    ray_robot: Ray direction in robot frame
Nr   r   r   )r   r   r   r   )r   r   r   r   rH   )r   linalginvarraynorm)point_2drK   rL   cam_pose_invcam_pos_robotcam_rot_c2rfxfycxcyx_camy_camz_camray_cam	ray_robots                  r#   unproject_2d_to_rayru      s     99==-L !Q'Mrr2A2v&K4[%+4[%+a[2#Ea[2#EEhhe,-G		w//G%I##r%   c                     U b  Uc  g[        XU5      u  pE[        US   5      S:  a  gUS   * US   -  nXFU-  -   nUR                  5       nXS'   U$ )aj  Recover 3D keypoint from 2D image projection of ground projection and height.

The 2D point is the projection of the keypoint's ground projection (Y=0), not the keypoint itself.
So we need to:
1. Find where the ray from the camera through the 2D point intersects the ground plane (Y=0, index 2)
2. Then move up by the height to get the actual 3D keypoint

Args:
    kp_2d_image: 2D image coordinates of the ground projection of the keypoint
    height: Height (Z coordinate, index 2) of the actual 3D keypoint
    camera_pose: 4x4 camera pose matrix
    cam_K: 3x3 camera intrinsics

Returns:
    3D keypoint position
Nr   ư>)ru   abscopy)	kp_2d_imageheightrK   rL   cam_pos	ray_imaget_groundground_pointkp_3ds	            r#   #recover_3d_from_keypoint_and_heightr      su    " fn,[uMG
 9Q<4 
{Yq\)H	11L E!HLr%   c                     U b  Uc  g[        XU5      u  pE[        US   5      S:  a  gXS   -
  US   -  nUS:  a  gXFU-  -   nU$ )a  Recover 3D keypoint from direct 2D keypoint projection and height.

The 2D point is the direct projection of the 3D keypoint (not its ground projection).
We find the point along the ray from the camera through the 2D point at the specified height.

Args:
    kp_2d_image: 2D image coordinates of the direct keypoint projection
    height: Height (Y coordinate, index 2) of the 3D keypoint in MuJoCo Z-up convention
    camera_pose: 4x4 camera pose matrix
    cam_K: 3x3 camera intrinsics

Returns:
    3D keypoint position, or None if invalid
Nr   rw   r   )ru   rx   )rz   r{   rK   rL   r|   ray_directiontr   s           r#   *recover_3d_from_direct_keypoint_and_heightr      sm     fn05QG =t# 
!*	a 00A1u -''ELr%   c                    / n/ n/ n	[        SUS-   5       H  n
X*-   nU[        U5      :  a    O[        X   R                  5      S nX S3-  nUR	                  5       (       d  MO  [
        R                  " U5      nUSS2SS24   nUSS2S4   nX-  U-   nUR                  U5        U(       a  UR                  US   5        U(       d  M  U	R                  U5        M     [        U5      S:  a  [
        R                  " U5      O%[
        R                  " / 5      R                  SS5      nU/nU(       aL  [        U5      S:  a  [
        R                  " U5      O[
        R                  " / 5      nUR                  U5        U(       a]  [        U	5      S:  a  [
        R                  " U	5      O&[
        R                  " / 5      R                  SSS5      n	UR                  U	5        [        U5      S:X  a  US   $ [        U5      $ )a  
Load ground truth 3D trajectory from gripper poses.

Args:
    episode_dir: Path to episode directory
    frame_files: List of frame file paths
    start_idx: Starting frame index
    window_size: Number of future frames to load
    kp_local: Local keypoint offset (from KEYPOINTS_LOCAL_M_ALL[KP_INDEX])
    return_heights: If True, also return heights array
    return_orientations: If True, also return orientations as rotation matrices

Returns:
    trajectory_gt_3d: (N, 3) array of 3D keypoints
    heights_gt: (N,) array of heights (only if return_heights=True)
    orientations_gt: (N, 3, 3) array of rotation matrices (only if return_orientations=True)
r   r9   z_gripper_pose.npyNr   r   r   )rangelenr+   r=   r>   r   r   rI   rf   r   tuple)r?   frame_files	start_idxwindow_sizekp_localreturn_heightsreturn_orientationstrajectory_gt_3d
heights_gtorientations_gtoffsetf_idxrA   	pose_pathposerotposr   rets                      r#   load_gt_trajectory_3dr      s   $ JO;?+"C$$;-223C8	K/@"AA	!!wwy!2A2rr6l2A2q5k$&eAh'""3'! ,$ 699I5JQ5Nrxx 01TVT\T\]_T`ThThijlmTn
C-0_q-@RXXj)bhhrl


:7:?7Ka7O"((?3UWU]U]^`UaUiUijkmnpqUr

?#
3x1}1v:r%   c                 R   [         R                  " U 5      n [         R                  " U5      nU R                  S:X  a  U R                  SS5      n UR                  S:X  a  UR                  S5      nU R                  S   nU R                  S   n	U R                  SS9n
/ nU
 H  nX-  nX-  nUR                  X/5        M     [         R                  " U5      n[        XX4U5      nU[        [        -
  -  [        -   n/ n[        [        [        U5      [        U5      5      5       H0  nUU   nUU   n[        UUXg5      nUc  M  UR                  U5        M2     [        U5      S:  a  [         R                  " U5      O%[         R                  " / 5      R                  SS5      nUUU4$ )aX  
Post-process model predictions: convert pixel scores to 3D trajectory.

Args:
    pixel_scores: (window_size, num_patches) or (num_patches,) - attention/pixel scores
    heights_pred: (window_size,) or scalar - predicted heights (normalized)
    H_patches, W_patches: Patch grid dimensions
    H_orig, W_orig: Original image dimensions
    camera_pose: 4x4 camera pose matrix
    cam_K: 3x3 camera intrinsics

Returns:
    trajectory_pred_3d: (N, 3) array of 3D keypoints
    trajectory_pred_2d_image: (N, 2) array of 2D image coordinates
    heights_pred_denorm: (N,) array of denormalized heights
r   r   r   r)   r   )r   rT   rV   r   r   argmaxrI   rf   r`   
MAX_HEIGHT
MIN_HEIGHTr   minr   r   )pixel_scoresheights_predr    r!   rX   rY   rK   rL   r   r3   pred_patch_idxpred_patchesidxpypxpred_image_coordsheights_pred_denormtrajectory_pred_3dr   kp_2dh
kp_3d_preds                         r#   post_process_predictionsr   "  s   $ ::l+L::l+L A#++Ar2A#++A.$$Q'K$$Q'K "((a(0N L_RH%  88L)L '|	SYZ '*z*ABZO 3s,-s3F/GHI!!$"?q+]
!%%j1 J :==O9PST9T"45Z\ZbZbceZfZnZnoprsZt02EEEr%   c                 2   SSK JnJn	  U R                  5       n
U
S   U:  a  XzS'   [	        U5       GH  nU" X4U5      nU	" X4X\5        [
        R                  " XE5        UR                  UR                  5        [        R                  " SSSSS9n[
        R                  " U[
        R                  R                  S5      n[        R                  " UR                   U   / S	Q   5      R#                  5       n[        R$                  " U5      R'                  5       nUR)                  [        R*                  " [,        R.                  " US
   US   US   US   /U
/5      S95        [        R$                  " U5      R'                  5       n[
        R                  " U[
        R                  R                  S5      nUR0                  U   R                  5       n[        R                  " SSSSS9nUR)                  [        R*                  " [,        R.                  " US
   US   US   US   /U/5      S95        [        R2                  " USS9nUR)                  UR                  5        [        R4                  " X-UU/SS[        R6                  " US9/S9nUR9                  US5        UR:                  UR                  SS& UR:                  S[=        UR>                  5       UR>                  SS& [
        R@                  " XE5        [
        R                  " XE5        [
        R                  " U[
        R                  R                  S5      nUR0                  U   R                  5       nUS   U:  a  [C        U
S   U5      U
S'   US-  S:X  d  GMY  [,        RD                  RG                  UU
-
  5      nUS:  d  GM    O   [
        R                  " XE5        [
        R                  " U[
        R                  R                  S5      nUR0                  U   R                  5       nUS   U:  Ga  XzS'   [	        S5       GH  nU" X4U5      nU	" X4X\5        [
        R                  " XE5        UR                  UR                  5        [        R                  " SSSSS9n[        R                  " UR                   U   / S	Q   5      R#                  5       n[        R$                  " U5      R'                  5       nUR)                  [        R*                  " [,        R.                  " US
   US   US   US   /U
/5      S95        [        R$                  " U5      R'                  5       n[
        R                  " U[
        R                  R                  S5      nUR0                  U   R                  5       n[        R                  " SSSSS9nUR)                  [        R*                  " [,        R.                  " US
   US   US   US   /U/5      S95        [        R2                  " USS9nUR)                  UR                  5        [        R4                  " X-UU/SS[        R6                  " US9/S9nUR9                  US5        UR:                  UR                  SS& UR:                  S[=        UR>                  5       UR>                  SS& [
        R@                  " XE5        [
        R                  " XE5        UR0                  U   R                  5       nUS   U:  d  GM    g   gg)a  Solve IK with separate tasks for keypoint position and gripper rotation.

Args:
    target_kp_pos: (3,) target keypoint position
    target_gripper_rot: (3, 3) target gripper rotation matrix
    configuration: mink.Configuration object
    robot_config: Robot configuration
    mj_model: MuJoCo model
    mj_data: MuJoCo data
    max_iterations: Maximum number of IK iterations
    min_height_above_ground: Minimum height above ground plane (Y=0) in meters. Default 0.02m (2cm).
r   )get_link_poses_from_robotposition_exoskeleton_meshesr   virtual_gripper_keypointbodyrH           )position_costorientation_cost)r   r   r   r   r   r   )wxyz_xyz	Fixed_JawgQ?gMbP?)costg{Gz?daqp)model)limitsN
   g?)$	exo_utilsr   r   ry   r   mujoco
mj_forwardupdateqposmink	FrameTask
mj_name2idmjtObj
mjOBJ_BODYR	from_quatxquat	as_matrixfrom_matrixas_quat
set_targetSE3r   concatenatexposPostureTasksolve_ikConfigurationLimitintegrate_inplaceqr   ctrlmj_stepmaxrd   rg   )target_kp_postarget_gripper_rotconfigurationrobot_configmj_modelmj_datamax_iterationsmin_height_above_groundr   r   target_kp_pos_constrained	iteration
link_poseskp_task
kp_body_idkp_rotkp_quatgripper_quatgripper_body_idgripper_posgripper_taskposture_taskvelcurrent_kp_poserrorfinal_kp_pos_s                              r#   ik_to_keypoint_and_rotationr   `  sd    Q !. 2 2 4 #&=='>!$>*	.|wO
#LGP(,W\\* ..!;VSVilm&&x1I1IKef
W]]:6|DEOOQ--'//1488R^^gaj'RS*V]^_V`bijkbl=m  pI  =J  .K  L  	M }}%78@@B ++Hfmm6N6NP[\ll?388:~~k6_cd"..<PQ?T`abTceqrset  wC  DE  wF  CG  IT  BU  3V  !W  	X''t<-mmM\<+PRVX^hlhh  GO  iP  hQ  R''T2'//Q'//*<3w||+<=Qx) 	(,&&x1I1IKef
 j1668 !66+./H/KMd+e%a( r>QIINN>4M#MNEu}Y +^ h(""8V]]-E-EGabJ<<
+002LA00'>!$rA2<7SJ'Th0  .nn%?WZmpqG[[z!:<!HISSUFmmF+335Gtxx'!*gVWjZabcZdfmnofpAq  tM  AN  2O   P  Q==);<DDFL$//&--:R:RT_`O!,,7<<>K>>+vScefL##DHHbnn|TUXdefXgiuvwix  {G  HI  {J  GK  MX  FY  7Z  %[  \++H4@L##GLL1--|/TVZ\blp  mD  mD  KS  mT  lU  VC++C6+ooGLLO+oo.@s7<</@AGLLONN8-h0"<<
388:LA"99;  1r%   c                    U R                   S   nUc  [        R                  " SS9n[        SSUSSS9n0 nUR	                  US   5      US	'   UR	                  US
   5      US'   UR	                  US   5      US'   Sn[        U5       Hi  nUS-  nUS-  nUS:  a    OXUR	                  UUU4   5      USU 3'   US-  nUS-  nUS-  nUS:  a    O%UR	                  UUU4   5      USU 3'   US-  nMk     US	   nUR                  5         UR                  U 5        UGb  [        U5      S:  Ga  UR                  USS2S4   USS2S4   SSSSS9  [        U5       HI  u  nu  nn[        R                  R                  U[        U5      -  5      nUR                  UUSUSSSS9  MK     Ub  [        U5      S:  ay  UR                  USS2S4   USS2S4   SSSSS9  [        U5       HH  u  nu  nn[        R                  R                  U[        U5      -  5      nUR                  UUSUSSS9  MJ     Ub  UR                  US   US   S S!SSS"S#S$9  UR                  S%U SU S&U S'U 3S#S(S)9  UR                  S*5        UR!                  S+S!S,9  US   nUR                  5         UR                  U5        UGb  [        U5      S:  Ga  UR                  USS2S4   USS2S4   SSSSS9  [        U5       HI  u  nu  nn[        R                  R                  U[        U5      -  5      nUR                  UUSUSSSS9  MK     Ub  [        U5      S:  ay  UR                  USS2S4   USS2S4   SSSSS9  [        U5       HH  u  nu  nn[        R                  R                  U[        U5      -  5      nUR                  UUSUSSS9  MJ     Ub  UR                  US   US   S S!SSS"S#S$9  UR                  S-U	 SU
 S&U S'U 3S#S(S)9  UR                  S*5        UR!                  S+S!S,9  SU;   Ga  US   nUR                  5         UR                  U5        ["        R$                  " XS4["        R&                  S.9nUb  [        U5      S:  a  [        [        U5      5       H  nUUS4   UUS4   nn[)        ["        R*                  " ["        R,                  " USU
S-
  5      5      5      n [)        ["        R*                  " ["        R,                  " USU	S-
  5      5      5      n!/ S/QUU!U SS24'   M     Ub  [        U5      S:  a  [        [        U5      5       H  nUUS4   UUS4   n#n"[)        ["        R*                  " ["        R,                  " U"SU
S-
  5      5      5      n"[)        ["        R*                  " ["        R,                  " U#SU	S-
  5      5      5      n#/ S0QUU#U"SS24'   M     Sn$USU$-
  -  UU$-  -   n%UR                  U%5        UR                  S1S#S(S)9  UR                  S*5        [        [/        [0        S#5      5       GH  nSU 3U;  a  M  USU 3   n&U&R                  5         ["        R$                  " XS4["        R&                  S.9n'Ub  U[        U5      :  a  UUS4   UUS4   nn[)        ["        R*                  " ["        R,                  " USU
S-
  5      5      5      n [)        ["        R*                  " ["        R,                  " USU	S-
  5      5      5      n!/ S/QU'U!U SS24'   Ub  U[        U5      :  a  UUS4   UUS4   n#n"[)        ["        R*                  " ["        R,                  " U"SU
S-
  5      5      5      n"[)        ["        R*                  " ["        R,                  " U#SU	S-
  5      5      5      n#/ S0QU'U#U"SS24'   U&R                  U'5        U&R                  S2US-    S33S#S49  U&R                  S*5        GM     [        [/        [0        S#5      5       H  nSU 3U;  a  M  Uc  M  UUR                   S   :  d  M(  UU   R3                  X5      n(U(U(R/                  5       -
  U(R5                  5       U(R/                  5       -
  S5-   -  n)USU 3   n*U*R                  5         U*R                  U)S6SSS79  U*R                  S8US-    3S#S49  U*R                  S*5        M     X4$ )9a  
Create 4x4 grid visualization of predictions.

Args:
    rgb_lowres: (H, W, 3) RGB image at low resolution
    dino_vis: (H_patches, W_patches, 3) DINO visualization
    trajectory_points_lowres: (N, 2) GT trajectory in low-res coordinates
    trajectory_points_patches: (N, 2) GT trajectory in patch coordinates
    predicted_trajectory_lowres: (M, 2) Predicted trajectory in low-res coordinates
    predicted_trajectory_patches: (M, 2) Predicted trajectory in patch coordinates
    current_kp_2d_lowres: (2,) Current EEF position in low-res coordinates
    current_kp_2d_patches: (2,) Current EEF position in patch coordinates
    attention_scores: (window_size, num_patches) Attention scores
    H_patches, W_patches: Patch grid dimensions
    episode_id: Episode identifier string
    start_idx: Start frame index
    window_size: Number of future timesteps
    fig: Optional figure to plot on (for live updates)
    axes_dict: Optional dict of axes (for live updates)

Returns:
    fig, axes_dict: Figure and axes dict for live updates
r   N)   r   )figsize   333333?)figurehspacewspacerb   rgb)r   r   dinorc   dino_onehotr   onehot_r   
attention_b-r   ffffff?zGT Trajectory	linewidthalphalabelowhite      ?color
markersizemarkeredgecolormarkeredgewidthr-zPred Trajectoryx   r
  r  r  ro   Current EEFr   r  r  r  r  zorderzLow-Res Image (z)
z	 - Frame bold)fontsize
fontweightoffupper rightlocr  zDINO Patch Features (rR   )rH   rH   rH   )rH   r   r   z*DINO + One-hot Pixels
(White=GT, Red=Pred)z
One-hot t+z
(White=GT, Red=Pred)r  :0yE>hot)cmapvminvmaxzAttention t+)r   pltr   r   add_subplotr   clearimshowr   plot	enumeratecmviridisplasma	set_titler*   legendr   zerosr2   r+   roundclipr   max_timestepsr   r   )+
rgb_lowresdino_vistrajectory_points_lowrestrajectory_points_patchespredicted_trajectory_lowrespredicted_trajectory_patchescurrent_kp_2d_lowrescurrent_kp_2d_patchesattention_scoresr    r!   
episode_idr   r   fig	axes_dictRES_LOWgsgrid_idxr   rowcolax1ir  yr
  ax2ax3overlaykp_xkp_y
patch_x_gt
patch_y_gtpatch_x_predpatch_y_predr  blended	ax_onehot
onehot_imgattention_mapattention_map_normax_attns+                                              r#   visualize_predictionsrV    sk	   @ q!G {jj*a3s3?	 ??2d84	%  OOBtH5	& $'??2d8#<	-  {#Aa-CQ,Cax'*r#s(|'DIsm$MH a-CQ,Cax*-//"S#X,*GI
1#&'MH $" E
CIIKJJz+4L0MPQ0Q)!Q$/1I!Q$1OQUabjm  vE  	F"#;<IAv1FFNN1s+C'D#DEEHHQ3e7dgHh = '2s;V7WZ[7[HH0A68STUWXTX8Y[_kltw  @QH  R&'BC	6Aqa#.I*J&JKAs%AqQ D '%a(*>q*A4TU!(!=Y[ 	 	]MMOG9AgYc*YykZegtzM{HHUOJJ=1J- F
CIIKJJx ,5N1ORS1S*1a402KAqD2QSWcdlo  xG  	H"#<=IAv1FFNN1s+D'E#EFEHHQ3e7dgHh > (3<X8Y\]8]HH1!Q$79UVWYZVZ9[]amnvy  BSH  T&'CD	6Aqa#.J*K&KLAs%AqQ E (&q)+@+CTVW!(!=Y[ 	 	]MM))Ai[J<yYbXcdoq  EM  FHHUOJJ=1J- 	!&		

8 ((I!4BJJG %0S9R5SVW5W389:6q!t<>WXY[\X\>]d "''$9q=*I!JK
 "''$9q=*I!JK
5D
J12	 ; (3<X8Y\]8]3;<=-I!Q$-OQmnoqrnrQsl"288BGGL!YQR],S#TU"288BGGL!YQR],S#TU9HlA56	 > a%i(7U?:

7Cr^de 3}b)*QC=	)sm,	XXyQ7rzzJ
 %0Q=V9W5W21a48:STUWXTX:Y$DRXXbggdAy1}&EFGJRXXbggdAy1}&EFGJ4CJz:q01 (3C@\<]8])Ead)KMijkmnjnMo,LrxxaQ(OPQLrxxaQ(OPQL8GJ|\145$j1-CDrRu1 +6 3}b)*s9, 'A0@0F0Fq0I,I,Q/77	MM"/-2C2C2E"E-J[J[J]`m`q`q`sJsvzJz!{*QC 01GMMONN-ENJQqSE2R@LL + >r%   c                    UR                  SS9n/ nU H   nUU-  nUU-  nUR                  UU/5        M"     [        R                  " U5      nUR                  SS9R	                  5       n/ nU H   nUU-  nUU-  nUR                  UU/5        M"     [        R                  " U5      nUb  UR	                  5       nO2[        U5      S:  a  US   O[        R                  " US-  U
S-  /5      nU SS2SS24   R                  XS5      R	                  5       n[        S5       HX  nUSS2SS2U4   nUR                  5       UR                  5       nnUU:  a  UU-
  UU-
  -  USS2SS2U4'   ML  SUSS2SS2U4'   MZ     [        R                  " USS5      nUR                  5         UR                  U5        [        U5      S:  Ga  UR                  USS2S4   USS2S4   S	SS
SS9  [        U5       HI  u  nu  n n![        R                   R#                  U[        U5      -  5      n"UR                  U U!SU"SSSS9  MK     [        U5      S:  ay  UR                  USS2S4   USS2S4   SSS
SS9  [        U5       HH  u  nu  n n![        R                   R%                  U[        U5      -  5      n"UR                  U U!SU"SSS9  MJ     Ub  UR                  US   US   SSSSSSS9  UR'                  U SUS-    SU
 SU S3SS9  UR)                  SS S!9  UR+                  S"5        / n#/ n$Sn%UbV  [-        U[.        R0                  5      (       a#  UR                  X5      R	                  5       S:  n%OUR3                  X5      S:  n%[        [        US5      5       GH  n&U&UR4                  S   :  Ga   UU&   R3                  X5      n'U%b4  [        R6                  " U%5      (       a  U'U%   R                  5       n(U(U'U%) '   U'U'R                  5       -
  U'R                  5       U'R                  5       -
  S#-   -  n)[        R                   R9                  U)5      SS2SS2SS24   n*U%b)  [        R:                  " U*5      n+/ S$QU+U%) '   S
U*-  S%U+-  -   n*[.        R<                  " U*5      R?                  SSS5      RA                  5       n,U#R                  U,5        [        RB                  " SX4[        RD                  S&9n-U&[        U5      :  a  UU&S4   UU&S4   n/n.[G        [        RH                  " [        R                  " U.SUS-
  5      5      5      n0[G        [        RH                  " [        R                  " U/SU
S-
  5      5      5      n1S'U-SS2U1U04'   U&[        U5      :  a  UU&S4   UU&S4   n3n2[G        [        RH                  " [        R                  " U2SUS-
  5      5      5      n2[G        [        RH                  " [        R                  " U3SU
S-
  5      5      5      n3S'U-SU3U24'   S(U-SU3U24'   S(U-SU3U24'   [.        R<                  " U-5      RA                  5       n4U$R                  U45        GM     [        U#5      S:  a;  [K        U#SSSS)9n5U5R?                  SSS5      RM                  5       R	                  5       n6[        U$5      S:  a;  [K        U$SSS(S)9n7U7R?                  SSS5      RM                  5       R	                  5       n8UR                  5         [        U#5      S:  a  [        U$5      S:  a  [        W6R4                  S   W8R4                  S   5      n9[N        RP                  " U6U9U6R4                  S   4[N        RR                  S*9n:[N        RP                  " U8U9U8R4                  S   4[N        RR                  S*9n;[        RT                  " U:U;/5      n<UR                  U<5        UR'                  S+SS9  Ok[        U#5      S:  a,  UR                  W65        U%b  S,OS-n=UR'                  S.U= 3SS9  O0[        U$5      S:  a!  UR                  W85        UR'                  S/SS9  UR+                  S"5        UR                  5         UR	                  5       n>U[V        [X        -
  -  [X        -   n?U>[V        [X        -
  -  [X        -   n@[        RZ                  " S[        U@5      S-   5      nAUR]                  UAS0-
  U@S1S2S3S4S59  UR]                  UAS0-   U?S[        U@5       S1S2S6S7S59  UR_                  S8S9S9  URa                  S:S9S9  UR'                  S;SS9  UR)                  S S9  URc                  S%S<9  UR                  5         [-        U[.        R0                  5      (       a  UR	                  5       OUnB[-        U	[        Rd                  5      (       a  U	OU	nC[        RZ                  " S[        WB5      S-   5      nDUR]                  UDS0-
  UBS1S2S=S>S59  UR]                  UDS0-   UCS[        UB5       S1S2S?S@S59  UR_                  S8S9S9  URa                  SAS9S9  UR'                  SBSS9  UR)                  S S9  URc                  S%S<9  g)Ca  
Visualize a training sample during training.

Args:
    dino_tokens_sample: (num_patches, dino_feat_dim) DINO tokens
    groundplane_coords_sample: (num_patches, 2) Ground-plane XZ coordinates
    current_eef_pos_sample: (2,) Current EEF position (optional, can be None)
    onehot_targets_sample: (max_timesteps, num_patches) One-hot targets
    heights_sample: (max_timesteps,) GT heights
    seq_id_sample: Episode ID string
    attention_scores: (max_timesteps, num_patches) Attention scores
    heights_pred_sample: (max_timesteps,) Predicted heights
    H_patches, W_patches: Patch grid dimensions
    max_timesteps: Maximum number of timesteps
    ax_vis, ax_attn, ax_height: Matplotlib axes
    epoch: Current epoch number
r   r)   )r   Nr   r   r   r  r   r  GTr  r  r   r  r	  r  Predr  r   r  r  r  r  r   r  z	 | Epoch z | DINO Patches ()r  r  r  r  r  r  )r   rH   rH   r   rR   rH   r   )nrowpadding	pad_value)interpolationz;Attention Maps (top) | One-hot: White=GT, Red=Pred (bottom)z (Volume Masked) zAttention MapszOne-hot: White=GT, Red=Predg?g?g333333?greenz	GT Height)widthr  r
  r  redzPred HeightTimestep	   z
Height (m)zHeight Trajectory)r  bluez
GT GripperorangezPred GripperzGripper ValuezGripper Open/Close Trajectory)3r   rI   r   rf   numpyr   viewr   r   r   r1  r&  r'  r(  r)  r$  r*  r+  r,  r-  r.  r*   r   r   Tensorr   r   anyr   
zeros_liker   r   r   r/  r2   r+   r0  r   r   cv2resizeINTER_LINEARvstackr   r   r.   bar
set_xlabel
set_ylabelgridr   )Edino_tokens_samplegroundplane_coords_samplecurrent_eef_pos_sampleonehot_targets_sampleheights_samplegrippers_sampleseq_id_sampler;  heights_pred_samplegrippers_pred_sampler    r!   r2  ax_visrU  	ax_height
ax_gripperepochvolume_mask_samplepredicted_patch_indicesr8  r   patch_ypatch_xgt_patch_indicesr6  r:  r4  rE  channelmin_valmax_valr  rF  r
  attention_imgsonehot_imgsvolume_mask_2dr   rS  min_valid_valuerT  attention_rgbmask_overlayattention_tensorrR  rJ  rK  rL  rM  rN  rO  onehot_tensorattention_gridattention_grid_nponehot_gridonehot_grid_nptarget_widthattention_resizedonehot_resizedcombined
mask_labelheights_gt_npr   heights_gt_denorm	timestepsgrippers_gt_npgrippers_pred_nptimesteps_grippersE                                                                        r#   visualize_training_sampler    s
   0 /5515=#% &"	/$++Wg,>? ' $&88,H#I  -333:@@B ""	/!(('7);<   !#)B C ) 6 < < > ADD]@^ab@b 9! <hjhphpr{}~r~  AJ  LM  AM  rN  iO "!RaR%(--iAFLLNH1X1a7#";;='++-W!(7!2w7H IHQ1W #HQ1W  wwxA&H LLN
MM(
$%)-ad35NqRSt5TVZfgorz~"#<=IAv1FFNN1s+D'E#EFEKK1c1ggjKk >
 +,q0KK4QT:<XYZ\]Y]<^`dpqy|  EKK  L&'CD	6Aqa#.J*K&KLAq#UqRST E ()!,.CA.FYZ#*A][] 	 	_ iay8I)TUV_U``abmop
MMmaM0
KK NK N%(%,,77/44YJPPRUXXN/77	MPSSN3}b)*%%a((,Q/77	MM )bff^.D.D"/"?"C"C"E1@~o."/-2C2C2E"E-J[J[J]`m`q`q`sJsvzJz!{FFJJ'9:1a!8DM )!}}];0?n_- #m 3cL6H H$//>FFq!QOUUW!!"23 XXq)7rzzJ
 s,--21a48:STUWXTX:Y$DRXXbggdAy1}&EFGJRXXbggdAy1}&EFGJ47Jq*j01 s/00)Ead)KMijkmnjnMo,LrxxaQ(OPQLrxxaQ(OPQL8;Jq,458;Jq,458;Jq,45((4::<=)Y +^ >Q">1PST*221a;??AGGI
;!!Q#N$,,Q1599;AAC MMO
>Q3{#3a#7,2215~7K7KA7NOJJ'8<IZI`I`abIc:dtw  uE  uE  FN\>CWCWXYCZ4[knk{k{|99/@Ax Wcef	^	q	 ()+9+E'2
N:,7"E	[	A	~&7"ELL OO"((*M-j1HIJV%j)@AJN		!S!23a78IMM)c/#4CsRYalMmMM)c/#67N<M8N#OWZbemr  {HM  Ia02+b9a NNN 0:?ELL0Y0Y_**,_nN/9:NPRPZPZ/[/[+au		!S%81%<=NN$s*N#SX^frNsNN$s*,<=Qc.>Q,RZ]ehpx  AON  P*q1/A682Fq!OO#Or%   r<   )FF)d   g{Gz?)r   NN)N)!__doc__rg  r   matplotlib.pyplotpyplotr$  matplotlib.gridspecr   r   rl  r   timepathlibr   torchvision.utilsr   scipy.spatial.transformr   r   r   r$   r7   rF   rP   r`   ru   r   r   r   r   r   rV  r   r%   r#   <module>r     s    S   (  
    ' 1 '<1,0*@$2$L"L4p:F|fl (*~H GK	Er%   