
    ]pi                         S 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	r
SSKrSSKJs  Jr  \R                   R#                  S\R                   R%                  \5      5        SSKJrJr  SSKJr  SrSrS\R6                  S	\
R8                  4S
 jr\R<                  " 5       S\S\S\R@                  S	\!\
R8                  \
R8                  \
R8                  \"4   4S j5       r#S r$\%S:X  a  \$" 5         gg)zGenerate a simple teaser image from the real 2D heatmap model.

Outputs a 1x3 grid:
  - Left: RGB input
  - Right: RGB with predicted 2D trajectory overlaid
  - Third: Predicted height per timestep (bar chart)
    N)Path)RealTrajectoryDatasetN_WINDOW)TrajectoryHeatmapPredictori  z1real_dino_tracks/checkpoints/real_tracks/best.pthrgb_bchwreturnc                    [         R                  " / SQU R                  S9R                  SSSS5      n[         R                  " / SQU R                  S9R                  SSSS5      nX-  U-   R	                  5       R                  5       R                  5       S   n[        R                  " UR                  SSS5      SS5      $ )z,(1,3,H,W) normalized -> (H,W,3) float [0,1].)g
ףp=
?gv/?gCl?device      )gZd;O?gy&1?g?r      )
torchtensorr   viewdetachcpunumpynpclip	transpose)r   meanstd
rgb_denorms       Z/Users/cameronsmith/Projects/robotics_testing/3dkeygrip/real_dino_tracks/teaser_img_gen.py_denorm_rgbr      s    <<-hooFKKAqRSUVWD
,,,X__
E
J
J1aQRTU
VC.4'//1557==?BJ77:''1a0!Q77    modelsampler   c           	      x   US   R                  S5      R                  U5      nUS   R                  5       R                  5       R	                  [
        R                  5      nUS   R                  5       R                  5       R	                  [
        R                  5      nUS   R                  5       R                  5       R	                  [
        R                  5      n[        UR                  SS5      5      n[        R                  " US   US9n[        R                  " [        US	   5      [        R                  US
9n	[        R                  " [        US   5      [        R                  US
9n
U " USSUU	U
S9u  pn[
        R                  " [        S4[
        R                  S9n[        [        5       H  nUSU4   n[        R                   " UR#                  S5      SS9R%                  U5      n['        UR#                  S5      R)                  5       R+                  5       5      nUR,                  u  nnUU-  nUU-  n[        U5      [        U5      4X'   M     US   R/                  5       R                  5       R                  5       R	                  [
        R                  5      n[1        U5      nUUUU4$ )zZRun model on a single dataset sample and extract argmax 2D trajectory + predicted heights.rgbr   trajectory_2dtrajectory_3dtrajectory_gripper
episode_idr   r
   )r   r   )dtyper   NF)gt_target_heatmaptrainingstart_keypoint_2dcurrent_heightcurrent_gripperr   )r&   )dim)	unsqueezetor   r   astyper   float32strgetr   r   floatzerosr   rangeFsoftmaxr   view_asintargmaxitemshaper   r   )r   r   r   r!   	traj2d_gttraj3d	traj_gripr%   r)   r*   r+   pred_logitspred_height_pred_xytlogits_tprobs_tflat_idxHWyxpred_height_mrgb_viss                            r   predict_trajectory_xyrO   %   s0   
 -
!
!!
$
'
'
/C'++-335<<RZZHIO$((*00299"**EF+,00288:AA"**MIVZZh78JYq\&A\\%t"5U]]SYZNll51#6emmTZ[O"'+%'#Ka hh!}BJJ7G8_q!t$))HMM"-15==hGw||B'..05578}}1MqLAha)
   N))+//1779@@LM#GG]J66r   c            	         [         R                  " SS9n U R                  S[        [        SS9  U R                  S[        SSS9  U R                  S	[        S S
