
    Mj[                         d Z ddlZddlZddlmZmZmZmZm	Z
 ej                  j                  eej                  j                               Z ej                  g d      Zd Zd Zd Zd Zd	 Zd
 Zd Zy)zSPinhole calibration + board-pose utilities for the capture-relocalization pipeline.    N)DICTARM_BASEUMI_TOPUMI_BACKboard)g      ?      r   c                 
   t         j                  |       \  }}}i }||S |D ]^  }t        |      }|j                  ||      \  }}	|&t	        |      dk\  s5|j                  dd      |	j                  dd      f||d   <   ` |S )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
             2/home/cameronsmith/cad_recovery_mnt/calib_utils.pydetect_boardsr    	   s    ,,T2OGS!
C
{
 IDM%%gs3S?s3x2~!$R!3S[[Q5G HCV	I
 J    c           	         |\  }}t        j                  |d|dz  gd||dz  gg dgt              }t        j                  t        j
                  z  t        j                  z  t        j                  z  t        j                  z  t        j                  z  t        j                  z  }| D cg c]$  }|d   j                  t         j                        & }}| D cg c]$  }|d   j                  t         j                        & }	}t        j                  ||	||f|t        j                  d      |      \  }
}}}}|t        t!        ||            |
fS c c}w c c}w )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.r          @r   r      r%      flags)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_focalrF      s6    DAq	GQC(1gq3w*?KU	SB**S-J-JJ''(*-*E*EF # 0 01363C3CDE -2
2q1Q4;;rzz"
2C
2,1
2q1Q4;;rzz"
2C
2!$!4!4S#1vr288TU;^c!dCD%d3ue$%s** 3
2s   +)E)Ec                    t        j                  | j                  t        j                        |j                  t        j                        |d t         j
                        \  }}}t        j                  | j                  t        j                        |||d       \  }}t        t        j                  j                  |j                  dd      |z
  d      j                               }t        ||      |fS )Nr'   r   r   r%   )axis)r,   solvePnPr4   r)   r5   SOLVEPNP_ITERATIVEprojectPointsr+   linalgnormr   meanrt_to_T)	objptsimgptsrB   okrvectvecprojr   rA   s	            r   
solve_poserV   %   s    \\&--

";V]]2::=VXY[_(+(>(>@NBdbjj 94q$OGD!
		t||B2V;!DIIK
LC4##r!   c                     t        j                  d      }t        j                  t        j                  | t
                    d   |d dd df<   t        j                  |t
              j                         |d ddf<   |S )N   r   r   )r)   eyer,   	Rodriguesasarrayr+   ravel)rS   rT   Ts      r   rO   rO   -   sg    
q	Abjju56q9Abqb"1"fIzz$&,,.Abqb!eHHr!   c                     |dt        j                  t        j                  |       dz        z  z  }t        j                  |d|dz  gd||dz  gg dgt              S )Nr#   r   r$   )r)   tanradiansr*   r+   )fovy_degr=   r>   fs       r   K_from_fovyrc   4   sT    	S266"**X.4556A88aAG_q!QWoyA5IIr!   c                     t        j                  d      }t        j                  |t              j	                  dd      t
        z  |ddddf<   t        j                  | t              |dddf<   |S )z3MuJoCo camera world pose -> T_camCV_in_world (4x4).rX   r   N)r)   rY   r[   r+   r   RX180)posxmatr]   s      r   cam_mj_to_cvrh   9   sb    
q	A

4'//15=Abqb"1"fIzz#u%Abqb!eHHr!   c                     | ddddf   t         z  }| dddf   }|dddf   |dddf   }}|dj                  d t        |      t        |      z   D              fS )z.-> (pos, xyaxes string) for a MuJoCo <camera>.Nr   r   r%    c              3   $   K   | ]  }|d  
 yw)z.6fN ).0r@   s     r   	<genexpr>zcam_cv_to_mj.<locals>.<genexpr>F   s     ?Ac7?s   )re   joinr8   )T_camCV_in_worldRrf   xys        r   cam_cv_to_mjrt   A   so    !RaR 5(A
2A2q5
!CQT7AadGqA?T!WtAw->????r!   )__doc__numpyr)   r,   aruco_boardsr   r   r   r   r   r   arucoArucoDetectorDetectorParametersr   diagre   r    rF   rV   rO   rc   rh   rt   rl   r!   r   <module>r|      sj    Y  L L99""4)E)E)GH!"+$J
@r!   