+
    Uj"R                       R t ^ RIHt ^ RIt^ RIHt ^ RIHt ^ RIt^ RI	t
^ RIt^ RIHt ^RIHtHtHtHtHtHtHtHtHt R]! ]P2                  ! 4       R,          4      3tR	tR
 R ltR R ltR R ltR R lt ! R R]4      t R# )u  AR-view (feetech UMI) scene+wrist dataset for smithbot_v4 training.

Mirrors the interface of ``yam_local_train_mirror/lib/dataset.py:YamScene``
so ``train.py`` needs only the import swap. Reads ``record_dataset.py``
output (a flat ``state/*.npz`` + ``scene/*.jpg`` + ``wrist/*.jpg`` tree)
plus ``episodes.json`` for demonstration segmentation.

Schema references:
- ``/data/cameron/claude_feetech_controller/ar_view/DATASET_FORMAT.md``
- ``/data/cameron/claude_feetech_controller/ar_view/TRAINING_DATA_README.md``

Per-sample output matches YamScene exactly (train.py + loss + viz eat
the same tensors) — see YamScene docstring for target semantics.
)annotationsN)Path)Optional)Dataset)	IMAGENET_MEANIMAGENET_STDROT_KMEANS_K_imagenet_norm_chw
_R_to_quat_quat_canonical_kmeans_quats_nearest_centroid_idx_project_world_to_uvzJ/data/cameron/claude_feetech_controller/ar_view/wrist_umi_calibration.jsonz"ar_view/wrist_umi_calibration.jsong?c                   V ^8  d   QhRR/# )   returnOptional[np.ndarray] )formats   "5/data/cameron/repos/smithbot_v4/lib/dataset_arview.py__annotate__r   .   s      %9     c                 $   \          Fs  p  \        P                  ! \        V 4      P	                  4       4      p\        P                  ! TR,          \        P                  R7      pTP                  R8X  g   Kq  Tu # 	  R#   \
         d     K  i ; i)u   Load the frozen 4x4 T_wrist_cam→umi (see TRAINING_DATA_README.md §wrist calibration).
Returns None if the calibration file isn't available — callers should
then fall back to per-frame wrist_cam_pose from state npz.T_wrist_cam_umidtypeN)   r   )
_WRIST_UMI_CAL_PATHSjsonloadsr   	read_text	Exceptionnpasarrayfloat64shape)pdTs      r   _load_wrist_umi_calibrationr)   .   st     "	

47,,./A JJq*+2::>77fH "   		s   -B  BBc                    V ^8  d   QhRRRR/# r   rootr   	frame_idxintr   )r   s   "r   r   r   =   s      4 C r   c                    V R ,          VR R2,          p\         P                  ! \        V4      4      pVf   \        RV 24      hV# )scene06d.jpgzscene frame missing: )cv2imreadstrRuntimeErrorr,   r-   r&   bgrs   &&  r   _read_arview_scene_framer9   =   sH    wIc?$//A
**SV
C
{21#677Jr   c                    V ^8  d   QhRRRR/# r+   r   )r   s   "r   r   r   E   s      4 C r   c                l    V R ,          VR R2,          p\         P                  ! \        V4      4      pV# )wristr1   r2   )r3   r4   r5   r7   s   &&  r   _read_arview_wrist_framer=   E   s0    wIc?$//A
**SV
CJr   c               $    V ^8  d   QhRRRRRR/# )r   posesz
np.ndarrayvisibler   r   )r   s   "r   r   r   K   s!      Z *  r   c                   \         P                  ! V\        R7      pVP                  4       '       g'   V ^ ,          P	                  \         P
                  4      # \         P                  ! W,          ^ R7      P	                  \         P
                  4      pVR,          p\         P                  P                  V4      w  rVpWW,          p\         P                  P                  V4      ^ 8  d   VR;;,          R,          uu&   WW,          p\         P                  ! ^\         P
                  R7      p	WR&   VR,          V	R&   V	# )a	  Given (N, 4, 4) camera poses and (N,) visibility bools, return a
single fixed 4x4. Uses per-element median across the visible subset
(crude but adequate for a physically-fixed camera whose per-frame PnP
solve is noisy). Falls back to poses[0] if nothing is visible.r   axisN   NrE   )NNNrG   rE   rF   )r"   r#   boolanyastyper$   medianlinalgsvddeteye)
r?   r@   maskmedRU_VtR_orthoouts
   &&        r   _median_pose_over_visiblerZ   K   s    
 ::gT*D88::Qxrzz**
))EKa
(
/
/


;CFAyy}}QHA"fG	yy}}W!	%B&
&&"**
%CKUCJJr   c                  t    ] tR t^atRt^ ^^R^ ^ ]RRRR^ R3R R lltR R ltR	 R
 ltR R lt	R R lt
RtR# )ArViewSceneup  AR-view (feetech UMI) training samples. World frame = calib board.

Reads:
  ``root/state/NNNNNN.npz`` — per-frame poses + gripper (schema in DATASET_FORMAT.md)
  ``root/scene/NNNNNN.jpg`` + ``root/wrist/NNNNNN.jpg`` — 500-px images
  ``root/meta.json`` — intrinsics + FPS + camera conventions
  ``root/scene_wrist/episodes.json`` OR ``root/episodes.json`` — segmentation

Uses ``umi_pose`` as the end-effector world pose (no FK, unlike YAM).
Wrist camera pose per frame uses ``wrist_cam_pose`` when
``wrist_visible`` is true, else falls back to
``umi_pose(f) @ inv(T_wrist_cam→umi)`` from the frozen calibration.
i  Ng{Gz?c               P    V ^8  d   QhRRRRRRRRRRRRR	RR
RRRRRRRRRRRRR/# )r   r,   r   n_windowr.   frame_stride	pred_sizeimg_sizen_zn_gripper_binsn_rot_clustersz_rangezOptional[tuple[float, float]]
grip_rangerot_centroidsr   z_padfloatpast_nepisodes_jsonzOptional[Path]r   )r   s   "r   r   ArViewScene.__annotate__p   s     d8 d8d8 d8 	d8
 d8 d8 d8 d8 d8 /d8 2d8 ,d8 d8 d8 &d8r   c                	&   \        V4      V n        W n        W0n        W@n        WPn        W`n        Wpn        Wn        \        V4      V n
        \        P                  ! V P                  R ,          P                  4       4      pVP                  RR4      P                  R4      '       g   Q R4       h\        VR,          4      pVR,          pVP                  R/ 4      p\        VR,          4      p\!        V P                  ^ 4      pVP"                  R	,          w  ppVV8X  g   Q R
V RV 24       h\$        P&                  ! VR,          \$        P(                  R7      pV'       dE   VP                  R4      '       d.   \$        P&                  ! VR,          \$        P(                  R7      MRpR pV! VVVV4      pVe   V! VVVV4      MRpRpVe   V.M. pVV P                  R,          R,          V P                  R,          .,          pV F5  pVf   K	  \        V4      P+                  4       '       g   K*  \        V4      p M	  Vf   \-        RV P                   RV R24      h\        P                  ! VP                  4       4      pVP                  R. 4      p V '       g   \-        RV 24      h. p!\/        V4       EFI  p"\$        P0                  ! V P                  R,          V"R R2,          4      p#T!P3                  R\$        P&                  ! V#R,          \$        P(                  R7      R\$        P&                  ! V#R,          \$        P(                  R7      R\$        P&                  ! V#R,          \$        P(                  R7      R\5        V#R,          4      R\5        V#R,          4      R\5        V#R,          4      R \$        P6                  ! V#R ,          4      '       g   \9        V#R ,          4      M\$        P:                  /4       EKL  	  \=        4       p$V$V n        V$f   \A        R!4       . V n!        V  EFK  p%\        V%R",          4      p&\        V%R#,          4      p'V'V&,
          ^,           p(V(^8  d   KA  \$        PD                  ! \/        V&V'^,           4       U"u. uF  p"V!V",          R,          NK  	  up"^ R$7      p)\$        PF                  ! \/        V&V'^,           4       U"u. uF  p"V!V",          R,          NK  	  up"\4        R7      p*\I        V)V*4      p+\$        PJ                  PM                  V+4      p,\$        PN                  ! V(^3\$        P(                  R7      p-\$        PN                  ! V(^3\$        PP                  R7      p.\$        PN                  ! V(\$        PP                  R7      p/\$        PN                  ! V(^^3\$        P(                  R7      p0\$        PR                  ! V(\4        R7      p1\U        \/        V&V'^,           4      4       F  w  p2p"V!V",          R,          p3V3V0V2&   V3R[,          V-V2&   \W        \Y        V3R\,          4      4      P[                  \$        PP                  4      V.V2&   V!V",          R ,          p4\$        P6                  ! V44      '       d   R%V1V2&   R&V/V2&   K  \9        V44      V/V2&   K  	  V PB                  P3                  R'V%P                  R(R)V& R*V' 24      R+T&R,T(R-VP[                  \$        PP                  4      R.Ve    VP[                  \$        PP                  4      MRR/V,P[                  \$        PP                  4      R0\$        P\                  ! ^\$        PP                  R7      R1V-R2V.R3V/R4V1RV0/4       EKN  	  V PB                  '       g0   \-        R5V P                   R6V R7V R8W#,          ^,            R2	4      hV	f   \$        P^                  ! V PB                   U5u. uF  p5V5R1,          R],          NK  	  up54      p6\9        V6Pa                  4       4      V,
          p7\9        V6Pc                  4       4      V,           p8\A        R9V7R: R;V8R: R<V6Pa                  4       R= R;V6Pc                  4       R= R>V R?24       MV	w  p7p8\A        R@V7R: R;V8R: RA24       V7V8uV n2        V n3        V
f   \$        P^                  ! V PB                   U5u. uF  p5V5R3,          V5R4,          ,          NK  	  up54      p9\9        V9Pa                  4       4      \h        ,
          p:\9        V9Pc                  4       4      \h        ,           p;\A        RBV:R: R;V;R: RCV9Pa                  4       R= R;V9Pc                  4       R= RD2	4       MV
w  p:p;V:V;uV n5        V n6        VeK   VP"                  V^38X  g   Q hVP[                  \$        PP                  4      V n7        \A        REV RF24       EM\$        P^                  ! V PB                   U5u. uF  p5V5R2,          NK  	  up5^ R$7      p<\A        RGV RH\q        V<4       RI24       \a        V\q        V<4      4      p=V=V8  d   \A        RJV RKV= RL\q        V<4       RM24       \s        V<V=4      V n7        V=V8  dx   \$        Pt                  ! V Pn                  RN,          VV=,
          ^34      P[                  \$        PP                  4      p>\$        P^                  ! V Pn                  V>.^ R$7      V n7        V PB                   EF  p5\w        V5R2,          V Pn                  4      V5RO&   V5R3,          V Pj                  ,
          V Pl                  V Pj                  ,
          ,          p4\$        Px                  ! V4V,          ^ V^,
          4      P[                  \$        Pz                  4      V5RP&   V5R1,          R],          V Pd                  ,
          V Pf                  V Pd                  ,
          ,          p?\$        Px                  ! V?V,          ^ V^,
          4      P[                  \$        Pz                  4      V5RQ&   \$        P^                  ! V5R1,          \$        PR                  ! V5R,,          ^34      .^R$7      p@V5R/,          V@P|                  ,          P|                  R^,          pAV5R-,          VAP|                  ,          P|                  pBVBR_,          \$        P~                  ! VBR`,          RR4      ,          pBWE,          pC\$        PD                  ! \$        Px                  ! VBRa,          VC,          ^ V^,
          4      \$        Px                  ! VBRb,          VC,          ^ V^,
          4      .^R$7      pD\$        P                  ! VD4      P[                  \$        Pz                  4      V5RS&   VBP[                  \$        PP                  4      V5RT&   EK  	  . V nA        \U        V PB                  4       FC  w  pEp5\/        V5R,,          ^,
          4       F   p"V P                  P3                  XEV"34       K"  	  KE  	  V P                  '       g   \-        RU4      h\A        RV\q        V P                  4       RW\q        V PB                  4       RXV RYV RZ2	4       R# u up"i u up"i u up5i u up5i u up5i )cz	meta.jsoncamera_convention opencvz2AR-view meta.json missing opencv camera convention
num_framesr0   r<   
save_widthNr   Nzscene JPEG width z != meta.save_width K_saver   Nc                    V P                  4       pVR;;,          W1,          ,          uu&   VR;;,          W1,          ,          uu&   VR;;,          W2,          ,          uu&   VR;;,          W2,          ,          uu&   V# )    )rv   rv   )rv   r   )   rw   )rw   r   )copy)K_srcsrc_wsrc_hdstKs   &&&& r   _scale_K&ArViewScene.__init__.<locals>._scale_K   sR    

AdGs{"GdGs{"GdGs{"GdGs{"GHr   scene_wristzepisodes.jsonzno episodes.json in z
 (checked )episodeszno episodes listed in stater1   z.npzscene_cam_posewrist_cam_poseumi_posescene_visiblewrist_visibleumi_visiblegripper_radz{  WARN: wrist_umi_calibration.json not found; falling back to per-frame wrist_cam_pose (may be stale on non-visible frames)startendrB   Fg        ep_ididep_rV   frame0nK_in
K_wrist_inT_w2cT_lfree_worldquatsgrips
grip_validzno usable episodes in z (n_window=u    × frame_stride=u    requires ep length ≥ z  auto z_range: [z.4fz, z]m  (data [z.3fu	   ], pad ±zm)z  z_range from caller: [z]mz  auto grip_range: [z]rad  (data [z])z  using rot K-means (K=z) from callerz  fitting K-means (K=z, deterministic seed=0) on u    quaternions…z  clamping K from u    → z (only z training quats available):Nrw   Nrot_bingrip_binz_bingư>yx_binstart_pix_imgz*no samples; all episodes are 0- or 1-framez  built z samples across z episodes (n_window=z, frame_stride=z'; future frames clipped to episode end)rI   rD   rH   r   )rH   rE   )rH   rs   )rH   :r   rF   N)rH   rw   )rH   rv   )Br   r,   r^   r_   r`   ra   rb   rc   rd   r.   rj   r   r   r    get
startswithr9   r%   r"   r#   r$   existsr6   rangeloadappendrJ   isnanri   nanr)   _T_wrist_cam_to_umiprintr   stackarrayrZ   rN   invzerosfloat32ones	enumerater   r
   rL   rQ   concatenateminmaxz_loz_hi_ARVIEW_GRIP_RANGE_PADgrip_logrip_hirg   lenr   tiler   clipint64r(   maximumfloorentries)Fselfr,   r^   r_   r`   ra   rb   rc   rd   re   rf   rg   rh   rj   rk   metan_frames_totalr0   r<   W_savebgr0H_saveW_verifyK_scene_saveK_wrist_saver~   
K_scene_inr   ej_path
candidatescejraw_epsr   ir'   T_wc2umiraw_epe_starte_endn_epscene_poses_allscene_vis_all	T_scene_wT_w2c_scener   r   r   umi_pose_allr   kr(   gepall_zr   r   all_gr   r   all_qK_effpadzee_hee_camuvscaleyxeisF   &&&&&&&&&&&&&&&                                                       r   __init__ArViewScene.__init__p   s6   " J	 (" ,,&k zz499{2==?@xx+R0;;HEE 	A@	AET,/0W"%U<()'		15::b>6! 	Gz)=fXF	G!zz%/D UYYx%8%8 

5?"**E>B 		 lFFHE
%1 |VVXF7; 	
 (5(Am_r
tyy=0?B		O35 	5
A}a!1!1q'  ?!5dii[
:,VWXYYZZ))+,&&R(!7yABB ~&A		G+3tn<=ALL "**Q/?-@

"S "**Q/?-@

"S"**Qz]

"S$q'9":$q'9":$q'7"8!MJZA[A[%-(8"9acagag  ' /0#+  R S %'F&/*Gu&E7?Q&Dax !hh38%!)3L(N3La ).a1A(B(B3L(NTUWOHH05guqy0I&K0I1 ',Ah&?&?0I&KRVXM1/=QI))--	2K xxq	<HHHdAYbjj9EHHT4E88T1aL

CLT2J!%";<1!HZ("#Qh*:ai+@AHHTa!H]+88A;;$)JqM"E!H$QxE!H = MM  DCy%*AB'T
))"**5z?Uj//

;[_++BJJ74HjL" E b }}}(;xj I##/.0HI^abIbHccdfg g
 ?NN4==#Q=RBzN4$8$8=#QRE%-D%-D%d3Zr$s <!IIK,Buyy{3.?yrS T !JD$,T#Jbc
"EF#T	49 NN/3}}$>/< %'wK<0@$A$A/<$> ?EEIIK(+AAGEIIK(+AAG(R} E!IIK,Buyy{3.?rC D  *GW%,g"dl $ &&>1*====!.!5!5bjj!AD+N+;=IJNN$--#H-BBwKK-#HqQE).)9 :E
|?4 5 E
3E~%*>*:%w G"5zl*DF G!.ue!<D ~%ggd004-5q9;;A6"**;M %'^^T5G5G4MTU%V" --B1"W+t?Q?QRByMGt||+t||0KLAWWQ%7'5'9;;A6"((;K zNJ%		1dii$))6KLA''!c'1cAg6==bhhGBwK>>2j>%'WWbgq\%:#<BCEDkDFF*--e4FV*vxx'**BERZZ6
D99B(E2772d8e#3Q	AFGGBtHu$4aQGIOPRB88B<..rxx8BxL"$))BJJ"7B!  . /1.FB2c7Q;'##RG, ( / |||KLLT\\*++;C<N;O P$$,:_\N K67 	8s(N&KZ $R$>  $Is   y:
y?
$z
 z	2zc                   V ^8  d   QhRR/# )r   r   r.   r   )r   s   "r   r   rl   V  s     ! ! !r   c                	,    \        V P                  4      # )N)r   r   r   s   &r   __len__ArViewScene.__len__V  s    4<<  r   c                   V ^8  d   QhRR/# )r   r   dictr   )r   s   "r   r   rl   Y  s     
 
 
r   c                	&   R V P                   RV P                  RV P                  RV P                  RV P                  RV P
                  RV P                  RV P                  V P                  3RV P                  V P                  3R	V P                  /
# )
r^   r_   r`   ra   rb   rc   rd   re   rf   rg   )r^   r_   r`   ra   rb   rc   rd   r   r   r   r   rg   r   s   &r   configArViewScene.configY  s    D--488d11d11		499-4<<6T//
 	
r   c               $    V ^8  d   QhRRRRRR/# )r   ep_idxr.   local_framer   r   r   )r   s   "r   r   rl   g  s'     K KS Ks K?S Kr   c                    V P                   V,          pV P                  eC   VR,          V,          pV\        P                  P	                  V P                  4      ,          pV# VR,          V,           p\        R4      h)u   4x4 wrist camera pose in world. Uses the frozen wrist_cam→umi
transform composed with umi_pose(f) when available (robust to
wrist_visible=False frames), else the raw per-frame wrist_cam_pose.r   r   u   Frozen wrist_umi_calibration missing and per-frame wrist pose not cached; add T_wrist_cam→umi calibration or extend ArViewScene.__init__ to persist wrist_cam_pose per episode.)r   r   r"   rN   r   r6   )r   r   r   r   r   T_world_wrist_cam	abs_frames   &&&    r   _wrist_pose_atArViewScene._wrist_pose_atg  su     ]]6"##/*~k2H (299==9Q9Q+R R$$xL;.	 JK 	Kr   c                   V ^8  d   QhRR/# )r   idxr.   r   )r   s   "r   r   rl   {  s     9 9s 9r   c                	X	   V P                   V,          w  r#V P                  V,          pVR ,          V,           p\        V P                  V4      p\	        W`P
                  4      pV P                  p\        P                  ! \        P                  ! ^V P                  ^,           \        P                  R7      V,          V,           ^ VR,          ^,
          4      p	RVR\        P                  ! VR,          V,          4      R\        P                  ! VR,          V	,          4      R\        P                  ! VR	,          V	,          4      R
\        P                  ! VR,          V	,          4      R\        P                  ! VR,          V	,          4      R\        P                  ! VR,          V	,          P                  \        P                  4      4      RVR\        P                  ! VR,          4      R\        P                  ! VR,          4      /
p
VR,          e   V P                   e   \#        V P                  V4      pVe   \	        WP
                  4      V
R&   WR&   V P%                  W#4      p\        P&                  P)                  V4      p\        P                  ! VR,          4      V
R&   \        P                  ! VP                  \        P                  4      4      V
R&   V P*                  ^ 8  Ed   \        P                  ! V P*                  ^ R \        P                  R7      V,          p\        P                  ! W>,
          ^ VR,          ^,
          4      pVR,          V,          P                  \        P                  4      p\-        VVR,          VR,          4      P                  \        P                  4      pVR,          V,          P                  \        P                  4      p\        P                  ! VR!,          P/                  4       4      V
R&   \        P                  ! VR,          V,          P                  \        P                  4      4      V
R&   \        P                  ! V4      V
R&   \        P                  ! V4      V
R&   \        P                  ! V4      V
R&   V
# )"r   r   r   rgb	start_pixr   	target_yxr   target_zr   target_gripr   
target_rotr   
target_xyzr   _bgr_src_K_inr   _T_w2cr   r   	wrist_rgb_wrist_bgr_src_K_wrist_T_w2c_wristpast_zr   	past_grippast_uvpast_xyzcurrent_xyzrG   r   )r   r   r9   r,   r	   ra   r_   r"   r   aranger^   r   torch
from_numpyrL   r   r   r=   r   rN   r   rj   r   rx   )r   r   r   r   r   	abs_startr8   r   sframesrY   w_bgrr   T_w2c_wristpast_offpast_framespast_xyz_worldr  cur_xyz_worlds   &&                 r   __getitem__ArViewScene.__getitem__{  sp   LL%	]]2xL5(	&tyy)< mm4 IIa*"((;a?%Gr#w{
 3))"_*=e*DE))"X,v*>?))"W+f*=>5++BzN6,BC%**2i=+@A%**:v&--bjj9;U%%bj1e&&r'{3
  l'D,D,D,P,TYY	BE #5e]]#KK (-$%$($7$7$B! iimm,=>"'"2"22l3C"DJ&+&6&6&&rzz2'4N# ;;?yya288DqHH''%"2Ar#w{CK
^K8??

KN*6
BwK99?

9K zN5188DM!,,^D-A-F-F-HICM$//7K(//

; =C"--g6C	N#..~>C
O!&!1!1-!@C
r   )r   r   r   r_   r   r   ra   rc   rd   r^   rb   rj   r`   r,   rg   r   r   )__name__
__module____qualname____firstlineno____doc__r   r   r   r   r   r  __static_attributes__r   r   r   r\   r\   a   sQ    "  *1548.2(,d8L!
K(9 9r   r\   )!r%  
__future__r   r   pathlibr   typingr   r3   numpyr"   r  torch.utils.datar   datasetr   r   r   r	   r
   r   r   r   r   r5   homer   r   r)   r9   r=   rZ   r\   r   r   r   <module>r.     s    #    
   $   Q		::;   ,S' Sr   