
    "i                          d Z ddlZddlmZ ddlmZ ddl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 ZddZd	 Zd
 Zd Zd Zd Zd ZddZd ZddZ	 	 ddZ	 ddZ dS )zMGeneral utilities for token selection model: geometry, visualization, and IK.    N)GridSpec)Path)	make_grid)Rotationc                    t          j        | dd          }t          |t          j                  rt          j        |          }|                                }dx}}|                                dk    r&|j        d         dk    r|	                    d          }|                                dk    rU|j        d         |j        d	         k     r|
                    dd
d          }|j        \  }}}|                    d	|          }n}|                                d
k    rD|j        d         |j        d         k     r'|j        d         dk    r|                    dd          }n!|                    |j        d         d	          }|||fS )aB  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        V/Users/cameronsmith/Projects/robotics_testing/3dkeygrip/volume_tracks_smolvla/utils.pyload_dino_featuresr$      s_    Jt%eDDDE%$$ ( ''KKMME  I	yy{{aEKNa//a  yy{{a;q>EKO++MM!Q**E"'+	9ab!$$			;q>EKN**u{1~/D/DOOAq))Eek!nb11)Y&&    c                    ||=t          t          j        |                     }| |z  dk    r|dz  }| |z  dk    | |z  }t          j        t          j        |          t          j        |          d          \  }}t          j        |                                |                                gd                              t          j                  }|||fS )ah  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
    Nr   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-,,--	I%**NI I%**9,	RYy%9%929Y;O;OZ^___Hhh 0 0 2 2H4D4D4F4FGaPPPWWXZXbccOIy00r%   c                    t          |j                  d}| d| dz  }| d| dz  }|                                r<|                                r(t          j        |          t          j        |          fS | dz  }| dz  }|                                r<|                                r(t          j        |          t          j        |          fS dS )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"Fy"F"F"FFM7	7777J ;*"3"3"5"5 ;w}%%rwz':':::!$;;O,L ?L$7$7$9$9 ?w'')>)>>>:r%   c                     t          j        | d          }||z  dd         }|d         dk    rdS ||z  }|dd         |d         z  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   \   s[    8S))Jz)2A2.I|qt"Jbqb>JqM))r%   c                 0   | dS t          j        | t           j                  } | j        dk    r| S | j        dk    r|                     dd          } d}nd}||z  }||z  }t          j        | d         |z  | d	         |z  gd
          }|r|d         S |S )aU  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     ~tZbj111F{a{a2&&fnGfnGh	'	!6&>G#;<2  O  "q!!r%   c                 t   t           j                            |          }|dddf         }|ddddf         }|d         |d         }}|d         |d         }	}| d         |z
  |z  }
| d         |	z
  |z  }d	}t          j        |
||g          }|t           j                            |          z  }||z  }||fS )
a\  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     9==--L !Q'Mrr2A2v&K4[%+B4[%+Ba[2#Ea[2#EEhue,--G	w///Gg%I)##r%   c                     | |dS t          | ||          \  }}t          |d                   dk     rdS |d          |d         z  }|||z  z   }|                                }||d<   |S )a  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      s    " fnt,[+uMMGY
 9Q<4t 
{Yq\)HX	11L EE!HLr%   c                     | |dS t          | ||          \  }}t          |d                   dk     rdS ||d         z
  |d         z  }|dk     rdS |||z  z   }|S )aS  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      s     fnt0k5QQG] =t##t 