S9  U R                  S[
        SSS9  U R                  S[        SSS9  U R                  SSSS9  U R                  5       n[        R                  " [        R                  R                  R                  5       (       a  SO&[        R                  R                  5       (       a  SOS5      n[        SU 35        [        SUR                   S35        [        [         ["        SS9n[        R$                  " UR                  US9nSS KnSU;   aT  SU;   aN  [)        US   5      Ul        [)        US   5      Ul        [        S UR*                  S! S"UR,                  S! S#35        S$U;   aT  S%U;   aN  [)        US$   5      Ul        [)        US%   5      Ul        [        S&UR.                  S' S"UR0                  S' S(35        UR3                  US)   5        UR5                  U5      R7                  5       n[        S*UR9                  S+S,5       35        [        S-UR:                   35        [=        UR:                  [         UR>                  UR>                  c  S.OS S/9n[A        U5      S:X  a  [C        S05      e[        [D        RF                  " URH                  S[A        U5      S.-
  5      5      nXg   n[K        X8U5      u  pp[L        RN                  " S.S1S2S39u  pUu  nnnURQ                  U	5        URS                  S4S5S6S79  URU                  S85        URQ                  U	5        URW                  U
S S 2S4   U
S S 2S.4   S9S:S;S<S=9  URY                  U
S S 2S4   U
S S 2S.4   S>S:S?S@SA9  URY                  U
SB   U
SC   SDSES?SFSA9  URS                  SGS5S6S79  URU                  S85        USH-  n[D        RZ                  " ["        5      nUR]                  UUSIS<SJ9  URS                  SKS5S6S79  UR_                  SL5        URa                  SM5        URc                  SNSOSPSQ9  URe                  SR["        SS-
  5        URg                  U5        URi                  5         [)        [D        Rj                  " U5      5      n[)        [D        Rl                  " U5      5      n[m        STSUUU-
  SV-   -  5      nURo                  UU-
  UU-   5        URq                  U SWU SX3SYSZ9  URs                  5         [u        URv                  5      nURx                  R{                  SNSNS[9  UR}                  US\S]S^9  [        S_U 35        UR~                  (       d  [L        R                  " 5         [L        R                  " U5        g )`Nz+Generate teaser image (1 sample, 1 episode))descriptionz--checkpointzPath to model checkpoint)typedefaulthelpz--dataset_rootz&scratch/parsed_moredata_pickplace_homezDataset rootz	--episodez(Optional episode name (e.g. episode_001)z--sample_idxr   z)Sample index within the loaded episode(s)z--save_pathzscratch/real_teaser.pngzWhere to save the teaser imagez--dont_show
store_truezDon't open a window; just save)actionrT   mpscudar   zUsing device: z
Loading model from z...F)target_sizen_windowfreeze_backbone)map_location
min_height
max_heightu*   ✓ Loaded height range from checkpoint: [z.6fz, z] mmin_grippermax_gripperu+   ✓ Loaded gripper range from checkpoint: [z.3f]model_state_dictu   ✓ Loaded model from epoch epoch?z
Loading dataset: r   )dataset_root
image_sizeepisodemax_episodeszDataset returned 0 samples.r   )      )figsizez	RGB input   bold)fontsize
fontweightoff-limer   g?)color	linewidthalpha   blackg      ?)sc
edgecolors
linewidths)r   r   )r   r   <   cyang?zPredicted trajectoryg     @@z#4C78A8)rs   ru   zPredicted height (mm)rE   mmTrL   g      ?)axisru   g333333g?g      ?g?gư>z (idx=)   )rn   )parentsexist_ok   tight)dpibbox_inchesu   ✓ Saved teaser image to )BargparseArgumentParseradd_argumentr2   DEFAULT_CHECKPOINT_PATHr:   
parse_argsr   r   backendsrW   is_availablerX   print
checkpointr   
IMAGE_SIZEr   loadr   r4   
MIN_HEIGHT
MAX_HEIGHTMIN_GRIPPERMAX_GRIPPERload_state_dictr/   evalr3   re   r   rg   lenRuntimeErrorr   r   
sample_idxrO   pltsubplotsimshow	set_titler   plotscatterarangebarh
set_ylabel
set_xlabelgridset_ylim
set_yticksinvert_yaxisminmaxset_xlimsuptitletight_layoutr   	save_pathparentmkdirsavefig	dont_showshowclose)parserargsr   r   r   model_moduledatasetidxr   rN   rD   rM   r%   figaxesax0ax1ax2	height_mmt_idxh_minh_maxpadr   s                           r   mainr   L   sJ   $$1^_F
S:QXrs
(s<dkyz
#tBlm
S!Bmn
)-	   lAabD\\5>>#5#5#B#B#D#D%TYT^T^TkTkTmTm&sxyF	N6(
#$	!$//!2#
67&:bghEDOO&AJ !z!lj&@"'
<(@"A"'
<(@"A:<;R;RSV:WWYZfZqZqruYvvyz{
"}
'B#(M)B#C #(M)B#C ;L<T<TUX;YY[\h\t\tux[yyz{|	*%789HHV!!#E	()E(F
GH	 1 12
34#&&,,.QD	G 7|q899
bggdooq#g,*:;
<C\F2GW]2^/GmQ73ICMCcJJwMM+vM>HHUOJJwHHWQT]GAqDM3fQTHUKK1wq!t}f]`KaKKwt}f]`KaMM(2&MIHHUO &IIIhEHHUIYcH:MM)B6MJNN3NN4HHT4H(LLx#~&NN5"&&#$E"&&#$E
c3%%-$./
0CLLeck*LLJ<vcU!,rL:T^^$I4$7KK	sK8	&yk
23>>
IIcNr   __main__)&__doc__r   ossyspathlibr   matplotlib.pyplotpyplotr   r   r   r   torch.nn.functionalnn
functionalr7   pathinsertdirname__file__datar   r   r   r   r   r   Tensorndarrayr   no_graddictr   tupler2   rO   r   __name__ r   r   <module>r      s     	 
       277??8, - 0 ,
M 8%,, 82:: 8 #7%#7/3#7=B\\#7
2::rzz2::s23#7 #7L[| zF r   