
    zi                     .   S r SSKrSSKJr  SSKJs  Jr  SSKJr  SSK	J
r
  SSKrSSKr\R                  " S5        SSKJr  SSKJr  SSKJr  SSKJr  SSKrSSKrSSKr\R4                  R7                  S\R4                  R9                  \5      5        SSKJrJ r   SS	K!J"r"J#r#J$r$  SSK!r%S
 r&S r'S r(S r)Sr*Sr+Sr,Sr-\" S5      r.\" S5      r/\.Ra                  SSS9  \/Ra                  SSS9  S r1S r2SS jr3S r4SS jr5S r6S r7\8S:X  a  \7" 5         gg) a/  Train trajectory heatmap predictor on real data.

Model is initialized from a 2D-only checkpoint (just_2d_tracks/latest.pt). Height and gripper
are discretized into 32 bins (uniform over [min,max]) and supervised with cross-entropy over
the trajectory; heatmaps remain cross-entropy as in the 2D model.
    N)
DataLoaderTkAgg)GridSpec)Path)tqdm)RealTrajectoryDatasetN_WINDOW)TrajectoryHeatmapPredictorN_HEIGHT_BINSN_GRIPPER_BINSc                     [         R                  n[         R                  nX-
  X!-
  S-   -  nUR                  SS5      nU[        S-
  -  R                  5       R                  S[        S-
  5      nU$ )a  Discretize continuous height values into bin indices.

Args:
    height_values: (B, N_WINDOW) or (N_WINDOW,) tensor of heights in [MIN_HEIGHT, MAX_HEIGHT]

Returns:
    bin_indices: (B, N_WINDOW) or (N_WINDOW,) tensor of bin indices in [0, N_HEIGHT_BINS-1]
:0yE>              ?   r   )model_module
MIN_HEIGHT
MAX_HEIGHTclampr   long)height_valuesmin_hmax_h
normalizedbin_indicess        exp_dino_tracks/train.pydiscretize_heightr      sq     ##E##E'EMD,@AJ!!#s+J!2399;AA!]UVEVWK    c                     [         R                  n[         R                  nX-
  X!-
  S-   -  nUR                  SS5      nU[        S-
  -  R                  5       R                  S[        S-
  5      nU$ )a  Discretize continuous gripper values into bin indices.

Args:
    gripper_values: (B, N_WINDOW) or (N_WINDOW,) tensor of gripper values in [MIN_GRIPPER, MAX_GRIPPER]

Returns:
    bin_indices: (B, N_WINDOW) or (N_WINDOW,) tensor of bin indices in [0, N_GRIPPER_BINS-1]
r   r   r   r   r   )r   MIN_GRIPPERMAX_GRIPPERr   r   r   )gripper_valuesmin_gmax_gr   r   s        r   discretize_gripperr%   2   sq     $$E$$E (U]T-ABJ!!#s+J!!34::<BB1nWXFXYKr   c                     [         R                  n[         R                  nU R                  SS9n[        R
                  " SS[        U R                  S9nXC   nXRU-
  -  U-   nU$ )zDecode height bin logits back to continuous height values.

Args:
    bin_logits: (B, N_WINDOW, N_HEIGHT_BINS) logits for each bin

Returns:
    height_values: (B, N_WINDOW) continuous height values in [MIN_HEIGHT, MAX_HEIGHT]
dimr   r   device)r   r   r   argmaxtorchlinspacer   r+   )
bin_logitsr   r   r   bin_centersr   r   s          r   decode_height_binsr1   F   sf     ##E##E###+K..c=ARARSK)J%-058Mr   c                     [         R                  n[         R                  nU R                  SS9n[        R
                  " SS[        U R                  S9nXC   nXRU-
  -  U-   nU$ )zDecode gripper bin logits back to continuous gripper values.

Args:
    bin_logits: (B, N_WINDOW, N_GRIPPER_BINS) logits for each bin

Returns:
    gripper_values: (B, N_WINDOW) continuous gripper values in [MIN_GRIPPER, MAX_GRIPPER]
r'   r(   r   r   r*   )r   r    r!   r,   r-   r.   r   r+   )r/   r#   r$   r   r0   r   r"   s          r   decode_gripper_binsr3   \   sf     $$E$$E###+K..c>*BSBSTK)J5=1E9Nr      -C6?  i  z*exp_dino_tracks/checkpoints/just_2d_trackszscratch/exp_training_visT)parentsexist_okc                 L   U R                   u  p#pE/ n[        U5       H_  nU SS2U4   nUSS2U4   n	UR                  US5      n
U	R                  US5      n[        R                  " XSS9nUR                  U5        Ma     [        R                  " U5      R                  5       nU$ )a  Compute cross-entropy loss for heatmap prediction across all timesteps.

Args:
    pred_logits: (B, N_WINDOW, H, W) predicted logits for each timestep
    target_heatmap: (B, N_WINDOW, H, W) one-hot target heatmaps

Returns:
    loss: scalar cross-entropy loss (averaged over timesteps)
