The code for hand-to-eye calibration.
Created by Wei Yu on April 30th, 2019.
0 The end pose of the robot and the external parameters of the corresponding image at an initial position.
0.1 End pose of the robot.
That is the description of the end coordinate in the robot base coordinate.
% The translation vector in mm. % Here tx0 means the translation component along x axis at the initial position. tx0 = 937; ty0 = -94; tz0 = 117; % The rotation matrix. rotation0 = [-0.9337 0.1022 0.3433; ... 0.0971 0.9948 -0.0321; ... -0.3448 0.0034 -0.9387]; % The homogeneous transformation matrix. t0 = [rotation0 [tx0; ty0; tz0]; 0 0 0 1];
0.2 External parameters of the corresponding image.
That is the description of the camera coordinate in the world coordinate.
% The translation vector in mm. % Here etx0 means the translation component along x axis at the initial position. etx0 = 73.8203; ety0 = 161.9148; etz0 = 956.5806; % The rotation vector in rad. rvec0 = [-0.1828 -0.2717 -3.0922]; % The rotation matrix. % Tip: the customized function Rodrigues converts a rotation vector to a rotation matrix. erotation0 = Rodrigues(rvec0); % The homogeneous transformation matrix. et0 = [erotation0 [etx0; ety0; etz0]; 0 0 0 1];
PS. Rodrigues formula:
A rotation vector to a rotation matrix:
Conversely, a rotation matrix to a rotation vector:
1 The end pose of the robot and the external parameters of the corresponding image at the first position.
1.1 End pose of the robot.
That is the description of the end coordinate in the robot base coordinate.
% The translation vector in mm. % Here tx1 means the translation component along x axis at the first position. tx1 = 877; ty1 = -139; tz1 = 218; % The rotation matrix. rotation1 = [-0.9585 0.0277 0.2837; ... 0.0409 0.9983 0.0405; ... -0.2821 0.0504 -0.9581]; % The homogeneous transformation matrix. t1 = [rotation1 [tx1; ty1; tz1]; 0 0 0 1];
1.2 External parameters of the corresponding image.
That is the description of the camera coordinate in the world coordinate.
% The translation vector in mm. % Here etx1 means the translation component along x axis at the first position. etx1 = 40.3038; ety1 = 54.3779; etz1 = 1013.6; % The rotation vector in rad. rvec1 = [0.0999 0.1997 3.1017]; % The rotation matrix. % Tip: the customized function Rodrigues converts a rotation vector to a rotation matrix. erotation1 = Rodrigues(rvec1); % The homogeneous transformation matrix. et1 = [erotation1 [etx1; ety1; etz1]; 0 0 0 1];
2 The end pose of the robot and the external parameters of the corresponding image at the second position.
2.1 End pose of the robot.
That is the description of the end coordinate in the robot base coordinate.
% The translation vector in mm. % Here tx2 means the translation component along x axis at the second position. tx2 = 870; ty2 = -276; tz2 = 133; % The rotation matrix. rotation2 = [-0.9788 -0.0996 0.1787; ... -0.1008 0.9949 0.0027; ... -0.1780 -0.0153 -0.9839]; % The homogeneous transformation matrix. t2 = [rotation2 [tx2; ty2; tz2]; 0 0 0 1];
2.2 External parameters of the corresponding image.
That is the description of the camera coordinate in the world coordinate.
% The translation vector in mm. % Here etx2 means the translation component along x axis at the second position. etx2 = -92.7295; ety2 = 141.0039; etz2 = 1059.3; % The rotation vector in rad. rvec2 = [0.0705 -0.0098 -3.1281]; % The rotation matrix. % Tip: the customized function Rodrigues converts a rotation vector to a rotation matrix. erotation2 = Rodrigues(rvec2); % The homogeneous transformation matrix. et2 = [erotation2 [etx2; ety2; etz2]; 0 0 0 1];
3 Transformation matrices of the end of robot and the camera from a position to another position.
3.1 From the initial position to the first position.
% Transformation matrix of the end of robot. % For efficiency, use "end1 = t0/t1" instead. end1 = t0 * inv(t1) ; % Transformation matrix of the camera. % For efficiency, use "camera1 = et0/et1" instead. camera1 = et0 * inv(et1);
3.2 From the first position to the second position.
% Transformation matrix of the end of robot. % For efficiency, use "end2 = t1/t2" instead. end2 = t1 * inv(t2); % Transformation matrix of the camera. % For efficiency, use "camera2 = et1/et2" instead. camera2 = et1 * inv(et2);
4 Calculation procedure of the hand-eye matrix.
4.1 Firstly, calculate the rotation component of the hand-eye matrix.
% Get rotation matrices from transformation matrices. r_end1 = end1(1:3, 1:3); r_camera1 = camera1(1:3, 1:3); r_end2 = end2(1:3, 1:3); r_camera2 = camera2(1:3, 1:3); % Find axes of rotation of rotation matrices. % Here kc means the axis of rotation of the camera transformation, kd is for the end transformation. q = acos((trace(r_camera1) - 1) / 2); kc1(1, 1) = q / (2 * sin(q)) * (r_camera1(3, 2) - r_camera1(2, 3)); kc1(2, 1) = q / (2 * sin(q)) * (r_camera1(1, 3) - r_camera1(3, 1)); kc1(3, 1) = q / (2 * sin(q)) * (r_camera1(2, 1) - r_camera1(1, 2)); q = acos((trace(r_camera2) - 1) / 2); kc2(1, 1) = q / (2 * sin(q)) * (r_camera2(3, 2) - r_camera2(2, 3)); kc2(2, 1) = q / (2 * sin(q)) * (r_camera2(1, 3) - r_camera2(3, 1)); kc2(3, 1) = q / (2 * sin(q)) * (r_camera2(2, 1) - r_camera2(1, 2)); q = acos((trace(r_end1) - 1) / 2); kd1(1, 1) = q / (2 * sin(q)) * (r_end1(3, 2) - r_end1(2, 3)); kd1(2, 1) = q / (2 * sin(q)) * (r_end1(1, 3) - r_end1(3, 1)); kd1(3, 1) = q / (2 * sin(q)) * (r_end1(2, 1) - r_end1(1, 2)); q = acos((trace(r_end2) - 1) / 2); kd2(1, 1) = q / (2 * sin(q)) * (r_end2(3, 2) - r_end2(2, 3)); kd2(2, 1) = q / (2 * sin(q)) * (r_end2(1, 3) - r_end2(3, 1)); kd2(3, 1) = q / (2 * sin(q)) * (r_end2(2, 1) - r_end2(1, 2)); % Calculate the rotation component of the hand-eye matrix. % For efficiency, use "rotation = kc/kd" instead. kc3 = cross(kc1, kc2); kd3 = cross(kd1, kd2); kc = [kc1 kc2 kc3]; kd = [kd1 kd2 kd3]; rotation = kc * inv(kd);
4.2 Secondly, calculate the translation component of the hand-eye matrix.
% Get translation vectors from transformation matrices. t_end1 = end1(1:3, end); t_camera1 = camera1(1:3, end); t_end2 = end2(1:3, end); t_camera2 = camera2(1:3, end); % Calculate the translation component of the hand-eye matrix. % For efficiency, use translation = (h'*h)/h'*y instead. c = rotation * t_end1 - t_camera1; d = rotation * t_end2 - t_camera2; a = r_camera1 - [1 0 0; 0 1 0; 0 0 1]; b = r_camera2 - [1 0 0; 0 1 0; 0 0 1]; h = [a; b]; y = [c; d]; translation = inv(h' * h) * h' * y; handeye_matrix = [rotation translation; 0 0 0 1];
5. Print the result hand-eye matrix.
% Set print format. % Here 'g' means abolishing the scientific counting method. format long g % Print the result hand-eye matrix. disp('The description of the base coordinate in the camera coordinate, i.e. cameraHbase:')
The description of the base coordinate in the camera coordinate, i.e. cameraHbase:
disp(handeye_matrix)
disp('The description of the camera coordinate in the base coordinate, i.e. baseHcamera:')
The description of the camera coordinate in the base coordinate, i.e. baseHcamera:
disp(inv(handeye_matrix))
