I read a lot of blogs in two weeks, tried the codes on the blogs and even the codes on github, various languages matlab, c++, python, learned a lot of platforms for hand-eye calibration - halcon, ros (these two still need Learn from scratch, not enough time), and finally saw the blog of Yuxiang ros, refer to it and summarize it completely, with a link
This blog only records the summary of the learning process, which can be used for reference. If you have any questions, you can point out and contact me
Table of contents
Principle of hand-eye calibration
The eye is on the hand, and the purpose of the eye on the hand is to find the transformation matrix X from the end to the camera, which also becomes the hand-eye matrix
The figure shows,
The pose of the calibration board in the coordinate system of the manipulator = the pose of the calibration board in the camera coordinate system —> the pose of the camera in the end coordinate system —> the pose of the end in the base coordinate system of the manipulator
base to end can be obtained through the kinematics of the mechanical arm
end to camera is interceding X
camera to object is obtained by taking a photo of the calibration board with the camera
As long as there are two sets of data, the identity can be expressed
Except for X, the remaining matrices can be obtained by the above methods. After transposing items through mathematical methods, we often say AX=XB
The next goal is to find X
Get the hand-eye matrix X
eyeinhandcalibrate.py
def get_matrix_eular_radu(x, y, z, rx, ry, rz):
rmat = tfs.euler.euler2mat(math.radians(rx), math.radians(ry), math.radians(rz))
rmat = tfs.affines.compose(np.squeeze(np.asarray((x, y, z))), rmat, [1, 1, 1])
return rmat
def skew(v):
return np.array([[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]])
def rot2quat_minimal(m):
quat = tfs.quaternions.mat2quat(m[0:3, 0:3])
return quat[1:]
def quatMinimal2rot(q):
p = np.dot(q.T, q)
w = np.sqrt(np.subtract(1, p[0][0]))
return tfs.quaternions.quat2mat([w, q[0], q[1], q[2]])
# hand = [1.1988093940033604, -0.42405585264804424, 0.18828251788562061, 151.3390418721659, -18.612399542280507,
# 153.05074895025035,
# 1.1684831621733476, -0.183273375514656, 0.12744868246620855, -161.57083804238462, 9.07159838346732,
# 89.1641128844487,
# 1.1508343174145468, -0.22694301453461405, 0.26625166858469146, 177.8815855486261, 0.8991159570568988,
# 77.67286224959672]
hand = [
# -0.05448,-0.15018,0.06552,89.61059916,-2.119943842,-1.031324031,
# -0.10149,-0.23025,0.04023,96.7725716,6.187944187,5.328507495,
# -0.10114,-0.2207,0.04853,97.00175472,5.729577951,1.375098708 毫米单位
-54.48, -150.18, 65.52, 89.61059916, -2.119943842, -1.031324031,
-101.49,-230.25, 40.23, 96.7725716, 6.187944187, 5.328507495,
-101.14,-220.7 , 48.53, 97.00175472, 5.729577951, 1.375098708
]
# camera = [-0.16249272227287292, -0.047310635447502136, 0.4077761471271515, -56.98037030812389, -6.16739631361851,
# -115.84333735802369,
# 0.03955405578017235, -0.013497642241418362, 0.33975949883461, -100.87129330834215, -17.192685528625265,
# -173.07354634882094,
# -0.08517949283123016, 0.00957852229475975, 0.46546608209609985, -90.85270962096058, 0.9315977976503153,
# 175.2059707654342]
camera = [
# -0.0794887,-0.0812433,0.0246,0.0008,0.0033,0.0182,
# -0.078034,-0.0879632,0.4881494,-0.1085,0.0925,-0.1569,
# -0.1086702,-0.0881681,0.4240367,-0.1052,0.1251,-0.1124,
-79.4887, -81.2433, 24.6, 0.0008, 0.0033, 0.0182,
-78.034, -87.9632, 488.1494, -0.1085, 0.0925, -0.1569,
-108.6702, -88.1681, 424.0367, -0.1052, 0.1251, -0.1124,
]
Hgs, Hcs = [], []
for i in range(0, len(hand), 6):
Hgs.append(get_matrix_eular_radu(hand[i], hand[i + 1], hand[i + 2], hand[i + 3], hand[i + 4], hand[i + 5],))
Hcs.append(
get_matrix_eular_radu(camera[i], camera[i + 1], camera[i + 2], camera[i + 3], camera[i + 4], camera[i + 5]))
Hgijs = []
Hcijs = []
A = []
B = []
size = 0
for i in range(len(Hgs)):
for j in range(i + 1, len(Hgs)):
size += 1
Hgij = np.dot(np.linalg.inv(Hgs[j]), Hgs[i])
Hgijs.append(Hgij)
Pgij = np.dot(2, rot2quat_minimal(Hgij))
Hcij = np.dot(Hcs[j], np.linalg.inv(Hcs[i]))
Hcijs.append(Hcij)
Pcij = np.dot(2, rot2quat_minimal(Hcij))
A.append(skew(np.add(Pgij, Pcij)))
B.append(np.subtract(Pcij, Pgij))
MA = np.asarray(A).reshape(size * 3, 3)
MB = np.asarray(B).reshape(size * 3, 1)
Pcg_ = np.dot(np.linalg.pinv(MA), MB)
pcg_norm = np.dot(np.conjugate(Pcg_).T, Pcg_)
Pcg = np.sqrt(np.add(1, np.dot(Pcg_.T, Pcg_)))
Pcg = np.dot(np.dot(2, Pcg_), np.linalg.inv(Pcg))
Rcg = quatMinimal2rot(np.divide(Pcg, 2)).reshape(3, 3)
A = []
B = []
id = 0
for i in range(len(Hgs)):
for j in range(i + 1, len(Hgs)):
Hgij = Hgijs[id]
Hcij = Hcijs[id]
A.append(np.subtract(Hgij[0:3, 0:3], np.eye(3, 3)))
B.append(np.subtract(np.dot(Rcg, Hcij[0:3, 3:4]), Hgij[0:3, 3:4]))
id += 1
MA = np.asarray(A).reshape(size * 3, 3)
MB = np.asarray(B).reshape(size * 3, 1)
Tcg = np.dot(np.linalg.pinv(MA), MB).reshape(3, )
print(tfs.affines.compose(Tcg, np.squeeze(Rcg), [1, 1, 1]))
Among them, hand is the pose of the base coordinates, which is generally obtained from the teach pendant. I use the ur5 robotic arm. Note that the units are mm and rad
Three rows of three sets of data, hand=(x, y, z, rx, ry, rz), and the radian system should be converted to the angle system
The input after the camera is the camera's external parameters (translation vector and rotation matrix),
Three rows of three sets of data, camera=(x, y, z, rx, ry, rz), and the radian system should be converted to the angle system
I use the matlab toolkit to calibrate the external parameters of the camera
Note: The obtained rotation vector should be converted into a rotation matrix, and then converted into Euler angles. For Python and matlab codes, see the link below
The output is the hand-eye calibration matrix X
[[ 9.97623261e-01 -4.70586261e-02 5.03320417e-02 9.72987830e+02]
[ 4.65612713e-02 9.98854765e-01 1.10094003e-02 -1.27118401e+03]
[-5.07924869e-02 -8.63971002e-03 9.98671857e-01 -4.29111524e+02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
Verify accuracy
According to the principle of hand-eye calibration,
Using the theoretical verification method, use two sets of data to combine them with an equation to obtain X
clc;
%T1*X*T2 = T3*X*T4 = T5*X*T6
%T1为相机到标定板,T2为基坐标到末端
T1 = [ 0.9998 -0.0182 0.0033 -79.4887
0.0182 0.9998 -0.0008 -81.2433
-0.0033 0.0009 1.0000 24.6000
0 0 0 1.0000 ];
T2 = [ -0.2681 -0.4478 -0.8530 -54.4800
-0.3725 -0.7684 0.5205 -150.1800
-0.8885 0.4572 0.0392 65.5200
0 0 0 1.0000];
T3 = [ 0.9835 0.1556 0.0924 -78.0340
-0.1652 0.9803 0.1078 -87.9632
-0.0738 -0.1213 0.9899 488.1494
0 0 0 1.0000 ];
T4 = [ 0.5753 0.8124 -0.0951 -101.4900
0.6340 -0.5163 -0.5758 -230.2500
-0.5169 0.2709 -0.8120 40.2300
0 0 0 1.0000 ] ;
T5 = [ 0.9859 0.1113 0.1248 -108.6702
-0.1246 0.9867 0.1042 -88.1681
-0.1115 -0.1183 0.9867 424.0367
0 0 0 1.0000 ] ;
T6 = [ 0.1654 -0.8344 -0.5258 -101.1400
-0.9468 0.0149 -0.3215 -220.7000
0.2761 0.5510 -0.7875 48.5300
0 0 0 1.0000
];
%X = [[ 0.997623261 -0.0470586261 0.0503320417 972.987830]
% [ 0.0465612713 0.998854765 0.0110094003 -1271.18401]
% [-0.0507924869 -0.00863971002 0.998671857 -429.111524]
% [ 0 0 0 1]]
%ans1 = T1*X*T2
%ans2 = T3*X*T4
%ans3 = T5*X*T6
T1*X*T2==T3*X*T4
X
The result is as follows
X =
1.0e+03 *
0.0010 -0.0000 0.0001 0.9730
0.0000 0.0010 0.0000 -1.2712
-0.0001 -0.0000 0.0010 -0.4291
0 0 0 0.0010