Nr'   mean	reduction)	shaperangeviewFcross_entropyappendr-   stackr:   )pred_logitstarget_heatmapBNHWlossestpred_logits_ttarget_heatmap_tpred_logits_flattarget_heatmap_flatloss_tlosss                 r   compute_heatmap_lossrR   ~   s     ""JA! F1X#AqD))!Q$/ )--a4.33Ar: !1RXYf  ;;v##%DKr   c                     [        U5      nU R                  u  p4nU R                  X4-  [        5      nUR                  X4-  5      n[        R
                  " XgSS9S-  nU$ )a^  Compute cross-entropy loss for discretized height prediction across all timesteps.

Args:
    pred_height_logits: (B, N_WINDOW, N_HEIGHT_BINS) predicted bin logits for each timestep
    target_height: (B, N_WINDOW) target heights (continuous values) in [MIN_HEIGHT, MAX_HEIGHT]

Returns:
    loss: scalar cross-entropy loss (averaged over timesteps)
r:   r;   g      D@)r   r=   r?   r   r@   rA   )	pred_height_logitstarget_heighttarget_binsrF   rG   _	pred_flattarget_flatrQ   s	            r   compute_height_lossrZ      sa     $M2K !&&GA!"''}=I""15)K ??9VDSHDKr   c                     [        U5      nU R                  u  pEnU R                  XE-  [        5      nUR                  XE-  5      n[        R
                  " XxSS9S-  n	U	$ )a  Compute cross-entropy loss for discretized gripper prediction across all timesteps.

Args:
    pred_gripper_logits: (B, N_WINDOW, N_GRIPPER_BINS) predicted bin logits for each timestep
    target_gripper: (B, N_WINDOW) target gripper values (continuous) in [MIN_GRIPPER, MAX_GRIPPER]
    pos_weight: unused (kept for API compatibility)

Returns:
    loss: scalar cross-entropy loss (averaged over timesteps)
r:   r;      )r%   r=   r?   r   r@   rA   )
pred_gripper_logitstarget_gripper
pos_weightrV   rF   rG   rW   rX   rY   rQ   s
             r   compute_gripper_lossr`      sa     %^4K "''GA!#((?I""15)K ??9VDQFDKr   c                 r   [         R                  " / SQU R                  S9R                  SSS5      n[         R                  " / SQU R                  S9R                  SSS5      nX-  U-   R	                  5       R                  5       n[        R                  " UR                  SSS5      SS5      nUR	                  5       R                  5       nUR	                  5       R                  5       n	[        R                  " UR                  5       UR                  5      u  p[        R                  " X/5      nXxX4$ )a  Get visualization arrays for a single sample.

Args:
    rgb: (3, H, W) normalized RGB image tensor
    target_heatmap: (H, W) ground truth heatmap tensor
    pred_heatmap: (H, W) predicted heatmap probabilities tensor
    target_2d: (2,) target pixel location tensor

Returns:
    rgb_vis: numpy array of denormalized RGB
    pred_heat_vis: numpy array of predicted heatmap
    target_pt: numpy array of GT 2D location
    pred_pt: numpy array of predicted 2D location (argmax)
g
ףp=
?gv/?gCl?r*      r   gZd;O?gy&1?g?   r   )r-   tensorr+   r?   cpunumpynpclip	transposeunravel_indexr,   r=   array)rgbrE   pred_heatmap	target_2dr:   std
rgb_denormrgb_vis	pred_heat	target_ptpred_ypred_xpred_pts                r   visualize_samplery      s      <<-cjjAFFq!QOD
,,,SZZ
@
E
EaA
NC)d"'')//1Jggj**1a3Q:G  "((*I%%'I %%i&6&6&8)//JNFhh'(Gy11r   c           
      "   U R                  5         SnSnSnSnSn	U GHa  n
U
S   R                  U5      nU
S   R                  U5      nU
S   R                  U5      nU
S   R                  U5      nU
S   R                  U5      nUSS2S4   nUSS2SS4   nUSS2S4   nUSS2SS2S4   nU " XS	UUUS
9u  nnn[        U5      n[        U5      n[	        UU5      n[        UU5      n[        UU5      nUU-   U-   nUR                  5         UR                  5         UR                  5         UUR                  5       -  nUUR                  5       -  nUUR                  5       -  nUUR                  5       -  nU	S-  n	GMd     XY-  Xi-  Xy-  X-  4$ )zTrain for one epoch.

Returns:
    avg_total_loss: average total loss over epoch
    avg_heatmap_loss: average heatmap loss over epoch
    avg_height_loss: average height loss over epoch
    avg_gripper_loss: average gripper loss over epoch
r   rn   heatmap_targettrajectory_3dtrajectory_2dtrajectory_gripperNre   Tgt_target_heatmaptrainingstart_keypoint_2dcurrent_heightcurrent_gripperr   )traintor1   r3   rR   rZ   r`   	zero_gradbackwardstepitem)model
dataloader	optimizerr+   just_heatmap
total_losstotal_heatmap_losstotal_height_losstotal_gripper_loss	n_batchesbatchrn   rE   r|   r}   r~   r   r   r   rU   rD   rT   r]   pred_heightpred_gripperheatmap_lossheight_lossgripper_lossrQ   s                                r   train_epochr     s    
KKMJIEloof%/033F;o.11&9o.11&9"#78;;FC *!Q$/ 'q!Qw/ -QT2 &aAg. @ESuy:K\j8G@I<')<
 ));<*+>? ,KH)*<mL+,?AST k)L8 	 	diik!
