From 61c4845c26178ba161c043e0b12a611cd4d51f48 Mon Sep 17 00:00:00 2001 From: Kazadhum Date: Fri, 29 Mar 2024 21:28:26 +0000 Subject: [PATCH] running into a problem when calling calibrateRobotWorldhandEye; also need to confirm calc of pattern_T_cam (#912) --- .../other_calibrations/cv_handeye_calib.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/atom_evaluation/scripts/other_calibrations/cv_handeye_calib.py b/atom_evaluation/scripts/other_calibrations/cv_handeye_calib.py index 5d082d84..1b752df9 100755 --- a/atom_evaluation/scripts/other_calibrations/cv_handeye_calib.py +++ b/atom_evaluation/scripts/other_calibrations/cv_handeye_calib.py @@ -74,6 +74,7 @@ def cvHandEyeCalibrate(objp, dataset, camera, pattern, number_of_corners): # NOTE: The documentation calls this the transform from world to cam because in their example (which is eye-in-hand), their pattern's frame is their world frame. This is not the case in ours. So when the documentation mentions the world to cam tf, it corresponds to our pattern/target to camera tf. (I think.) # Use solvePnP() to get this transformation + # Need to check if these calculations (from here to l. 108) are correct print('Calculating transform from camera to pattern for collection 006....') @@ -100,7 +101,12 @@ def cvHandEyeCalibrate(objp, dataset, camera, pattern, number_of_corners): # I think I need to invert the transform matrix + pattern_T_cam = np.linalg.inv(cam_T_pattern) + #Split into R and t matrices for the calibration function + pattern_R_cam = pattern_T_cam[0:3, 0:3] + pattern_t_cam = (pattern_T_cam[0:3, 3]).T + ################################################ # Get transform from base to gripper @@ -108,14 +114,20 @@ def cvHandEyeCalibrate(objp, dataset, camera, pattern, number_of_corners): # Hard coded for now, testin w/ one collection before iterating through all collections base_T_gripper = getTransform('base_link', 'flange', tmp_transforms) - # print(base_T_gripper) + + # Split into R and t matrices for the calibration function + base_R_gripper = base_T_gripper[0:3,0:3] + base_t_gripper = (base_T_gripper[0:3, 3]).T + ################################################ # Calibrate ################################################ + + cv2.calibrateRobotWorldHandEye(pattern_R_cam, pattern_t_cam, base_R_gripper, base_t_gripper) # THIS IS PRODUCING AN ERROR, SEE ISSUE #912 + exit(0) - cv2.calibrateRobotWorldHandEye() def getPatternConfig(dataset, pattern):