一个70张图片，每张的机器人位姿存在txt文件里面

'''
在执行手眼标定时，需要将标定板放置某一固定位置，并同时将相机固定在机械臂末端。
接着控制机械臂末端位于不同的位置，记录下此时机械臂相对于基座的位姿，并使用相机拍摄标定板上的棋盘格图像。
将图像放入./images文件夹中，并将位姿信息输入到chessboard_handeye_calibration.py文件的pose_vectors变量中。
最后运行chessboard_handeye_calibration.py，即可得到相机相对于机械臂末端的位姿矩阵。
'''

import cv2
import numpy as np
import transforms3d
import glob


def pose_vectors_to_end2base_transforms(pose_vectors):
    # 提取旋转矩阵和平移向量
    R_end2bases = []
    t_end2bases = []

    # 迭代遍历每个位姿的旋转矩阵和平移向量
    for pose_vector in pose_vectors:
        # 提取旋转矩阵和平移向量
        R_end2base = euler_to_rotation_matrix(pose_vector[3], pose_vector[4], pose_vector[5])
        t_end2base = pose_vector[:3]

        # 提取旋转矩阵和平移向量
        R_end2bases.append(R_end2base)
        t_end2bases.append(t_end2base)

    return R_end2bases, t_end2bases


def euler_to_rotation_matrix(rx, ry, rz, unit='deg'):  # rx, ry, rz是欧拉角，单位是度
    '''
    将欧拉角转换为旋转矩阵：R = Rz * Ry * Rx
    :param rx: x轴旋转角度
    :param ry: y轴旋转角度
    :param rz: z轴旋转角度
    :param unit: 角度单位，'deg'表示角度，'rad'表示弧度
    :return: 旋转矩阵
    '''
    if unit == 'deg':
        # 把角度转换为弧度
        rx = np.radians(rx)
        ry = np.radians(ry)
        rz = np.radians(rz)

    # 计算旋转矩阵Rz 、 Ry 、 Rx
    Rx = transforms3d.axangles.axangle2mat([1, 0, 0], rx)
    Ry = transforms3d.axangles.axangle2mat([0, 1, 0], ry)
    Rz = transforms3d.axangles.axangle2mat([0, 0, 1], rz)

    # 计算旋转矩阵R = Rz * Ry * Rx
    rotation_matrix = np.dot(Rz, np.dot(Ry, Rx))

    return rotation_matrix


#################### 输入 ##########################################################################################################
# 输入位姿数据，注意欧拉角是角度还是弧度
pose_file = './eye_in_hand_data_example/pose.txt'
pose_vectors = np.loadtxt(pose_file)

# 定义棋盘格参数
square_size = 25.0  # 假设格子的边长为30mm
pattern_size = (7, 9)  # 在这个例子中，假设标定板有9个内角点和6个内角点

# 导入相机内参和畸变参数
# 焦距 fx, fy, 光心 cx, cy
# 畸变系数 k1, k2
fx, fy, cx, cy = 1395.52898441919, 1394.87190020274, 971.157203214145, 532.584981394128
k1, k2 = 0, 0
K = np.array([[fx, 0, cx],
              [0, fy, cy],
              [0, 0, 1]], dtype=np.float64)  # K为相机内参矩阵
dist_coeffs = np.array([k1, k2, 0, 0], dtype=np.float64)  # 畸变系数

# 所有图像的路径
# 所有图像的路径
images = sorted(glob.glob('./eye_in_hand_data_example/*.png'),
                key=lambda x: int(x.split('\\')[-1].split('.')[0]))
###########################################################################################################################


# 准备位姿数据
obj_points = []  # 用于保存世界坐标系中的三维点
img_points = []  # 用于保存图像平面上的二维点

# 创建棋盘格3D坐标
objp = np.zeros((np.prod(pattern_size), 3), dtype=np.float32)
objp[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) * square_size

# 迭代处理图像
det_success_num = 0  # 用于保存检测成功的图像数量
for image in images:
    img = cv2.imread(image)  # 读取图像
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)  # RGB图像转换为灰度图像

    # 棋盘格检测
    ret, corners = cv2.findChessboardCorners(gray, pattern_size)

    if ret:
        det_success_num += 1
        # 如果成功检测到棋盘格，添加图像平面上的二维点和世界坐标系中的三维点到列表
        obj_points.append(objp)
        img_points.append(corners)

        # 绘制并显示角点
        cv2.drawChessboardCorners(img, pattern_size, corners, ret)
        cv2.namedWindow('img', cv2.WINDOW_NORMAL)
        cv2.resizeWindow('img', 640, 480)
        cv2.imshow('img', img)
        cv2.waitKey(500)

cv2.destroyAllWindows()

# # 打印obj_point和img_point的形状
# print(np.array(obj_points).shape)
# print(np.array(img_points).shape)


# 求解标定板位姿
R_board2cameras = []  # 用于保存旋转矩阵
t_board2cameras = []  # 用于保存平移向量
# 迭代的到每张图片相对于相机的位姿
for i in range(det_success_num):
    # rvec：标定板相对于相机坐标系的旋转向量
    # t_board2camera：标定板相对于相机坐标系的平移向量
    ret, rvec, t_board2camera = cv2.solvePnP(obj_points[i], img_points[i], K, dist_coeffs)

    # 将旋转向量(rvec)转换为旋转矩阵
    # R：标定板相对于相机坐标系的旋转矩阵
    R_board2camera, _ = cv2.Rodrigues(rvec)  # 输出：R为旋转矩阵和旋转向量的关系  输入：rvec为旋转向量

    # 将标定板相对于相机坐标系的旋转矩阵和平移向量保存到列表
    R_board2cameras.append(R_board2camera)
    t_board2cameras.append(t_board2camera)

# # 打印R_board2cameras和t_board2cameras的形状
# print(np.array(R_board2cameras).shape)
# print(np.array(t_board2cameras).shape)

# 求解手眼标定
# R_end2bases：机械臂末端相对于机械臂基座的旋转矩阵
# t_end2bases：机械臂末端相对于机械臂基座的平移向量
R_end2bases, t_end2bases = pose_vectors_to_end2base_transforms(pose_vectors)

# R_camera2end：相机相对于机械臂末端的旋转矩阵
# t_camera2end：相机相对于机械臂末端的平移向量
R_camera2end, t_camera2end = cv2.calibrateHandEye(R_end2bases, t_end2bases,
                                                  R_board2cameras, t_board2cameras,
                                                  method=cv2.CALIB_HAND_EYE_TSAI)

# 将旋转矩阵和平移向量组合成齐次位姿矩阵
T_camera2end = np.eye(4)
T_camera2end[:3, :3] = R_camera2end
T_camera2end[:3, 3] = t_camera2end.reshape(3)

# 输出相机相对于机械臂末端的旋转矩阵和平移向量
print("Camera to end rotation matrix:")
print(R_camera2end)
print("Camera to end translation vector:")
print(t_camera2end)

# 输出相机相对于机械臂末端的位姿矩阵
print("Camera to end pose matrix:")
np.set_printoptions(suppress=True)  # suppress参数用于禁用科学计数法
print(T_camera2end)

结果：

和参考值，还是差不多