l//11[--//l//11Q	_ b !#5#ACTC`bt  cA  A  Ar   c                 Z
   U R                  5         SnSnSnSnSnSnSn	Sn
SnSn[        R                  " 5          [        U5       GH  u  pUS   R	                  U5      nUS   R	                  U5      nUS   R	                  U5      nUS   R	                  U5      nUS   R	                  U5      nUSS2S4   nUSS2SS2S4   nUSS2SS4   nUSS2S4   nU " USS	UUUS
9u  nnn[        U5      n[        U5      nU " UUSUUUS
9u  nnn[        U5      n [        U5      n![        UU5      n"[        UU5      n#[        UU5      n$U"U#-   U$-   n%UU%R                  5       UR                  S   -  -  nUU"R                  5       UR                  S   -  -  nUU#R                  5       UR                  S   -  -  nUU$R                  5       UR                  S   -  -  nUR                  u  n&n'n(n)/ n*[        U'5       H  n+USS2U+4   n,[        R                  " U,R                  U&S5      SS9R                  U&U(U)5      n-U-R                  U&S5      R!                  SS9n.U.U)-  n/U.U)-  n0[        R"                  " U0U//SS9R%                  5       n1USS2U+4   n2[        R&                  " U1U2-
  SS9R)                  5       n3U*R+                  U35        M     U[)        U*5      [,        -  -  n[        R.                  " UU-
  5      R1                  SS9R)                  5       n4UU4R                  5       -  n[        R.                  " U U-
  5      R1                  SS9R)                  5       n5U	U5R                  5       -  n	[        R.                  " UU-
  5      R1                  SS9R)                  5       n6U
U6R                  5       -  n
XR                  S   -  nUS:X  d  GM  Ub  GM  / n7[        [,        5       HO  n+USU+4   n,[        R                  " U,R                  S5      SS9R                  U(U)5      n-U7R+                  U-5        MQ     [        R"                  " U75      n7US   n8US   n9U8R3                  5       S:X  a$  U8R5                  S5      R7                  [,        5      n8U9R3                  5       S:X  a$  U9R5                  S5      R7                  [,        5      n9US   US   U7US   U8US   U9US   S.nGM     SSS5        X;-  n:XK-  n;X[-  n<Xk-  n=X{-  n>X-  n?X-  n@X-  nAU:U;U<U=U>U?U@UAU4	$ ! , (       d  f       N9= f)a  Validate model.

Returns:
    avg_loss: average validation loss
    avg_heatmap_loss: average heatmap loss
    avg_height_loss: average height loss
    avg_gripper_loss: average gripper loss
    avg_pixel_error: average pixel distance error
    avg_height_error: average height error (in meters)
    avg_gripper_error: average gripper error
    sample_data: dict with first sample for visualization
r   Nrn   r{   r}   r|   r~   re   Fr   Tr'   r   r(   rn   rE   ro   r}   r   rU   r   r^   )evalr-   no_grad	enumerater   r1   r3   rR   rZ   r`   r   r=   r>   r@   softmaxr?   r,   rC   floatnormsumrB   r	   absr:   r)   	unsqueezeexpand)Br   r   r+   r   r   r   r   total_pixel_errortotal_height_errortotal_height_error_tftotal_gripper_error	n_samplessample_data	batch_idxr   rn   rE   r}   r|   r~   r   rU   r   r   rD   rT   r]   r   r   pred_logits_tfpred_height_logits_tfpred_gripper_logits_tfpred_height_tfpred_gripper_tfr   r   r   rQ   rF   rG   rH   rI   pixel_errorsrK   rL   pred_probs_tpred_flat_idxrv   rw   	pred_2d_ttarget_2d_tpixel_error_theight_errorheight_error_tfgripper_errorpred_heatmapspred_h_0pred_g_0avg_lossavg_heatmap_lossavg_height_lossavg_gripper_lossavg_pixel_erroravg_height_erroravg_height_error_tfavg_gripper_errorsB                                                                     r   validater   U  s    
JJLJIK	 )* 5I,//&)C"#3477?N!/255f=M!/255f=M!&';!<!?!?!G !.ad 3 *!Q'2M +1a73N 1A6O DI`dot>O`n<KDM@K+-@
 --?@K./BCL MRRUiw  CG>O`n<KMMIN13I
 00EFN12HIO 0^LL-.@-PK/0CEWXL+-<D$))+		!44J,"3"3"5		!"DD!1!1!3ciil!BB,"3"3"5		!"DD %**JAq!QL1X +AqD 1 yy););Ar)BJOOPQSTVWX !- 1 1!R 8 ? ?A ? F&!+&*!KK(8a@FFH	 ,AqD1 %

9{+B J N N P##M2  \!2X!== !99[=%@AFF1FMQQSL,"3"3"55 $ii(FGLLQRLSWWYO!_%9%9%;;! "IIl5G&GHMMRSMTXXZM=#5#5#771%I A~+"5 "xA$/1$5M#$99]-?-?-C#K#P#PQRTU#VL!((6 ) !&M : 'q>'?<<>Q&'11!4;;HEH<<>Q&'11!4;;HEH q6&4Q&7$1%21%5#+%21%5$,&8&;	Q !6 
h %H)5'3O)5'3O)5/;+7%8H/[k  nA  CT  Va  a  a{ 
s   NTTDT
T*c                  2   [         R                  " SS9n U R                  SS[        SS9  U R                  S[        SS	S
9  U R                  S[
        [        SS
9  U R                  S[        [        SS
9  U R                  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5        [#        UR$                  [&        S9n[)        U5      n[        XAR*                  -  5      nXE-
  n[        R,                  R.                  R1                  X6U/[        R2                  " 5       R5                  S5      S9u  px[!        S[)        U5       S35        [!        S[)        U5       S35        [7        UUR8                  SS S!9n	[7        UUR8                  S"S S!9n
[!        S[)        U5       S35        [!        S[)        U5       S35        [!        S#5        [;        [&        [<        S"S$9nUR?                  U5      n[A        S% URC                  5        5       5      n[A        S& URC                  5        5       5      n[!        S'US( S)US( S*S+U-  U-  S, S-35        [D        RF                  " [I        S. URC                  5       5      URJ                  S/S09nS nS nS nURL                  nURL                  (       a  [N        RP                  RS                  U5      (       d  URU                  S1S25      n[)        U5      S3:X  ad  US2   RW                  5       S4:X  a  S5OS6nUS    U-   n[N        RP                  RS                  U5      (       a  Un[!        S7URL                   S8U 35        URL                  (       Ga]  [N        RP                  RS                  U5      (       Ga8  [!        S9U S:35        [        RX                  " UUS;9nUS<   nUR[                  5       n0 n/ nUR]                  5        Hb  u  nnUU;   d  M  UR^                  UU   R^                  :X  a  UUU'   M2  URa                  U S=UR^                   S>UU   R^                   35        Md     [c        URe                  5       5      [c        URe                  5       5      -
  n[c        URe                  5       5      [c        URe                  5       5      -
  nU(       a  [!        S?[g        U5       35        U(       a  [!        S@[g        U5       35        U(       a"  [!        SA5        U H  n[!        SBU 35        M     URi                  U5        URk                  US"SC9   URk                  USD   5        URo                  SGS 5      S2-   nSHU;   a*  SIU;   a$  USH   USI   4n[!        SJUS    SK SLUS2   SK SM35        SNU;   a*  SOU;   a$  USN   USO   4n[!        SPUS    SK SLUS2   SK SQ35        [!        SRU 35        O4URL                  (       a#  [!        SSURL                   35        [!        ST5        S S K8n!Ubu  Uu  U!l9        U!l:        [!        SUU!Rr                  SK SLU!Rt                  SK SM35        [w        U!Rr                  U!Rt                  -
  5      SV:  a  [!        SW5        [!        SX5        GO}[!        SY5        / n"Xx4 H^  n#[y        [)        U#5      5       HC  n$U#U$   n%U%SZ   R{                  5       n&U&S S 2S34   n'U"R}                  U'R                  5       5        ME     M`     [        R                  " U"5      n"[	        U"R                  5       5      n([	        U"R                  5       5      n)U(U!l9        U)U!l:        [!        S[U!Rr                  SK SLU!Rt                  SK SM35        [!        S\U!Rr                  S]-  S, S^U!Rt                  S]-  S, S_35        [!        S`[)        U"5       Sa35        [w        U!Rr                  U!Rt                  -
  5      SV:  a  [!        SW5        [!        Sb5        [!        Sc5        Ub8  Uu  U!lD        U!lE        [!        SdU!R                  SK SLU!R                  SK SQ35        OASeU!lD        SfU!lE        [!        SgU!R                  SK SLU!R                  SK SQ35        [!        Sh5        [!        Si5        [        R                  " 5         [        R                  " Sj[<        -  Sk4Sl9n*[        Sm[<        U*SnSo/ SpQSq9n+U*R                  U+S S S 24   5      n,U*R                  U+S2S S 24   5      n-U*R                  U+S3S S 24   5      n./ n/[        SrSs/5       H  u  n0n1/ n2[y        [<        5       H[  n3U*R                  U+U0St-  St-   U34   5      n4U4R                  Su5        U0S :X  a  U4R                  SvU3 3SwSxSy9  U2Ra                  U45        M]     U/Ra                  U25        M     / n5[        SrSs/5       H\  u  n0n1/ n2[y        [<        5       H0  n3U*R                  U+U0St-  Sj-   U34   5      n4U2Ra                  U45        M2     U5Ra                  U25        M^     / n6[        SrSs/5       H\  u  n0n1/ n2[y        [<        5       H0  n3U*R                  U+U0St-  Sz-   U34   5      n4U2Ra                  U45        M2     U6Ra                  U25        M^     [        R                  " S"S{9  U*R                  R                  5         [        R                  " S|5        [!        S}UR                   S~35        [	        S5      n7/ n8/ n9/ n:/ n;/ n</ n=[y        XR                  5       GH  n>[!        SS 35        [!        SU> SUR                   35        [!        S 5        [        XXU>S3:  S9u  n?n@nAnBU8Ra                  U@5        U9Ra                  UA5        U:Ra                  UB5        [!        SU?S SU@S SUASK SUBSK S3	5        [        XU5      u	  nCnDnEnFnGnHnInJnKU;Ra                  UD5        U<Ra                  UE5        U=Ra                  UF5        [!        SUCS SUGS, SUHS]-  S SUJS 35        UR                  5         [        R                  " 5          [        [        U	5      5      nLULS   S S2 R?                  U5      nMULS   S    nNULS   S    nOULSZ   S    nPULS   S    nQUOS    nRUPS   nSUQS    nTU" UMS S"URUSUTS9u  nUnVnW[        UVS S2 5      S    nX[        UWS S2 5      S    nY/ nZ[y        [<        5       HN  n3WUS U34   n[[        R                  " U[R                  S5      S S9R                  U[5      n\WZRa                  U\5        MP     [        R                  " WZ5      nZWMS    WNUZWOWXS    WPS S 2S34   WYS    WQS.n]S S S 5        U,R                  5         [        R                  " [)        U85      5      n^U,R                  U^U8SSrSS3S9  U,R                  U^U;SSsSS3S9  U,R                  S5        U,R                  S5        U,R                  SW@S SWDS 35        U,R                  SS9  U,R                  SnS9  U-R                  5         U-R                  U^U9SSrSS3S9  U-R                  U^U<SSsSS3S9  U-R                  S5        U-R                  S5        U-R                  SWASK SWESK 35        U-R                  SS9  U-R                  SnS9  U.R                  5         [        R                  " [)        U:5      5      n^U.R                  U^U:SSrSS3S9  U.R                  U^U=SSsSS3S9  U.R                  S5        U.R                  S5        U.R                  SWBSK SWFSK 35        U.R                  SS9  U.R                  SnS9  [        W]Sr4WKSs4/5       GH  u  n0u  n%n1U%c  M  [        R                  " / SQU%S   R                  S9R                  StS2S25      n_[        R                  " / SQU%S   R                  S9R                  StS2S25      n`U%S   U`-  U_-   R                  5       R{                  5       na[        R                  " UaR                  S2S3S 5      S S25      nb[y        [<        5       GH  n3U/U0   U3   n4U4R                  5         U%S   U3   R                  5       R{                  5       ncU%S   U3   R                  5       R{                  5       ndU%S   U3   R                  5       R{                  5       ne[        R                  " UdR                  5       UdR^                  5      u  nfng[        R                  " UgUf/5      nhU4R                  Wb5        U4R                  UdSSS9  U4R                  UeS    UeS2   SS+SSS3SSxS9	  U4R                  UhS    UhS2   SS+SStSSxS9  [        R                  R                  UeUh-
  5      niSvU3 3SUiS S3/njSU%;   Ga  SU%;   Ga  U%S   nkUkR                  5       S :X  a  WkR                  5       R                  5       nlOSWkR                  5       S :  a!  WkU3   R                  5       R                  5       OWkR                  5       R                  5       nlU%S   nmUmR                  5       S :  a!  WmU3   R                  5       R                  5       OWmR                  5       R                  5       nn[w        WlUn-
  5      S]-  noWjRa                  SUlS]-  S S35        SU%;   a  SU%;   a  U%S   npUpR                  5       S :  a!  WpU3   R                  5       R                  5       OWpR                  5       R                  5       nqU%S   nrUrR                  5       S :  a!  WrU3   R                  5       R                  5       OWrR                  5       R                  5       nsWjRa                  SWqS, SUsS, S35        U4R                  SR                  Wj5      SS9  U4R                  Su5        U3S :X  a  U4R                  SSS9  SU%;   Ga  SU%;   Ga  U5U0   U3   ntUtR                  5         U%S   nkUkR                  5       S :  a!  WkU3   R                  5       R                  5       OWkR                  5       R                  5       S]-  nlU%S   nmUmR                  5       S :  a!  WmU3   R                  5       R                  5       OWmR                  5       R                  5       S]-  nnS S2/nuUnWl/nvSS/nwWtR                  UuUvUwSS9nxUtR                  Uu5        UtR                  SS/SS9  UtR                  SSS9  UtR                  SUlS SUnS S_3SS9  UtR                  SnSS9  S S K8n!UtR                  U!Rr                  S]-  Sx-
  U!Rt                  S]-  Sx-   /5        SU%;   d  GM(  SU%;   d  GM1  U6U0   U3   nyUyR                  5         U%S   npUpR                  5       S :  a!  WpU3   R                  5       R                  5       OWpR                  5       R                  5       nqU%S   nrUrR                  5       S :  a!  WrU3   R                  5       R                  5       OWrR                  5       R                  5       nsS S2/nuUsWq/nzSS/nwWyR                  UuUzUwSS9nxUyR                  Uu5        UyR                  SS/SS9  UyR                  SSS9  UyR                  SUqS, SUsS, S3SS9  UyR                  SnSS9  S S K8n!UyR                  U!R                  S|-
  U!R                  S|-   /5        GM     GM     U*R                  R                  5         U*R                  R                  5         [        R                  " S5        S S K8n!U>UR[                  5       UR[                  5       U?WCU!Rr                  U!Rt                  U!R                  U!R                  S.	n[        R                  " U[        S-  5        UCU7:  d  GM  WCn7[        R                  " U[        S-  5        [!        SUCS S35        GM     [        R                  " 5         [        R                  " 5         [!        S5        [!        S5        [!        SU7S 35        [!        S[         35        g ! [l         a$  n [!        SEU  35        [!        SF5         S n A GN#S n A ff = f! , (       d  f       GN$= f)Nz"Train trajectory heatmap predictor)descriptionz--dataset_rootzscratch/parsed_school_redo1zRoot directory with episodes)defaulttypehelpz--val_splitg?z*Fraction of episodes to use for validation)r   r   r   z--batch_sizezBatch size for trainingz--lrzLearning ratez--epochszNumber of epochsz--checkpointz4exp_dino_tracks/checkpoints/just_2d_tracks/latest.ptz:Path to checkpoint to resume from (default: 2D-only model)mpscudarg   zUsing device: z
Loading dataset...)dataset_root
image_size*   )	generatoru   ✓ Train: z samplesu	   ✓ Val: Tr   )
batch_sizeshufflenum_workersFz
Initializing model...)target_sizen_windowfreeze_backbonec              3   f   #    U  H'  oR                   (       d  M  UR                  5       v   M)     g 7fN)requires_gradnumel.0ps     r   	<genexpr>main.<locals>.<genexpr>0  s      O);Aiaggii);s   11c              3   @   #    U  H  oR                  5       v   M     g 7fr   )r   r   s     r   r   r   1  s     8%7''))%7s   zTrainable parameters: ,z / z (d   z.2fz%)c                     U R                   $ r   )r   )r   s    r   <lambda>main.<locals>.<lambda>6  s    r   r5   )lrweight_decay.r   re   ptz.pthz.ptzCheckpoint not found at z, using z
Loading checkpoint: zQ (initializing from 2D-only model; height/gripper 32-bin heads random if missing))map_locationmodel_state_dictz: checkpoint z
 vs model uA   ⚠ Missing keys in checkpoint (will use random initialization): u5   ⚠ Unexpected keys in checkpoint (will be ignored): u@   ⚠ Shape mismatches (will use random initialization for these):z    )strictoptimizer_state_dictuD   ⚠ Could not load optimizer state (architecture may have changed): z%  Starting with fresh optimizer stateepoch
min_height
max_heightu'   ✓ Found height range in checkpoint: [z.6fz, z] mmin_grippermax_gripperu(   ✓ Found gripper range in checkpoint: []u   ✓ Resumed from epoch u   
⚠ Checkpoint not found: zU  Initializing model from scratch (height/gripper heads will be randomly initialized)u)   ✓ Using height range from checkpoint: [gư>uQ     ⚠ WARNING: MIN_HEIGHT == MAX_HEIGHT! All height predictions will be constant!zT  This will cause all bins to decode to the same value. Check dataset or checkpoint.z,
Computing height statistics from dataset...r|   u)   ✓ Height range computed from dataset: [z  (r6   zmm to zmm)z  (computed from z% waypoint heights across all samples)zF  This will cause all bins to decode to the same value. Check dataset.uW   ✓ These values will be used as constants throughout training and saved in checkpointsu*   ✓ Using gripper range from checkpoint: [gɿg?u%   ✓ Using hard-coded gripper range: [zB  (Values >4.0 are mapped to -0.2, values >0.8 are clamped to 0.8)z!
Setting up live visualization...r\      )figsize	   g333333?g?)	r   r   r   re   r   re   r   re   r   )figurehspacewspaceheight_ratiosTrainValrc   offzt=bold
   )
fontweightfontsize   )blockg?z
Starting training for z
 epochs...inf
z<============================================================zEpoch /)r   zTrain Loss: z.4fz (Heatmap: z
, Height: z, Gripper: )zVal - Loss: z, Pixel Error: zpx, Height Error: z.3fzmm, Gripper Error: rn   r{   r}   r~   )r   re   r   r'   r(   r   zo-blue)labelcolor	linewidthzs-greenEpochzHeatmap Loss (CE)zHeatmap Loss | Train: z | Val: zupper right)loc)alphazHeight Loss (MSE)zHeight Loss | Train: zGripper Loss (MSE)zGripper Loss | Train: rb   r*   rd   rE   ro   g333333?hot)r  cmapwhiteoblackGT)csmarker
edgecolors
linewidthsr  zorderlimexPred)r  r  r  r  r  r  zPx:z.1fpxr   rU   zH:mmr   r^   zG:z(GT:r4   )r     )r  r  redgffffff?)r  r     zHeight (mm)zH: zmm (GT: y)r  axisGripperzG: z (GT: g{Gz?)	r   r   r   
train_lossval_lossr   r   r   r   z
latest.pthzbest.pthu   ✓ Saved best model (val_loss=z=
============================================================u   ✓ Training complete!zBest val loss: zCheckpoints saved to: )|argparseArgumentParseradd_argumentstrr   int
BATCH_SIZELEARNING_RATE
NUM_EPOCHS
parse_argsr-   r+   backendsr   is_availabler   printr   r   
IMAGE_SIZElen	val_splitutilsdatarandom_split	Generatormanual_seedr   r   r
   r	   r   r   
parametersoptimAdamWfilterr   
checkpointospathexistsrsplitlowerload
state_dictitemsr=   rB   setkeyssortedupdateload_state_dict	Exceptiongetr   r   r   r   r>   rh   extendtolistri   rm   minmaxr    r!   pltionr   r   add_subplotr   r'  	set_titleshowcanvasdrawpauseepochsr   r   r   r   nextiterr1   r3   r@   r   r?   view_asrC   cleararangeplot
set_xlabel
set_ylabellegendgridrf   rg   rj   rk   rl   r,   imshowscatterlinalgr   r)   r   joinbar
set_xticksset_xticklabelsset_ylimflush_eventssaveCHECKPOINT_DIRioff){parserargsr+   full_datasetdataset_sizeval_size
train_sizetrain_datasetval_datasettrain_loader
val_loaderr   n_trainablen_totalr   start_epochcheckpoint_height_valuescheckpoint_gripper_valuescheckpoint_pathalt	other_extalt_pathrC  model_state
model_dictfiltered_stateshape_mismatcheskvmissing_keysunexpected_keysmsger   all_heightsdatasetisampler|   waypoint_heightsr   r   figgsax_loss_heatmapax_loss_heightax_loss_gripperaxes_visrow_idx
split_namerow_axesrK   axaxes_heightaxes_gripperbest_val_losstrain_heatmap_lossestrain_height_lossestrain_gripper_lossesval_heatmap_lossesval_height_lossesval_gripper_lossesr   r)  train_heatmap_losstrain_height_losstrain_gripper_lossr*  val_heatmap_lossval_height_lossval_gripper_loss	val_errorval_height_errorval_height_error_tfval_gripper_error
sample_valtrain_sample_batch	train_rgbtrain_target_heatmap_alltrain_trajectory_2dtrain_trajectory_3dtrain_trajectory_grippertrain_start_keypoint_2dtrain_current_heighttrain_current_grippertrain_pred_logitstrain_pred_height_logitstrain_pred_gripper_logitstrain_pred_heighttrain_pred_grippertrain_pred_heatmapsrL   r   sample_trainepochs_ranger:   rq   rr   rs   rM   pred_heatmap_tr   rv   rw   r   	pixel_errtitle_partspred_h_tensorpred_htarget_h_tensortarget_h
height_errpred_g_tensorpred_gtarget_g_tensortarget_g	ax_heightx_posheightscolorsbars
ax_grippergripper_valss{                                                                                                                              r   mainr    s   $$1UVF
(2OVY:  <
E4H  J
S*5  7
UM+  -

j.  0
S:pX  ZD \\5>>#5#5#B#B#D#D%#(::#:#:#<#<%IF	N6(
#$ 

 !(&&L |$L<..01H(J!&!1!1!>!>8,//#//3 "? "M
 
KM*+8
45	Ic+&'x
01 ??	L ??	J 
KM*+8
45	Ic+&'x
01 

#$&:bghEHHVE O)9)9);OOK8U%5%5%788G	";q/WQKr#k/RYBYZ]A^^`
ab (%*:*:*<=77I K# $ooOww~~o..!((a0C3x1}&)!flln&<F%	q6I-77>>(++&.O4T__4EXoM^_`277>>/::&&7  8I  J  	KZZfE
 !!34%%'
 %%'DAqJ77jm111()N1%$++qcqwwizR\]^R_ReReQf,gh ( :??,-K4D4D4F0GGk..01C
8I4JJUV\]iVjUklmI&Q`JaIbcdTV'SEl# ( 	.)j7	;%%j1G&HI
 !nnWa014 :%,**D(2<(@*\BZ'[$;<TUV<WX[;\\^_wxy_z{~^  @C  D  EJ&=J+F)3M)BJ}D](^%<=VWX=YZ]<^^`az{|a}  B  aC  CD  E  F'}56	,T__,=>?ef !+;S8!89,:Q:QRU9VVXYeYpYpqtXuuxyz|&&)@)@@ADHeghj 	=>%3G3w<(  & 7 = = ?#0A#6 ""#3#:#:#<=	 ) 4 hh{+;??,-
;??,-
",",9,:Q:QRU9VVXYeYpYpqtXuuxyzL++D05VL<S<STX<XY\;]]`ab!#k"2!33XYZ|&&)@)@@ADHegZ\gi !,=V: ,"::<;S;STW:XXZ[g[s[stwZxxyz{ $( #& 5l6N6Ns5SSUVbVnVnorUsstuvRT 

./GGI **aj"-
.C	!Xc#c ;
=B oobAh/O__R1X.NoobAh/O H('5)9:xAGaK!OQ$6!78BGGEN!|r!X&2FOOB ! 	!  ; K('5)9:xAGaK!OQ$6!78BOOB ! 	8$  ; L('5)9:xAGaK!OQ$6!78BOOB ! 	H%  ; HH5JJOOIIcN 
$T[[M
<=%LM {KK06(mugQt{{m,- Q\qQ
M
&(9;M 	##$67""#45##$67Z,K8J37OzZkloYpp{  }O  PS  |T  TU  V  	W [cv[
	X"O5E#%8:KZ 	!!"23  1!!"23XcN/)CHZ[klp[pqtZu  vI  J[  \_  I`  a  	b 	

]]_!%d<&8!9*51!A699&AI'9:J'KA'N$"4_"Ea"H"4_"Ea"H'9:N'OPQ'R$&9!&<##6t#< $<Q$?!UZ[dx|  HMJaG[H]V_R79R !33KAa3P QRS T!45NqQR5S!TUV!W #%8_ 1!Q$ 7 yy););B)?QGOOP]^#**<8 % #(++.A"B !|": 3!403!4QT!: 21 5":	L9 R 	yy%9!:;\+?W\bnop\+=t5X_klm""7+""#67!!$:;Mc:RRZ[kloZp"qr=13' 	L*=t7Z`lmnL*;TV]ijk!!'*!!"56  #89J38OxXghkWl!mn-0#& 	yy%9!:;\+?W\bnop\+=t5X_klm""7+""#78!!$:;Mc:RRZ[kloZp"qr=13' .77#8
 .)G)fj ~ << 5fUm>R>RSXXYZ\]_`aD,,4VE]=Q=QRWWXY[\^_`C -#-499;AACJggj221a;QBG 8_g&q)
 $**:#;A#>#B#B#D#J#J#L !'!7!:!>!>!@!F!F!H$_5a8<<>DDF "$!1!1.2G2G2I>K_K_!`HHff%56	 		'"		.%	@ 

;q>;q>W!$Q $R  1 

9Q<13!$ &r  3
 IINN;+BC	!#A3x3yoR)@A F*&/H$*=$9M$((*a/!.!2!2!4!9!9!;BOBSBSBUXYBYq!1!5!5!7!<!<!>_l_p_p_r_w_w_y&,_&=OBQBUBUBWZ[B[q1557<<>apatatava{a{a}H!$Vh%6!7$!>J&&F4K+<B'?@!V+0@F0J$*>$:M>K>O>O>QTU>U]1-11388:[h[l[l[n[s[s[uF&,-=&>OBQBUBUBWZ[B[q1557<<>apatatava{a{a}H&&F3<tHS>'KLTYY{3a@6II-!I< !F*&/H +G 4Q 7IOO% %+=$9M?L?P?P?RUV?VmA.22499;\i\m\m\o\t\t\vz~~F&,_&=OCRCVCVCX[\C\ 2 6 6 8 = = ?bqbububwb|b|b~  CG   GH FE'0G%u-F$==vS=QD((/--tVnq-I(((C''#fS\(3s(S^_'`NN3N7 1&&(?(?$(F(K\MdMdgkMknpMp'qr "V+0@F0J!-g!6q!9J$$& %+>$:M>K>O>O>QTU>U]1-11388:[h[l[l[n[s[s[uF&,-=&>OBQBUBUBWZ[B[q1557<<>apatatava{a{a}H FE$,f#5L%u-F%>>%VSV>WD))%0..f~.J)))a)@((3vcl&#a)P[\(]OO#CO8 1'')A)AC)GIaIadgIg(hiO %.p 	



!		$
 	% % 0 0 2$-$8$8$:$ &11&11'33'33


 	

:~<= m#$MJJz>J#>?3HS>CDo 1t HHJHHJ	-	
"#	OM#.
/0	">"2
34}  	;XYZX[\]9::	;r _s/   9Ac ;D Ac8c
Ac5cAc0c0Ac5c8
Ad	__main__r   )F)9__doc__r-   torch.nnnntorch.nn.functional
functionalr@   torch.optimr@  torch.utils.datar   rh   ri   
matplotlibusematplotlib.pyplotpyplotrW  matplotlib.gridspecr   pathlibr   r   r+  sysrD  rE  insertdirname__file__r;  r   r	   r   r
   r   r   r   r   r%   r1   r3   r0  r1  r2  r7  rt  VIS_DIRmkdirrR   rZ   r`   ry   r   r   r  __name__ r   r   <module>r     s        '   
w   (    
 	 277??8, - 0 K K ((,, 


 BC
)
*   TD  1 dT *@.x2@AAHVar@5F zF r   