+
    !`Tj(              
          R t ^ RIt^ RIt^ RIt^ RIt^ RIt^ RIt]P                  P                  ^ ]P                  P                  ]P                  P                  ]P                  P                  ]4      4      4      4       ^ RIHt ^ RIHtHt ^ RIHtHt ^ RIHtHt ^ RIHt ^ RIHtHtHtHt ^ RIH t H!t! R	 t"R
 t#]$R8X  d
   ]#! 4        R# R# )a  Live real-vs-sim AR overlay with two render layers, two named cameras.

Cameras are referenced by role: "scene" (third-person, top row) and "wrist" (UMI wrist
cam, bottom row), resolved via cameras.json. Each uses its saved intrinsics from
calibrate_camera.py (frames are undistorted and the pose solved with the calibrated K);
if a camera has no saved intrinsics yet, it falls back to a live single-view focal estimate.

Grid per frame (posed by the live calibrated servos):
  scene row: [ rgb | scene render | umi render (if UMI board seen) ]
  wrist row: [ rgb | scene render (if base board seen) | blank ]

Usage:
  python arview.py --config arm [--scene-index N] [--wrist-index N] [--width 640] [--port ...]
N)Bus)CONFIGS
calib_path)detect_boardsestimate_focal)ARM_BASEUMI_V2)CALIB_BOARD)Layerlabelblank
blend_cell)
open_namedload_intrinsicsc                      / pV  FV  p \         P                  ! \        \        V4      4      4      pTP                  4        F  w  rETP                  YE4       K  	  KX  	  V#   \         d     Kj  i ; iN)jsonloadopenr   FileNotFoundErroritems
setdefault)configsoutcdatasides   *     ar_view/arview.pymerged_calibr   !   sh    
C	99T*Q-01D jjlFCNN3" #  J	 ! 		s   (A!!A0/A0c                     \         P                  ! 4       p V P                  R RR7       V P                  R\        ^R7       V P                  R\        ^ R7       V P                  R\        RR7       V P                  RR	R
.R
RR7       V P                  RRRR7       V P                  R\        ^
R7       V P                  R\        RRR7       V P                  RRR7       V P	                  4       pVP
                  P                  R4       Uu0 uF*  q"P                  4       '       g   K  VP                  4       kK,  	  pp\        VP                  4      p\        R4      pV'       g   V'       g   \        P                  ! R4       \        R4       VP                  R
8X  du   \        P                  P!                  \        P                  P#                  \        P                  P%                  \&        4      4      RR4      p\)        RV\*        R
R 4      pM0\)        R\,        VP                  ,          R,          \.        R R! 4      p\)        R"\,        R,          R,          \0        R#R$ 4      pWx.p	VP3                  V4       VP3                  V4       V	 FK  p
\        R%V
P4                   R&V
P6                  R' R(V
P8                   UUu. uF  w  p qNK
  	  upp 24       KM  	  VP:                  '       d   \=        VP:                  4      M	\=        4       p\?        \A        V4      \A        V4      ,            Uu0 uF  p\        V4      kK  	  up4      pV Uu. uF  qPC                  V4      '       g   K  VNK  	  ppV F  pVPE                  VR)4       K  	  \        R*V 24       / pRVPF                  3R+VPH                  33 F  w  pp\K        VV4      w  pppVf   \        R%V R,24       K+  VV9   d   RM
\M        V4      pR-VR.VR/VR0R/VV&   VV9   d"   \        R%V R1V R2VPN                   R324       Kt  Vf   \        R%V R1V R4V R5V R62	4       K  \        R%V R1V R7VR8,          R' R9VR:,           R;VR<,           R624       K  	  V'       g   \        P                  ! R=4       V	 U
u. uF  qPP                  NK  	  pp
\        \S        VPT                  ^,          ^,          4      4      p\V        PX                  ! R>\V        PZ                  4       RV	^ ,          PP                  /p\A        V4       EF  pVV9  d   K  VV,          pVP]                  V\.        4      p\        R?VPN                   R@V RAVRB,           RC24       . Rpp\_        V4      VPN                  8  Ed   VR-,          Pa                  4       w  ppV'       d   Vf   KC  VPb                  ^,          VPb                  ^ ,          3p\e        \V        Pf                  ! V\V        Ph                  4      V.4      pVRB,          V9   d    VPk                  VVRB,          ,          4       \V        Pl                  ! VVPT                  \        \S        VPb                  ^ ,          VPT                  ,          VPb                  ^,          ,          4      4      34      p \V        Pn                  ! R>\5        V RDV REV^ ,           R;V^,           RF\_        V4       RGVPN                   2
R[4      4       \V        Pp                  ! ^4      ^,          ^8X  g   EK   \_        V4      ^8  db   \s        VVRH\u        V4      ,          RI7      w  VR0&   pp!\        R%V RJV^ ,           R;V^,           RKVR0,          R\,          RL RMV!R' RN24       EKt  \        R%V RO24       EK  	  \        RP4         / p"V F  pVPw                  V4      p#V#f   K  V#V"V&   K!  	  V	 F  p
V
Py                  V"4       K  	  . p$VP{                  4        EF0  w  pp VR-,          Pa                  4       w  ppV'       d   Vfv   \        VVPT                  V RQVR.,           24      p%V$Pk                  \        P                  ! V%\        VVPT                  4      \        VVPT                  4      .4      4       K  VPb                  ^,          VPb                  ^ ,          p'p&\e        \V        Pf                  ! V\V        Ph                  4      V4      p(VR/,          pVew   VR0,          fl   V&\        VR:,          4      ,          pVR0,          P                  4       VR0&   VR0,          ^ ;;,          V,          uu&   VR0,          ^;;,          V,          uu&   VfY   VR0,          fN   V('       dF   \u        V(P                  4       RR RS7      p)\s        V).V&V'3RH\u        V&V'4      ,          RI7      w  VR0&    pVPT                  p*\        \S        V'V*,          V&,          4      4      p+\V        Pl                  ! VV*V+34      p,VR0,          eN   ^\        P                  ! \        P                  ! V'RT,          VR0,          R],          ,          4      4      ,          MRUp-V	^ ,          V	^,          p/p.\5        V,P                  4       V RVV& R;V' 24      .p0V0Pk                  \        V.V(VR0,          V,V-4      4       T0Pk                  VR8X  d   \        V/V(VR0,          V,V-4      M\        V+V*RW4      4       V$Pk                  \        P                  ! V04      4       EK3  	  V$'       Ed3   \u        RX V$ 4       4      p1V$ U2u. uFD  p2\V        P                  ! V2^ ^ ^ V1V2Pb                  ^,          ,
          \V        P                  4      NKF  	  p$p2\        P                  ! V$4      p3V3Pb                  ^,          VP                  8  d   VP                  V3Pb                  ^,          ,          p4\V        Pl                  ! V3VP                  \        \S        V3Pb                  ^ ,          V4,          4      4      3\V        P                  RY7      p3\V        Pn                  ! R>V34       \V        Pp                  ! ^4      ^,          \        RZ4      8X  g   EK   TP                  4        F  pTR-,          P                  4        K  	  T	 F  p
T
P                  4        K  	  \V        P                  ! 4        TP                  4        R# u upi u uppi u upi u upi u up
i   \|         d	    R)Rpp ELi ; iu up2i   TP                  4        F  pTR-,          P                  4        K  	  T	 F  p
T
P                  4        K  	  \V        P                  ! 4        TP                  4        i ; i)^z--configarm)defaultz--scene-index)typer"   z--wrist-indexz--widthi  z--boardarm_basecalibzUscene-localization board: the large calib board (default) or the small arm-base board)choicesr"   helpz--live-calibsceneu   comma-sep camera roles to intrinsic-calibrate live at startup instead of loading a saved file (default: scene — e.g. the iPhone, whose intrinsics drift))r"   r'   z--calib-framesz--max-displayi@  zDcap the montage width for display (detection is full-res regardless))r#   r"   r'   z--portN,gripperz3No calibration found. Run calibrate_servo.py first.z(Measuring board frames (loop closure)...calib_board_modelzcalib_board.xmlc                     V # r    jns   &r   <lambda>main.<locals>.<lambda>J   s    SU    modelarmboardc                     V # r   r-   r.   s   &r   r0   r1   L   s    bdr2   umiar_backc                     V R9   d   R # R# )jawN)r9   gripper_jawr-   r.   s   &r   r0   r1   N   s    "0F*F!PD!Pr2   z  z: board reproj z.2fzpx, posed servos FzLive servos: wristu   : no camera — skippingcapidxintrKz: index z-, will live-calibrate intrinsics at startup (z frames)uK   , NO saved intrinsics — estimating live (run: python calibrate_camera.py z	 --index )z, intrinsics loaded (rms rmszpx @ widthxheightuU   No cameras opened — check connections / cameras.json / --scene-index/--wrist-index.zAR view (q to quit)z
Hold on, recording the first z) frames for intrinsic calibration of the z camera (point it at the namez
 board)...zcalibrating z @ z: /g?)f_guessz live intrinsics @ z: f=z.0fzpx, reproj pxuB   : not enough board views — falling back to single-frame estimateuI     (window opening — if you don't see it, cmd-tab to the python process)u   : no frame — check camera c                 &    \        V ^ ,          4      # )    )len)vs   &r   r0   r1      s    C!Ir2   )keyg       @g      N@z rgb zumi n/a in wrist viewc              3   F   "   T F  qP                   ^,          x  K  	  R# 5i)   N)shape).0rs   & r   	<genexpr>main.<locals>.<genexpr>   s     4t!771::ts   !)interpolationq)rJ         )rJ   rJ   )rO   rO   )QargparseArgumentParseradd_argumentint
parse_args
live_calibsplitstripr   configsysexitprintboardospathjoindirnameabspath__file__r
   r	   r   r   r   build_posingr   reprojposingportr   sortedlistpingtorquescene_indexwrist_indexr   r   calib_frames
board_specroundrB   cv2namedWindowWINDOW_AUTOSIZEgetrK   readrP   r   cvtColorCOLOR_BGR2GRAYappendresizeimshowwaitKeyr   maxread_posposer   	Exceptionr   nphstackfloatcopyvaluesdegreesarctanr   copyMakeBorderBORDER_CONSTANTvstackmax_display
INTER_AREAordreleaseclosedestroyAllWindows)5apasr^   	arm_calib
grip_calibscene_modelscene_layer	umi_layerlayersL_busall_sidsr   livecamsroleovrr<   r=   r>   specsHdefboard_for_rolecamspecviewsnatokframeddisperrticksprowsphnatWnatHdetsviewWHrealfovyscene_Lumi_LcolswmaxrR   montscs5                                                        r   mainr   -   sY   		 	 	"BOOJO.OOO#qO9OOO#qO9OOICO5OOI
G'<gp  rOONGm  n OO$3O;OOO#t_  aOOHdO+
A%&\\%7%7%<J%<	)!'')%<JJ QXX&Ii(JZFG	
45ww'ggll277??277??83L#M#68IKG[+wVGWQXX%6w%?:WdeeWY/8&)PRI%FY':&177)?188C.8I\]\d\dJe\dQXQRTUWX1\dJeIfgh  #aff+SUCtIj9I'IJ'I!s1v'IJKH#58Cxx}CC8D5

3 	M$
 !D.!--0HI	c s+S!;Btf456z)tt/DS%fdCFT
:BtfHSE)VWXWeWeVffnop\BtfHSE *66:V9SEL M BtfHSE *u+c*%WaX?OqR S J hi#)*6a\\6E*uQWWq[1_%&DOO)3+>+>? vay334NT
z!4j!!$1//? @v6tF|nJP 	Qs%j1>>)E
)IB;;q>5;;q>2Ccll5#2D2DEvNAF|q QtF|_-::eaggs5Q!''9QTYT_T_`aTb9b3c/d%efDJJ,T\$s3q6(!CF82cRWj\YZ[\[i[iZj#k*,- {{1~$*u:?-eS#C.QCHaBtf/AxqQSXc]SVDWWbcfgjbkkmnoBtf^_`5 8 

UV@ELL%=!"E#J  u  D!ZZ\	c, #E
 1IB U]tQWW6RSVW\S]R^._`BKK		2uT177/CU4QRQXQXEY*Z [\"[[^U[[^d$S\\%9K9K%LeT6{#C(8uT']33A#Cy~~/CHHQK1$KHQK1$K<CH$4t{{}2EFD%3TFT4LRUX[\`bfXgRg%hNCHaGGdQho./zz%!Q0QTUXQYQeq2::biis
SXd^0K&LMMko!'F1IdiikdV5av+FGHJwc#hdKLTU\_JudCHdDI!&q!-D!EGBIIdO,C *F t4t44!%'!%A **1aAtaggaj7H#J]J]^!%  'yy::a=1==0A6B::dQ]]CdjjQRmVXFX@Y<Z,[47NNDD

0$7{{1~$C0;;=CJ  !AGGI 		m K, Kf K5. +f ! , %tB,D' ;;=CJ  !AGGI 		s   t$)t$t)4t/t40t4(t9u "<u t>9u E'u )F+u u +A
u5C:u 5u >uu uu A.w__main__)%__doc__rY   r   rf   rb   numpyr   ry   rg   insertri   rj   rk   feetech_busr   r   r   r   calib_utilsr   r   aruco_boardsr   r   calib_boardr	   render_corer
   r   r   r   camerasr   r   r   r   __name__r-   r2   r   <module>r      s      	 
  
 277??277??277??83L#MN O  ' 5 ) # 7 7 /	fR zF r2   