RoboticArmVisionModule/eye_to_hand.py at master · Eastshann/RoboticArmVisionModule · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
import cv2
import numpy as np
from utils import *

#------------------------#
#    1. 棋盘格参数设置
#------------------------#
square_size = 0.02      # 单位：米 (每格实际物理长度)
board_size = (11, 8)    # 棋盘格内角点数量 (宽11高8个内角点)

# 每个棋盘格角点的三维坐标 (世界坐标，Z=0)
object_points = create_points(board_size, square_size)


#---------------------------------------------------------#
#   2. 预设的相机内参与畸变参数 (假设已通过相机标定获得)
#---------------------------------------------------------#
camera_matrix = np.array([
    [1413.61372, 0.0, 928.151939],
    [0.0, 1410.14663, 550.316444],
    [0.0, 0.0, 1.0]
], dtype=np.float64)

dist_coeffs = np.array(
    [0.1990143, -0.65770062, -0.00158559, -0.00169318, 0.75776742],
    dtype=np.float64
)


#------------------------------------------#
#   3. 模拟采集的机器人 XYZWPR 位姿数据
#------------------------------------------#
xyzwpr_data = [
    [0.3014, -0.0406, 0.4012, 468, -59.04, 490],
    [0.2862, 0.0240, 0.4109, 520, -56.16, 492],
    [0.2308, 0.1520, 0.4099, 639, -60.96, 492],

    [0.2559, 0.1178, 0.3326, 603, -62.63, 492],
    [0.2839, -0.0130, 0.3274, 489, -67.67, 492],
    [0.2790, 0.0435, 0.3258, 537, -58.55, 511],

    [0.2545, -0.0574, 0.3658, 447, -66.72, 511],
    [0.2708, 0.0792, 0.3271, 568, -58.55, 549],
    [0.2202, 0.1346, 0.3728, 631, -70.08, 493],

    [0.2339, 0.0176, 0.3615, 518, -56.88, 493],
    [0.2063, 0.0767, 0.3910, 585, -66.00, 492],
    [0.1889, 0.1144, 0.3889, 630, -65.52, 492],

    [0.2687, 0.0237, 0.3598, 521, -71.75, 493],
    [0.2393, 0.1041, 0.3733, 598, -74.40, 493],
    [0.2852, -0.0323, 0.3270, 473, -65.51, 541]
]

# 存储机器人和相机的变换数据
R_gripper2base = [] # 机器人基座到末端旋转矩阵
t_gripper2base = [] # 机器人基座到末端平移向量
R_target2cam = []   # 相机坐标系下的棋盘格旋转矩阵
t_target2cam = []   # 相机坐标系下的棋盘格平移向量


#----------------------#
#   4. 逐组数据处理
#----------------------#
for i in range(0,15):
    image_path = f"./eye_to_hand/{i+1}.jpg" # 图片路径

    # 末端到基底的旋转平移矩阵 (从XYZWPR转换)

    xyzwpr_data[i][3] = control_value_to_angle(xyzwpr_data[i][3])
    xyzwpr_data[i][5] = control_value_to_angle(xyzwpr_data[i][5])

    Rg, tg = convert_XYZWPR_to_mat(*xyzwpr_data[i])
    # 注意手在眼外，需要求逆。g2b -> b2g
    R_b2g = Rg.T  # 旋转矩阵逆就是转置
    t_b2g = - Rg.T @ tg


    R_gripper2base.append(R_b2g)
    t_gripper2base.append(t_b2g)

    # 标定板到相机的旋转平移矩阵
    Rtc, tvec = get_target2cam(image_path, board_size, object_points, camera_matrix, dist_coeffs)

    if Rtc is not None:
        # 注意眼在手外，需要求逆
        # R_c2b = Rtc.T
        # t_c2b = - Rtc.T @ tvec

        R_target2cam.append(Rtc)
        t_target2cam.append(tvec)
    else:
        input("error")


#---------------------#
#   5. 执行手眼标定
#---------------------#
# 方法选择 Tsai（常用）
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,
    R_target2cam, t_target2cam,
    method=cv2.CALIB_HAND_EYE_TSAI
)


#-------------------------------------------------------#
#   6. 输出最终手眼变换矩阵 (即相机在机器人末端下的位姿)
#-------------------------------------------------------#
X = np.eye(4)
X[:3, :3] = R_cam2gripper
X[:3, 3] = t_cam2gripper.flatten()

print("相机到基座的变换矩阵 X =\n", X)