+
    >Rj[                         R t ^ RIt^ RIt^ RIHtHtHtHtH	t
 ]P                  P                  ]]P                  P                  4       4      t]P                  ! . R
O4      tR tR tR tR tR tR tR	 tR# )zSPinhole calibration + board-pose utilities for the capture-relocalization pipeline.N)DICTARM_BASEUMI_TOPUMI_BACKboardc                   \         P                  V 4      w  r#p/ pVf   V# V Ff  p\        V4      pVP                  W#4      w  rVf   K'  \	        V4      ^8  g   K9  VP                  R^4      V	P                  R^4      3WVR,          &   Kh  	  V# )zJ-> {name: (objpts Nx3, imgpts Nx2)} for each board with >=4 markers found.name)DETECTORdetectMarkersmkboardmatchImagePointslenreshape)
grayspecscornersids_outspecbobjimgs
   &&        ar_view/calib_utils.pydetect_boardsr   	   s    ,,T2OG!
C
{
DM%%g3?s3x2~!$R!3S[[Q5G HCV	 
 J    c           	        Vw  r4\         P                  ! V^ VR,          .^ W$R,          .. RO.\        4      p\        P                  \        P
                  ,          \        P                  ,          \        P                  ,          \        P                  ,          \        P                  ,          \        P                  ,          pV  Uu. uF(  qw^ ,          P                  \         P                  4      NK*  	  ppV  Uu. uF(  qw^,          P                  \         P                  4      NK*  	  p	p\        P                  ! WW43V\         P                  ! ^4      VR7      w  rrpV\        \!        W4      4      V
3# u upi u upi )zZhang with principal point fixed at center, square pixels, zero distortion.
views = list of (objpts, imgpts). Returns K, per-view (rvec, tvec), reproj err px.       @flags    r"      )nparrayfloatcv2CALIB_USE_INTRINSIC_GUESSCALIB_FIX_PRINCIPAL_POINTCALIB_FIX_ASPECT_RATIOCALIB_ZERO_TANGENT_DISTCALIB_FIX_K1CALIB_FIX_K2CALIB_FIX_K3astypefloat32calibrateCamerazeroslistzip)viewssize_whf_guessWHK0r    vr   r   errKdistrvecstvecss   &&&            r   estimate_focalrA      s&    DA	GQC(1g3w*?KU	SB**S-J-JJ''(*-*E*EF # 0 01363C3CDE -2
2EqQ4;;rzz"EC
2,1
2EqQ4;;rzz"EC
2!$!4!4Svr288TU;^c!dCDd3u$%s** 3
2s   
.E9>.E>c                    \         P                  ! V P                  \        P                  4      VP                  \        P                  4      VR \         P
                  R7      w  r4p\         P                  ! V P                  \        P                  4      WEVR 4      w  rg\        \        P                  P                  VP                  R^4      V,
          ^R7      P                  4       4      p\        WE4      V3# )Nr   )axisr	   )r'   solvePnPr/   r$   r0   SOLVEPNP_ITERATIVEprojectPointsr&   linalgnormr   meanrt_to_T)	objptsimgptsr=   okrvectvecprojr   r<   s	   &&&      r   
solve_poserQ   %   s    \\&--

";V]]2::=VXY[_(+(>(>@NBdbjj 94q$OGD
		t||B2V;!DIIK
LC4##r   c                     \         P                  ! ^4      p\        P                  ! \         P                  ! V \
        4      4      ^ ,          VR&   \         P                  ! V\
        4      P                  4       VR&   V# )   N   NrU   rU   rV   )r$   eyer'   	Rodriguesasarrayr&   ravel)rN   rO   Ts   && r   rJ   rJ   -   sS    
q	Abjju56q9AfIzz$&,,.AeHHr   c                     VR \         P                  ! \         P                  ! V 4      R ,          4      ,          ,          p\         P                  ! V^ VR ,          .^ W2R ,          .. RO.\        4      # )r   r!   )r$   tanradiansr%   r&   )fovy_degr8   r9   fs   &&& r   K_from_fovyrb   4   sR    	S266"**X.4556A88aAG_q!WoyA5IIr   c                    \         P                  ! ^4      p\         P                  ! V\        4      P	                  ^^4      \
        ,          VR&   \         P                  ! V \        4      VR&   V# )z3MuJoCo camera world pose -> T_camCV_in_world (4x4).rT   rW   )r$   rX   rZ   r&   r   RX180)posxmatr\   s   && r   cam_mj_to_cvrg   9   sN    
q	A

4'//15=AfIzz#u%AeHHr   c                    V R,          \         ,          pV R,          pVR,          VR,          rCVRP                  R \        V4      \        V4      ,            4       4      3# )z.-> (pos, xyaxes string) for a MuJoCo <camera>. c              3   (   "   T F  qR  x  K
  	  R# 5i)z.6fN ).0r;   s   & r   	<genexpr>cam_cv_to_mj.<locals>.<genexpr>F   s     ?->c7->s   rT   rW   )NNNr"   )ro   r#   )rd   joinr3   )T_camCV_in_worldRre   xys   &    r   cam_cv_to_mjru   A   sQ     5(A
5
!CT7AdGq?T!WtAw->????r   )g      ?      rv   )__doc__numpyr$   r'   aruco_boardsr   r   r   r   r   r   arucoArucoDetectorDetectorParametersr
   diagrd   r   rA   rQ   rJ   rb   rg   ru   rk   r   r   <module>r~      sh    Y  L L99""4)E)E)GH
!"+$J
@r   