+
    XRj{              
       P   R 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HtHt ^ RIHtHtHt ^ RIHtHt R]R,          R,          ]R	3R
]R,          R,          ]R3.t] EF  w  tttt]! ]]]]R 4      t] ! ] R]R,           R]PB                  R R24       ]PD                  ]PF                  ]PH                  ut%t&t']! ]%]&]4      w  t(t)R;t*t+Rt,]+^]PZ                  ! ]P\                  ! ],4      ^,          4      ,          ,          t/]P`                  ! ]/^ ]*^,          .^ ]/]+^,          .. RO.4      t1Rt2R  EF  t3](])R!,          ]3,          R,          ,           ])R",          R,          ,           t4]! ]4](])R#,          R7      w  t5t6]5]%Pn                  ]'&   ]! ]64      ]%Pp                  ]'&   ],]%Pr                  ]'&   ]Pt                  ! ]%]&4       ]! ]&Pv                  ]',          ]&Px                  ]',          4      t=]P|                  ! ]%]+]*4      ;_uu_ 4       t?]?P                  ]&]'R7       ]?P                  4       tBRRR4       ]! ]P                  ! ]B]P                  4      ].4      tE]R,          ]E9   g   EK;  ]! . ]E]R,          ,          O]1N5!  w  tFtG]P                  ]P                  P                  ]F4      ,          tK] ! R]P                  P                  ]KR$,          ]=R$,          ,
          4      R,          R R24       Rt2 M	  ]2'       g	   ] ! R4       ]P                  4        EK  	  R#   + '       g   i     L; i)%zLoop-closure self-test for both AR render layers (no cameras/servos needed):
for each layer, measure its board frame, render the board from a known angled pose,
localize that render, and check the recovered camera pose matches ground truth.N)CONFIGS)Layer
look_at_mjmat2quatboard_geom_pose)cam_mj_to_cvdetect_boards
solve_pose)ARM_BASEUMI_V2scenearmmodelarmboardumigripperar_backc                     V # )N )jns   &ar_view/_selftest.py<lambda>r      s    b    z	: board 'namez' measured, reproj z.2fpxi  g      I@Fg)\(?g?)up)cameraz  loop-closure recover err: i  z mmTz+  recover: board not visible from test pose)    r      )g      ?g      )NNN   )r   r   )r   r   ):N   Nr!   )N__doc__ossysnumpynpcv2mujocopathinsertdirnameabspath__file__configsr   render_corer   r   r   r   calib_utilsr   r   r	   aruco_boardsr
   r   caseslblmpboardmatLprintreprojr   dataextcammdextposxmatWHfovytanradiansfarrayKdonesgneyepRmjcam_poscam_quatcam_fovy
mj_forwardcam_xposcam_xmatTtrueRendererrupdate_scenerenderimgcvtColorCOLOR_RGB2GRAYdetsT_B_cam_	T_board_wlinalginvTrecnormcloser   r   r   <module>re      s  S 
 
  
  277??277??277??83L#MN O  D D ? ? )
75>'*Hj	A
#G,fi	@	B !CUCc2uc=1A	SE5=/)<QXXcN"
MNIAq#1c*ICKAD	Q

4(1,--.A
1aQ-!QA	:;ADDJ$t++d4j4.??Cd43		#"3-

3

3!QQZZ_ajjo>__Q1%%NN1SN)((*C & S\\#s/A/ABUGL=D #<T%-%8<!<JGQ;;w!77D0UeTYl@Z1[^b1bcf0ggjklD# $ ;<GGI= !$ &%s   <$N	N%