'!*	a 00A1uut a-''ELr%   Fc                 H   g }g }g }	t          d|dz             D ]}
||
z   }|t          |          k    r nt          ||         j                  d}| | dz  }|                                sUt          j        |          }|ddddf         }|dddf         }||z  |z   }|                    |           |r|                    |d                    |r|	                    |           t          |          dk    rt          j        |          n't          j        g           	                    dd          }|g}|rPt          |          dk    rt          j        |          nt          j        g           }|                    |           |ret          |	          dk    rt          j        |	          n(t          j        g           	                    ddd          }	|                    |	           t          |          dk    r|d         S t          |          S )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      sH   $ JO;?++ ( (F"C$$$$E;u-23399	Y"A"A"AA	!! 	wy!!2A2rr6l2A2q5kh$&&& 	(eAh''' 	(""3'''589I5J5JQ5N5Nrx 0111TVT\]_T`T`ThThijlmTnTn
C -0__q-@-@RXj)))bhrll


: $7:?7K7Ka7O7O"(?333UWU]^`UaUaUiUijkmnpqUrUr

?###
3xx1}}1v::r%   c                    t          j        |           } t          j        |          }| j        dk    r|                     dd          } |j        dk    r|                    d          }| j        d         }| j        d         }	|                     d          }
g }|
D ]#}||z  }||z  }|                    ||g           $t          j        |          }t          |||||          }|t          t          z
  z  t          z   }g }t          t          t          |          t          |                              D ];}||         }||         }t          ||||          }||                    |           <t          |          dk    rt          j        |          n't          j        g                               dd          }|||fS )a  
    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)   Nr   )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2A#++A..$Q'K$Q'K "((a(00N L & &I9_RH%%%%8L))L '|Y	6SYZZ '*z*ABZO 3s,--s3F/G/GHHII 2 2!!$"?q+W\]]
!%%j1119<=O9P9PST9T9T"4555Z\ZbceZfZfZnZnoprsZtZt02EEEr%   d   {Gz?c                    ddl m}m}	 |                                 }
|
d         |k     r||
d<   t	          |          D ]} ||||          } |	||||           t          j        ||           |                    |j                   t          j
        dddd          }t          j        |t
          j        j        d          }t          j        |j        |         g d	                                                   }t          j        |                                          }|                    t          j        t-          j        |d
         |d         |d         |d         g|
g                               t          j        |                                          }t          j        |t
          j        j        d          }|j        |                                         }t          j
        dddd          }|                    t          j        t-          j        |d
         |d         |d         |d         g|g                               t          j        |d          }|                    |j                   t          j        ||||gddt          j        |          g          }|                    |d           |j        |j        dd<   |j        dt=          |j                           |j        dd<   t          j         ||           t          j        ||           t          j        |t
          j        j        d          }|j        |                                         }|d         |k     rtC          |
d         |          |
d<   |dz  dk    r*t,          j"        #                    ||
z
            }|dk     r nt          j        ||           t          j        |t
          j        j        d          }|j        |                                         }|d         |k     r||
d<   t	          d          D ]} ||||          } |	||||           t          j        ||           |                    |j                   t          j
        dddd          }t          j        |j        |         g d	                                                   }t          j        |                                          }|                    t          j        t-          j        |d
         |d         |d         |d         g|
g                               t          j        |                                          }t          j        |t
          j        j        d          }|j        |                                         }t          j
        dddd          }|                    t          j        t-          j        |d
         |d         |d         |d         g|g                               t          j        |d          }|                    |j                   t          j        ||||gddt          j        |          g          }|                    |d           |j        |j        dd<   |j        dt=          |j                           |j        dd<   t          j         ||           t          j        ||           |j        |                                         }|d         |k    r dS dS dS )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   `  s    QPPPPPPP !. 2 2 4 4 #&==='>!!$>** , ,	..|XwOO
##L(GZPPP(G,,,W\*** .!;VSVilmmm&x1IKeff
W]:6|||DEEOOQQ-''//1148R^gaj'RS*V]^_V`bijkbl=m  pI  =J  .K  .K  L  L  L  	M  	M  	M }%788@@BB +Hfm6NP[\\l?388::~k6_cddd".<PQ?T`abTceqrset  wC  DE  wF  CG  IT  BU  3V  3V  !W  !W  !W  	X  	X  	X't<<<---mMG\<+PRVX^hlh  GO  iP  iP  iP  hQ  R  R  R''T222'/QQQ'/*<3w|+<+<*<=QQQx))) 	(G,,,&x1IKeff
 j16688 !666+./H/KMd+e+e%a( r>QINN>4M#MNNEu}} h((("8V]-EGabbJ<
+0022LA000'>!!$r 	 	A22<7SSJ''hTTTh000  ...n%?WZmpqqqG[z!:<<<!HIISSUUFmF++3355Gtx'!*gVWjZabcZdfmnofpAq  tM  AN  2O  2O   P   P   P  Q  Q  Q=);<<DDFFL$/&-:RT_``O!,7<<>>K>+vScefffL##DHbn|TUXdefXgiuvwix  {G  HI  {J  GK  MX  FY  7Z  7Z  %[  %[  %[  \  \  \+H4@@@L##GL111-|/TVZ\blp  mD  KS  mT  mT  mT  lU  V  V  VC++C666+oGLO+o.@s7</@/@.@AGLON8W---h000"<
388::LA"999 :? 10	 	r%   r   c                    | j         d         }|t          j        d          }t          dd|dd          }i }|                    |d                   |d	<   |                    |d
                   |d<   |                    |d                   |d<   d}t          |          D ]v}|dz  }|dz  }|dk    r nc|                    |||f                   |d| <   |dz  }|dz  }|dz  }|dk    r n)|                    |||f                   |d| <   |dz  }w|d	         }|                                 |                    |            |Lt          |          dk    r8|	                    |dddf         |dddf         dddd           t          |          D ]S\  }\  }}t          j                            |t          |          z            }|	                    ||d|ddd           T|t          |          dk    r|	                    |dddf         |dddf         dddd           t          |          D ]R\  }\  }}t          j                            |t          |          z            }|	                    ||d|dd           S|)|	                    |d         |d         d d!ddd"d#$           |                    d%| d| d&| d'| d#d()           |                    d*           |                    d+d!,           |d         }|                                 |                    |           |Lt          |          dk    r8|	                    |dddf         |dddf         dddd           t          |          D ]S\  }\  }}t          j                            |t          |          z            }|	                    ||d|ddd           T|t          |          dk    r|	                    |dddf         |dddf         dddd           t          |          D ]R\  }\  }}t          j                            |t          |          z            }|	                    ||d|dd           S|)|	                    |d         |d         d d!ddd"d#$           |                    d-|	 d|
 d&| d'| d#d()           |                    d*           |                    d+d!,           d|v r.|d         }|                                 |                    |           t#          j        |	|
dft"          j        .          }|t          |          dk    rt          t          |                    D ]}||df         ||df         }}t)          t#          j        t#          j        |d|
dz
                                } t)          t#          j        t#          j        |d|	dz
                                }!g d/||!| ddf<   |t          |          dk    rt          t          |                    D ]}||df         ||df         }#}"t)          t#          j        t#          j        |"d|
dz
                                }"t)          t#          j        t#          j        |#d|	dz
                                }#g d0||#|"ddf<   d}$|d|$z
  z  ||$z  z   }%|                    |%           |                    d1d#d()           |                    d*           t          t/          t0          d#                    D ]}d| |vr|d|          }&|&                                 t#          j        |	|
dft"          j        .          }'||t          |          k     r||df         ||df         }}t)          t#          j        t#          j        |d|
dz
                                } t)          t#          j        t#          j        |d|	dz
                                }!g d/|'|!| ddf<   ||t          |          k     r||df         ||df         }#}"t)          t#          j        t#          j        |"d|
dz
                                }"t)          t#          j        t#          j        |#d|	dz
                                }#g d0|'|#|"ddf<   |&                    |'           |&                    d2|dz    d3d#4           |&                    d*           t          t/          t0          d#                    D ]}d| |vr
|||j         d         k     r||                             |	|
          }(|(|(                                z
  |(                                |(                                z
  d5z   z  })|d|          }*|*                                 |*                    |)d6dd7           |*                    d8|dz    d#4           |*                    d*           ||fS )9aj  
    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_predictionsrX    s   @ q!G {j***a3s3???	 ??2d844	%  OOBtH55	& $'??2d8#<#<	-  {## 	 	Aa-CQ,Caxx'*r#s(|'D'DImmm$MH a-CQ,Caxx*-//"S#X,*G*GI&1&&'MHH E
CIIKKKJJz+4L0M0MPQ0Q0Q)!!!Q$/1I!!!Q$1OQUabjm  vE  	F  	F  	F"#;<< 	i 	iIAv1FNN1s+C'D'D#DEEEHHQ3e7dgHhhhh&2s;V7W7WZ[7[7[HH0A68STUTUTUWXTX8Y[_kltw  @QH  R  R  R&'BCC R R	6Aqa#.I*J*J&JKKAs%AqQQQQ'%a(*>q*A4TU!(!=Y[ 	 	] 	] 	]MMZGZZgZZ*ZZyZZegtzM{{{HHUOOOJJ=1J--- F
CIIKKKJJx ,5N1O1ORS1S1S*111a402KAAAqD2QSWcdlo  xG  	H  	H  	H"#<== 	i 	iIAv1FNN1s+D'E'E#EFFEHHQ3e7dgHhhhh'3<X8Y8Y\]8]8]HH1!!!Q$79UVWVWVWYZVZ9[]amnvy  BSH  T  T  T&'CDD R R	6Aqa#.J*K*K&KLLAs%AqQQQQ(&q)+@+CTVW!(!=Y[ 	 	] 	] 	]MMd)ddiddJddYbddoq  EM  F  F  FHHUOOOJJ=1J--- 	!!&		

8 (Iy!4BJGGG %0S9R5S5SVW5W5W3899:: E E6q!t<>WXY[\X\>]d "'$9q=*I*I!J!JKK
 "'$9q=*I*I!J!JKK
5D__
J122 (3<X8Y8Y\]8]8]3;<<== I I-I!Q$-OQmnoqrnrQsl"28BGL!YQR],S,S#T#TUU"28BGL!YQR],S,S#T#TUU9HlAAA566 a%i(7U?:

7Dr^deee 3}b))**  Q==	))mmm,	Xy)Q7rzJJJ
 %0Q=V9W9W5W5W21a48:STUWXTX:Y$DRXbgdAy1}&E&EFFGGJRXbgdAy1}&E&EFFGGJ4COOJz:qqq01 (3C@\<]<]8]8])Ead)KMijkmnjnMo,LrxaQ(O(OPPQQLrxaQ(O(OPPQQL8GJ|\11145$$$D1DDDrRRRu 3}b))**    9,, 'A0@0Fq0I,I,I,Q/77	9MMM"/-2C2C2E2E"E-J[J[J]J]`m`q`q`s`sJsvzJz!{ 0Q 0 01GMMOOONN-ENJJJ2QqS22R@@@LL	>r%   c                 J   |                     d          }g }|D ]#}||z  }||z  }|                    ||g           $t          j        |          }|                     d                                          }g }|D ]#}||z  }||z  }|                    ||g           $t          j        |          }||                                }n7t          |          dk    r|d         nt          j        |dz  |
dz  g          }| ddddf                             |
|d                                          }t          d          D ]a}|dddd|f         }|                                |	                                }}||k    r||z
  ||z
  z  |dddd|f<   Ud|dddd|f<   bt          j
        |dd          }|                                 |                    |           t          |          dk    r6|                    |dddf         |dddf         d	dd
d           t          |          D ]S\  }\  } }!t          j                            |t          |          z            }"|                    | |!d|"ddd           Tt          |          dk    r|                    |dddf         |dddf         ddd
d           t          |          D ]R\  }\  } }!t          j                            |t          |          z            }"|                    | |!d|"dd           S|)|                    |d         |d         dddddd           |                    | d|dz    d|
 d| dd           |                    dd !           |                    d"           g }#g }$d}%|at-          |t.          j                  r-|                    |
|                                          dk    }%n|                    |
|          dk    }%t          t          |d                    D ] }&|&|j        d         k     r=||&                             |
|          }'|%4t          j        |%          r |'|%                                         }(|(|'|% <   |'|'                                z
  |'	                                |'                                z
  d#z   z  })t          j                            |)          ddddddf         }*|%'t          j        |*          }+g d$|+|% <   d
|*z  d%|+z  z   }*t/          j        |*                              ddd                                           },|#                    |,           t          j!        d|
|ft          j"        &          }-|&t          |          k     r||&df         ||&df         }/}.tG          t          j$        t          j
        |.d|dz
                                }0tG          t          j$        t          j
        |/d|
dz
                                }1d'|-dd|1|0f<   |&t          |          k     r||&df         ||&df         }3}2tG          t          j$        t          j
        |2d|dz
                                }2tG          t          j$        t          j
        |3d|
dz
                                }3d'|-d|3|2f<   d(|-d|3|2f<   d(|-d|3|2f<   t/          j        |-                                           }4|$                    |4           t          |#          dk    rNtK          |#ddd)          }5|5                    ddd          &                                                                }6t          |$          dk    rNtK          |$ddd()          }7|7                    ddd          &                                                                }8|                                 t          |#          dk    rt          |$          dk    rt          |6j        d         |8j        d                   }9tO          j(        |6|9|6j        d         ftN          j)        *          }:tO          j(        |8|9|8j        d         ftN          j)        *          };t          j*        |:|;g          }<|                    |<           |                    d+d           nt          |#          dk    r6|                    |6           |%d,nd-}=|                    d.|= d           n?t          |$          dk    r,|                    |8           |                    d/d           |                    d"           |                                 |                                }>|tV          tX          z
  z  tX          z   }?|>tV          tX          z
  z  tX          z   }@t          j-        dt          |@          dz             }A|.                    |Ad0z
  |@d1d2d3d45           |.                    |Ad0z   |?dt          |@                   d1d2d6d75           |/                    d8d9           |0                    d:d9           |                    d;d           |                    d            |1                    d%<           |                                 t-          |t.          j                  r|                                n|}Bt-          |	t          j2                  r|	n|	}Ct          j-        dt          |B          dz             }D|.                    |Dd0z
  |Bd1d2d=d>5           |.                    |Dd0z   |Cdt          |B                   d1d2d?d@5           |/                    d8d9           |0                    dAd9           |                    dBd           |                    d            |1                    d%<           dS )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   r3  r(  r)  r*  r+  r&  r,  r-  r.  r/  r0  r*   r   r   Tensorr   r   anyr"  
zeros_liker   r   r   r1  r2   r+   r2  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!   r4  ax_visrW  	ax_height
ax_gripperepochvolume_mask_samplepredicted_patch_indicesr:  r   patch_ypatch_xgt_patch_indicesr8  r<  r6  rG  channelmin_valmax_valr  rH  r  attention_imgsonehot_imgsvolume_mask_2dr   rU  min_valid_valuerV  attention_rgbmask_overlayattention_tensorrT  rL  rM  rN  rO  rP  rQ  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    s9   0 /5515==#% & @ @"	/$++Wg,>????#%8,H#I#I  -333::@@BB " = ="	/!(('7);<<<< ")B C C ) 6 < < > > ADD]@^@^ab@b@b 9! < <hjhpr{}~r~  AJ  LM  AM  rN  iO  iO "!!!RaR%(--iAFFLLNNH1XX $ $111aaa7#";;=='++--W!(7!2w7H IHQQQ1W #HQQQ1WwxA&&H LLNNN
MM(
$%%))-aaad35NqqqRSt5TVZfgorz~"#<== 	l 	lIAv1FNN1s+D'E'E#EFFEKK1c1ggjKkkkk +,,q00KK4QQQT:<XYZYZYZ\]Y]<^`dpqy|  EKK  L  L  L&'CDD U U	6Aqa#.J*K*K&KLLAq#UqRSTTTT()!,.CA.FYZ#*A][] 	 	_ 	_ 	_ bbabb)bbV_bbbmoppp
MMmaM000
KK NK N%(%,77 	T/44Y	JJPPRRUXXNN/77	9MMPSSN3}b))** ,* ,*%a(((,Q/77	9MMM )bf^.D.D)"/"?"C"C"E"E1@~o."/-2C2C2E2E"E-J[J[J]J]`m`q`q`s`sJsvzJz!{FJJ'9::111aaa!8DM )!}];;0?n_- #m 3cL6H H$/>>FFq!QOOUUWW!!"2333 Xq)Y7rzJJJ
 s,----21a48:STUWXTX:Y$DRXbgdAy1}&E&EFFGGJRXbgdAy1}&E&EFFGGJ47Jqqq*j01 s/0000)Ead)KMijkmnjnMo,LrxaQ(O(OPPQQLrxaQ(O(OPPQQL8;Jq,458;Jq,458;Jq,45(44::<<=)))) >Q">1PSTTT*221a;;??AAGGII
;!!Q#NNN$,,Q15599;;AACC MMOOO
>Q3{#3#3a#7#7,215~7KA7NOOJ'8<IZI`abIc:dtw  uE  F  F  FN\>CWXYCZ4[knk{|||9/@AAx   Xceffff	^		q	 	 ()))+9+E''2
7:77"EEEE	[		A		~&&&7"EEELL OO"((**M-j1HIJV%j)@AJN	!S!233a788IMM)c/#4CsRYalMmmmMM)c/#67N<M8N8N7N#OWZbemr  {HM  I  I  Ia000222+b999a   NNN 0:?EL0Y0Yn_**,,,_nN/9:NPRPZ/[/[u++au	!S%8%81%<==NN$s*N#SX^frNsssNN$s*,<=Qc.>Q>Q=Q,RZ]ehpx  AON  P  P  P*q111/A66682FFFq!!!OO#Or%   r<   )FF)r   r   )r   NN)N)!__doc__ri  r   matplotlib.pyplotpyplotr&  matplotlib.gridspecr   r   rn  r   timepathlibr   torchvision.utilsr   scipy.spatial.transformr   r   r   r$   r7   rF   rP   r`   ru   r   r   r   r   r   rX  r   r%   r#   <module>r     s   S S           ( ( ( ( ( (  



         ' ' ' ' ' ' 1 1 1 1 1 1 ' ' '<1 1 1 1,  0* * *  @$ $ $2$ $ $L" " "L4 4 4 4p:F :F :F|f f f fl (*~ ~ ~ ~H GK	E E E E E Er%   