cv2.CalibrateHandEye 中的结果错误

Question

我正在尝试校准手眼机械臂和摄像头系统。我正在使用 cv2.CalibrateHandEye 函数来计算机器人底座和相机之间的变换矩阵。但我只得到错误的结果。

相机已校准，我使用 cv2.SolvePnp 函数从相机拍摄的图像中获取标记的平移和旋转向量，并使用 cv2.Rodrigues 将旋转向量转换为旋转矩阵。

根据末端执行器的旋转，使用tfs.euler.euler2mat生成末端的旋转矩阵。

我多次检查了这两个矩阵，我认为它们是正确的，但是 cv2.CalibrateHandeye 函数只是保留了甚至不接近真实值的答案。

这是一些代码。

我记录了大约 20 组图像和最终姿势，下面的代码是我如何从每组图像中提取矩阵

(success, rvec, tvec) = cv2.solvePnP(np.array(point_3d), np.array(corners_2d), mtx, dist,flags=cv2.SOLVEPNP_ITERATIVE)
R_board_in_camera = cv2.Rodrigues(rvec)[0]
T_board_in_camera = tvec
H_board_in_camera = np.zeros((4, 4), np.float)
H_board_in_camera[:3, :3] = R_board_in_camera
H_board_in_camera[:3, 3] = np.array(T_board_in_camera).flatten()
H_board_in_camera[3, 3] = 1

R_hand_in_base = tfs.euler.euler2mat(math.radians(angle_x), math.radians(angle_y), math.radians(angle_z), axes='rxyz')
T_hand_in_base = np.array([x,y,z])  # calculated in advance

H_hand_in_base = np.zeros((4, 4), np.float)
H_hand_in_base[:3, :3] = R_hand_in_base
H_hand_in_base[:3, 3] = T_hand_in_base.flatten()
H_hand_in_base[3, 3] = 1

获得所有矩阵后，我使用校准功能

n = len(Ts_hand_to_base)
R_base_to_hand = []
T_base_to_hand = []
R_board_to_camera = []
T_board_to_camera = []

for i in range(n):
    Ts_base_to_hand = np.linalg.inv(Ts_hand_to_base[i])
    R_base_to_hand.append(np.array(Ts_base_to_hand[:3, :3]))
    T_base_to_hand.append(np.array(Ts_base_to_hand[:3, 3]))
    R_board_to_camera.append(np.array(Ts_board_to_camera[i][0, :3]))
    T_board_to_camera.append(np.array(Ts_board_to_camera[i][:3, 3]))

R_camera_to_base, T_camera_to_base = cv2.calibrateHandEye(R_base_to_hand, T_base_to_hand, R_board_to_camera,T_board_to_camera, method=cv2.CALIB_HAND_EYE_DANIILIDIS)

这是我得到的一些结果。

# method = cv2.CALIB_HAND_EYE_HORAUD
H_camera_to_base:
 [[  1.    -0.01  -0.04  13.54]
 [  0.    -0.96   0.29 141.48]
 [ -0.04  -0.29  -0.96   0.  ]
 [  0.     0.     0.     1.  ]]

method = cv2.CALIB_HAND_EYE_HORAUD
 [[  -0.22   -0.98   -0.01  186.04]
 [  -0.95    0.21    0.23 -187.49]
 [  -0.22    0.05   -0.97  782.4 ]
 [   0.      0.      0.      1.  ]]

method = cv2.CALIB_HAND_EYE_TSAI
 [[ 0.51 -0.3   0.8  64.88]
 [-0.54  0.62  0.57 69.03]
 [-0.67 -0.72  0.15  0.  ]
 [ 0.    0.    0.    1.  ]]

method = CALIB_HAND_EYE_ANDREFF
error (-7:Iterations do not converge) Rotation normalization issue: determinant(R) is null in function 'normalizeRotation'

method = CALIB_HAND_EYE_PARK
 [[nan nan nan nan]
 [nan nan nan nan]
 [nan nan nan nan]
 [ 0.  0.  0.  1.]]

Answer 1

您是否找到了解决上述问题的方法？我也面临着同样的问题。