
    piP6                     T   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SS jrS rS r \!S:X  a  \ " 5         gg)zVVisualize trajectory predictions on the real dataset (2D heatmaps + height + gripper).    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       W/Users/cameronsmith/Projects/robotics_testing/3dkeygrip/real_dino_tracks/vis_eval_2d.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    c                    [        U R                  5       5      R                  nU R                  5         [	        U[        U5      5      n[        R                  " S[        U5      S-
  U[        S9nSn[        SUS-
  5      nX7-  U-   n	[        R                  " U	[        S[        -  SU	-  4S9u  pU	S:X  a  U[        R                  SS24   n[        S:X  a  USS2[        R                  4   n[        R                  " 5          [!        U5       GH8  u  pX   nUS   R#                  S5      R%                  U5      nUS	   R'                  5       R)                  5       nUS
   R'                  5       R)                  5       nUS   R'                  5       R)                  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  nnn[1        U5      n/ n/ n[3        [        5       H  nUSU4   n[4        R6                  " UR9                  S5      SS9R;                  U5      R'                  5       R)                  5       nUR=                  U5        [        R>                  " URA                  5       URB                  5      u  nn UR=                  [-        U 5      [-        U5      45        M     [        U5      (       a  [-        [        S U 5       5      5      OSn![        U!S5      n!XS-   -  n"UU"S-   S4   RE                  SSSSSS9  UU"S-   S4   RE                  SSSSSS9  UU"S-   S4   RE                  SSSSSS9  [3        [        5       GH[  nUU"S-   U4   n#U#RG                  U5        U#RI                  SU 3SSS 9  U#RK                  S!5        UU"S-   U4   n$U$RG                  UU   S"S#9  U$RK                  S!5        UU"S-   U4   n%U%RG                  U5        UU   n&UU   u  n n[        RL                  " U U/[        R.                  S9n'U%RO                  U&S   U&S   S$S%S&S'SS(SS)9	  U%RO                  U US*S%S+SS,SS-9  [-        [        RP                  RS                  U&U'-
  5      5      n(U%RI                  S.U(S/ 3S0S19  U%RK                  S!5        US:X  d  GMK  U%RU                  S2S3S49  GM^     XS-
  :  d  GM  U"U-   n)[3        [        5       H  nUU)U4   n*U*RK                  S!5        M     GM;     SSS5        [        RV                  " US5SS 9  [        RX                  " S6S7S8S99  U(       a%  [        RZ                  " US:S;S<9  []        S=U 35        g[        R^                  " 5         g! , (       d  f       N|= f)>zVisualize predictions on real dataset samples.

Layout: 3 rows per sample
  - Row 1: RGB (raw)
  - Row 2: Heatmap only (predicted probability map)
  - Row 3: RGB + extracted keypoint overlay (argmax)
r   r   dtyper   g@)figsizeNrgbtrajectory_2dtrajectory_3dtrajectory_gripperr
   r   r   )r    r   Fgt_target_heatmaptrainingstart_keypoint_2dcurrent_heightcurrent_gripperdimc              3   @   #    U  H  oR                  5       v   M     g 7f)N)max).0ps     r   	<genexpr>(visualize_predictions.<locals>.<genexpr>[   s     !B>a%%''>s   g      ?g:0yE>RGB   
   bold)rotationlabelpadfontsize
fontweightHeatr   zRGB+KPzt=	   )r<   r=   offhot)cmapwhiteZ   oblackGT)csmarker
edgecolors
linewidthslabelzorderlimexPred)rH   rI   rJ   rL   rM   rN   zPx:z.1f   )r<   zupper right   )locr<      g      ?g{Gz?gq=
ףp?)hspacewspacetop   tight)dpibbox_inchesu   ✓ Saved visualization to )0next
parametersr   evalminlenr   linspaceintr1   pltsubplotsr   newaxisr   no_grad	enumerate	unsqueezetor   r   r   floatfloat32r   rangeFsoftmaxr   view_asappendunravel_indexargmaxshape
set_ylabelimshow	set_titleaxisarrayscatterlinalgnormlegendsuptitlesubplots_adjustsavefigprintshow)+modeldatasettitlenum_samples	save_pathr   indicesrows_per_samplespacer_rowsn_rowsfigaxesiidxsampler"   r#   r$   r%   r*   r+   r,   pred_logitspred_heightpred_gripperrgb_vispred_probs_allpred_kp_xy_alltpred_logits_tpred_probs_tpred_ypred_x	heat_vmaxbase_rowax_rgbax_hmax_kptarget_2d_t	pred_2d_t	pixel_err
spacer_rowax_sps+                                              r   visualize_predictionsr       s    %""$%,,F	JJLk3w<0Kkk!S\A-{#FG Oaq)K*[8FVXHcFl7STIC{BJJM"1}ArzzM"	(FA\F-))!,//7C"?3779??AM"?3779??AM!'(<!=!A!A!C!I!I!K %]1-=f M"\\%d0C*DEMMbhiN#ll51CA1F+Gu}}eklO5:"&"3- /62Kl "#&G  NN8_ +AqD 1 yy););B)?QGOOP]^bbdjjl%%l3!#!1!1,2E2E2GI[I[!\%%uV}eFm&DE % HK>GZGZc!B>!BBC`cIIt,Ia/0HAq!,,UQVXek,lAq!,,Va"WYfl,mAq!,,XBY[hn,o 8_hlAo.g&  2aSA& IE" X\1_-^A.U;

5! X\1_-W%+A.!/!2HHff%5RZZH	NN&   
     	 ""))..y1H"IJ	#i_ 5B

5!6LL]QL?] %b ?"%7
xA Q/EJJu% ){ ) 
D LL7tDd;I3G<+I;78
U 
s   O,W4W8W
Wc                    [        U R                  5       5      R                  nU R                  5         SnSnSnSn[        R
                  " 5          [        [        U5      5       GHL  nX   nUS   R                  S5      R                  U5      n	US   n
US   nUS   nU
S   R                  U5      nUS   R                  U5      nUS   R                  U5      nU " U	SS	UUUS
9u  nnnUR                  u  nnnn/ n/ n/ n[        [        5       GHL  nUSU4   n[        R                  " UR                  S5      SS9R                  UU5      nUR                  S5      R                  5       nUU-  nUU-  n[        R                   " UU/[        R"                  S9n U
U   n![        R$                  " U U!-
  5      R'                  5       n"[        R(                  " USU4   UUS4   -
  5      R'                  5       n#[        R(                  " USU4   UU   -
  5      R'                  5       n$UR+                  U"5        UR+                  U#5        UR+                  U$5        GMO     U[,        R.                  " U5      -  nU[,        R.                  " U5      -  nU[,        R.                  " U5      -  nUS-  nGMO     SSS5        UX6-  XF-  S-  XV-  S.n%U%$ ! , (       d  f       N = f)z+Compute simple metrics on the real dataset.g        r   r"   r#   r$   r%   r&   NFr'   r-   r.   r   r   r     )	n_samplesavg_pixel_error_pxavg_height_error_mmavg_gripper_abs_error)r]   r^   r   r_   r   rg   rm   ra   ri   rj   rt   r   rn   ro   r   rs   r   rl   r|   itemabsrq   r   r   )&r   r   r   total_pixel_errortotal_height_errortotal_gripper_errorr   r   r   r"   r#   r$   r%   r*   r+   r,   r   r   r   BNHWpixel_errorsheight_errorsgripper_errorsr   r   r   pred_flat_idxr   r   r   r   pixel_error_theight_error_tgripper_error_tmetricss&                                         r   compute_metricsr      s   %""$%,,F	JJLI	W&C\F-))!,//7C"?3M"?3M!'(<!= !.a 0 3 3F ;*4033F;N0366v>O 6;"&"3- /62Kl %**JAq!QLMN8_ +AqD 1 yy););B)?QGLLQPQR , 1 1" 5 < < >&!+&*!LL&&)9O	 ,A. %

9{+B C H H J!&;q!t+<}QPQT?R+R!S!X!X!Z"'))LA,>ASTUAV,V"W"\"\"^##M2$$^4%%o6% %* !66"''-"88277>#::NIi ' 
p /; 2 >$F"5"A	G N} 
s   I&K
Kc                  v   [         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                  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;   a  S%U;   a|  [)        US$   5      Ul        [)        US%   5      Ul        [        S&UR*                  S' S(UR,                  S' S)35        [        S*UR*                  S+-  S, S-UR,                  S+-  S, S.35        S/U;   aT  S0U;   aN  [)        US/   5      Ul        [)        US0   5      Ul        [        S1UR.                  S2 S(UR0                  S2 S335        UR3                  US4   5        UR5                  U5      nUR7                  5         [        S5US6    35        [        S7UR8                   35        [;        UR8                  [         UR<                  UR<                  b  S OUR>                  S89n[        S9[A        U5       S:35        [        S;5        [C        X65      n[        S<S= 35        [        S>5        [        S= 5        [        S?US@    35        [        SAUSB   S, SC35        [        SDUSE   S2 SF35        [        SGUSH   SI 35        [        S= 5        URD                  (       dn  [        SJ5        [G        URH                  5      nURK                  SKSKSL9  S n	URL                  (       a  S#S K'n
U
RQ                  5         [S        X6SMURT                  U	SN9  g g )ONz/Visualize trajectory predictions (real dataset))descriptionz--checkpointzPath to model checkpoint)typedefaulthelpz--dataset_rootz&scratch/parsed_moredata_pickplace_homezRoot directory of real datasetz	--episodezROptional episode name to load (e.g. episode_001). If not set, uses --max_episodes.z--max_episodesr   z+Load only the first K episodes (default: 1)z--num_samplesr?   zNumber of samples to visualizez
--save_dirzscratch/real_eval_visz Directory to save visualizationsz--metrics_only
store_truez4Only compute and print metrics without visualization)actionr   z--debugz"Drop into pdb before visualizationmpscudar   zUsing device: z
Loading model from z...F)target_sizen_windowfreeze_backbone)map_locationr   
min_height
max_heightu*   ✓ Loaded height range from checkpoint: [z.6fz, z] mz  (r   z.2fzmm to zmm)min_grippermax_gripperu+   ✓ Loaded gripper range from checkpoint: [z.3f]model_state_dictu   ✓ Loaded model from epoch epochz
Loading dataset: )dataset_root
image_sizeepisodemax_episodesu   ✓ Loaded z samplesz
Computing metrics...
z<============================================================Metricsz	Samples: r   zAvg Pixel Error: r   z pxzAvg Height Error: r   z mmzAvg Gripper Abs Error: r   z.4fz
Generating visualizations...T)parentsexist_okz%Real Dataset - Trajectory Predictions)r   r   )+argparseArgumentParseradd_argumentstrDEFAULT_CHECKPOINT_PATHrc   
parse_argsr   r   backendsr   is_availabler   r   
checkpointr   
IMAGE_SIZEr   loadr   rk   
MIN_HEIGHT
MAX_HEIGHTMIN_GRIPPERMAX_GRIPPERload_state_dictrj   r_   r   r   r   r   ra   r   metrics_onlyr   save_dirmkdirdebugpdb	set_tracer   r   )parserargsr   r   r   model_moduler   r   r   r   r   s              r   mainr      sB   $$1bcF
S:Q6  8
(s<d<  >
#tp  r
(sAI  K
c1<  >
38O>  @
(R  T
	,@  BD \\5>>#5#5#B#B#D#D%#(::#:#:#<#<%IF	N6(
#$ 
!$//!2#
67&:bghEDOO&AJ !z!lj&@"'
<(@"A"'
<(@"A:<;R;RSV:WWYZfZqZqruYvvyz{L++D05VL<S<STX<XY\;]]`ab
"}
'B#(M)B#C #(M)B#C ;L<T<TUX;YY[\h\t\tux[yyz{|	*%789HHVE	JJL	(G)<(=
>? 
 1 12
34#&&!\\5T4;L;L	G 
KG~X
./ 
"$e-G	Bvh-	G	VH	Igk*+
,-	g&:;C@
DE	w'<=cB#
FG	#G,C$DS#I
JK	VH .0&td3	::MMOC((	
 r   __main__)rS   N)"__doc__r   ossyspathlibr   matplotlib.pyplotpyplotrd   r   r   r   torch.nn.functionalnn
functionalrn   pathinsertdirname__file__datar   r   r   r   r   r   Tensorndarrayr   r   r   r   __name__ r   r   <module>r     s    \  	 
       277??8, - 0 , 
M 8%,, 82:: 8CLHVS
l zF r   