*i:dZddlZddlmZddlmZddlmZmZm Z m Z ddl Z ddl Z ddlmZeGddZGd d ZGd d Zd edefdZdedefdZdS)z.Camera calibration engine using ChArUco boardsN) dataclass)Path)DictListOptionalTuple)RotationczeZdZUdZdZeed<dZeed<dZe ed<dZ e ed<d Z e ed <e d ed dfd ZdS)ChArUcoBoardConfigz+Configuration for ChArUco calibration board squares_x squares_ygQ? square_lengthgZd;O? marker_length DICT_6X6_250 dictionarydatareturncft|d|d|d|d|dS)Nr rrrr)r rrrr)r )rs 5/home/robot-lab/raiden_cmu/raiden/calibration/core.py from_dictzChArUcoBoardConfig.from_dictsA!;';'//L)     N)__name__ __module__ __qualname____doc__r int__annotations__rrfloatrrstr staticmethoddictrrrr r s55IsIsM5 M5   $J$$$  !5   \   rr c peZdZdZdefdZdejdee eje ejffdZ dejdee eje eje e e ejffdZ dejd ejd ejd ejdee eje ejff d Z d S)ChArUcoDetectorz Detects ChArUco boards in images board_configc"||_|j}ttj|r?t tj|}tj||_ntd|tj|j |j f|j |j |j|_ tj|_tj|j|j|_dS)NzUnknown ArUco dictionary: )size squareLength markerLengthr)r&rhasattrcv2arucogetattrgetPredefinedDictionary ValueError CharucoBoardr rrrboardDetectorParametersdetector_params ArucoDetectordetector)selfr& dict_name aruco_dicts r__init__zChArUcoDetector.__init__%s(!+ 39i ( ( G I66J!i?? KKDOOE)EEFF FY++(,*@A%3%3 ,    #y;;== //AUVV rimagercPt|jdkr tj|tj}n|}|j|\}}}|t|dkrdStj||||j \}}}|dkrdS||fS)a#Detect ChArUco board in image Args: image: Input image (grayscale or color) Returns: Tuple of (corners, ids) or (None, None) if detection failed - corners: Nx2 array of corner positions - ids: Nx1 array of corner IDs NrNN markerCorners markerIdsr;r2 lenshaper,cvtColorCOLOR_BGR2GRAYr6 detectMarkersr-interpolateCornersCharucor2 r7r;graymarker_corners marker_ids_ num_cornerscharuco_corners charuco_idss rdetectzChArUcoDetector.detect=s u{  q <s'9::DDD)- (C(CD(I(I% A  ZA!5!5:58I4W4W( * 5X5 5 1 _k !  : ++rc\t|jdkr tj|tj}n|}|j|\}}}|t|dkrdStj||||j \}}}|dkrdd||fS||||fS)aDetect ChArUco board and return ArUco marker info for debugging Args: image: Input image (grayscale or color) Returns: Tuple of (charuco_corners, charuco_ids, marker_corners, marker_ids) - charuco_corners: Nx2 array of ChArUco corner positions (or None) - charuco_ids: Nx1 array of ChArUco corner IDs (or None) - marker_corners: List of detected ArUco marker corners (or None) - marker_ids: Array of detected ArUco marker IDs (or None) r=Nr)NNNNr?rBrIs rdetect_with_markersz#ChArUcoDetector.detect_with_markerscs$ u{  q <s'9::DDD)- (C(CD(I(I% A  ZA!5!5))58I4W4W( * 5X5 5 1 _k !  ~z9 9 ^ZGGrcornersids camera_matrix dist_coeffsc ptj|||j||dd\}}}|sdS||fS)aEstimate board pose from detected corners Args: corners: Detected corner positions ids: Detected corner IDs camera_matrix: Camera intrinsic matrix dist_coeffs: Camera distortion coefficients Returns: Tuple of (rvec, tvec) or (None, None) if estimation failed - rvec: Rotation vector (3x1) - tvec: Translation vector (3x1) N)charucoCorners charucoIdsr2 cameraMatrix distCoeffsrvectvecr>)r,r-estimatePoseCharucoBoardr2)r7rTrUrVrWsuccessr]r^s r estimate_posezChArUcoDetector.estimate_posesX("i@@"*&"A  t :TzrN)rrrrr r:npndarrayrrrQlistrSrar#rrr%r%"s5**W%7WWWW0$,Z$, x #Xbj%99 :$,$,$,$,L)HZ)H hrz2HTNHRZDXX )H)H)H)HV!!Z!z ! Z ! x #Xbj%99 : !!!!!!rr%ceZdZdZdefdZdeejdeejde e e fde fdZ e jfd eejd ee ejejfd e de fd Zd ee ejejfde fd Zed e defdZdejdejdejdejdejdejdefdZdS)CameraCalibratorz'Calibrates cameras using ChArUco boardsr&c<||_t||_dS)N)r&r%r6)r7r&s rr:zCameraCalibrator.__init__s(' 55 r all_cornersall_ids image_sizercBt|dkrdddStj|||jj|dd\}}}}}d|||d|Dd |Dd S) aCalibrate camera intrinsics from multiple views Args: all_corners: List of detected corners from each view all_ids: List of detected IDs from each view image_size: Image size (width, height) Returns: Dictionary with calibration results: - camera_matrix: 3x3 intrinsic matrix - distortion_coeffs: Distortion coefficients - reprojection_error: RMS reprojection error - success: Whether calibration succeeded r=Fz%Need at least 3 views for calibrationr`errorN)rYrZr2 imageSizer[r\Tc6g|]}|Sr#tolist).0rs r z9CameraCalibrator.calibrate_intrinsics.. 000Qahhjj000rc6g|]}|Sr#rp)rrts rrtz9CameraCalibrator.calibrate_intrinsics..rur)r`rVdistortion_coeffsreprojection_errorrvecstvecs)rCr,r-calibrateCameraCharucor6r2rqflatten) r7rhrirjretrVrWrzr{s rcalibrate_intrinsicsz%CameraCalibrator.calibrate_intrinsicss( {  a  $/VWW W I , ,*"m)$! -   6]K*1133!,!4!4!6!6!=!=!?!?"%00%00000%000    r robot_poses camera_posesmethodc"t|dkst|dkrdddSt|t|krdddSg}g}|D]D}||ddddf||ddddfEg}g}|D]F\} } tj| \} } || || Gtj|||||\} }tj| \}} d | ||||d S) a`Perform hand-eye calibration (eye-in-hand configuration) Solves the AX=XB problem where: - A: Transformation between robot gripper poses - B: Transformation between camera poses - X: Transformation from camera to gripper (what we're solving for) Args: robot_poses: List of 4x4 robot gripper-to-base transformation matrices camera_poses: List of (rvec, tvec) tuples for board-to-camera transforms method: OpenCV calibration method (default: Tsai) Returns: Dictionary with calibration results: - rotation_matrix: 3x3 rotation matrix (camera to gripper) - translation_vector: 3x1 translation vector (camera to gripper) - rotation_vector: 3x1 rotation vector (axis-angle) - success: Whether calibration succeeded r=FzNeed at least 3 pose pairsrlz2Robot poses and camera poses must have same lengthN)R_gripper2baset_gripper2base R_target2cam t_target2camrT)r`rotation_matrixtranslation_vectorrotation_vectorr)rCappendr, RodriguescalibrateHandEyerqr}_get_method_name)r7rrrrrposerrr]r^RrM R_cam2gripper t_cam2gripperrvec_cam2grippers rcalibrate_hand_eyez#CameraCalibrator.calibrate_hand_eyes2 {  a  3|#4#4q#8#8$/KLL L {  s<00 0 0 M  1 1D  ! !$rr2A2v, / / /  ! !$rr1Q3w- 0 0 0 0  & & &JD$=&&DAq    " " "    % % % %(+';))%% ( ( ( $ }"mM::!,3355"/"7"7"9"9"@"@"B"B/7799@@BB++F33    rct|dkrdddSg}g}|D]j\}}tj|\}}|t j|||kt|dkr't j|}n|d}| } tj|d} | j } | | z} tj| \} }d| | |  dd S) a Calibrate a fixed scene camera For a fixed scene camera viewing a fixed calibration board, we compute the camera's pose relative to the board frame (world frame). We average across multiple observations for robustness. Args: camera_poses: List of (rvec, tvec) tuples for board-to-camera transforms Returns: Dictionary with calibration results: - rotation_matrix: 3x3 rotation matrix (camera to board/world frame) - translation_vector: 3x1 translation vector (camera position in board/world frame) - rotation_vector: 3x1 rotation vector - reference_frame: "board" (the board frame is used as world frame) - success: Whether calibration succeeded FzNeed at least 1 poserlraxisTr2)r`rrrreference_frame) rCr,rrr from_matrixr} concatenatemean as_matrixrbTrq)r7r rotations translationsr]r^rrM avg_rotationavg_R_board_to_camavg_t_board_to_camavg_R_cam_to_boardavg_t_cam_to_boardavg_rvecs rcalibrate_scene_cameraz'CameraCalibrator.calibrate_scene_camera,sw( |  q $/EFF F  & 0 0JD$=&&DAq   X1!44 5 5 5     / / / / y>>A  #/ ::??AALL$Q  &(@  )+F  (;6(;(;<<?8 8 8 8 8 t = = = = =\ =$+$+Z$+j $+ j $+ z $+Z$+ $+$+$+$+$+$+rrf poses_filerc~t|d5}tj|cdddS#1swxYwYdS)zLoad calibration poses from JSON file Args: poses_file: Path to calibration poses JSON file Returns: Dictionary with poses data rsN)openjsonload)rfs rload_calibration_posesrs j#  !y||s 266results output_filect|}|jddt|d5}t j||dddddS#1swxYwYdS)zSave calibration results to JSON file Args: results: Calibration results dictionary output_file: Path to output JSON file T)parentsexist_okwr)indentN)rparentmkdirrrdump)rr output_pathrs rsave_calibration_resultsrs{##KTD999 k3  (1 '1Q''''((((((((((((((((((sA!!A%(A%)rr dataclassesrpathlibrtypingrrrrr,numpyrbscipy.spatial.transformr r r%rfr rrr#rrrsg44 !!!!!!............ ,,,,,,          (MMMMMMMM`d+d+d+d+d+d+d+d+N s t     (d ( ( ( ( ( ( (r