matlab手眼标定

这里只罗列出可以用于手眼标定（相机坐标系和机械臂基坐标系之间的变换矩阵，3X4的。我的项目是eye to hand,所以得到的结果对于eye in hand怎么用还不太清楚）

原文链接：http://math.loyola.edu/~mili/Calibration/index.html

分为三种情况：

第一种：

你只想用几对同源点的空间三维位置坐标作为输入数据，得到变换矩阵。

方法1：论文：K.S. Arun, T.S. Huang, S.D. Blostein, Least-squares fitting of two 3-d point sets, IEEE Trans. Pattern Anal. Mach. Intell. 9 (5) (1987) 698-700

matlab代码：

function [R,t] = arun(A,B)
% Registers two sets of 3DoF data
% Assumes A and B are d,n sets of data
% where d is the dimension of the system 
% typically d = 2,3
% and n is the number of points
% typically n>3
%
% Mili Shah
% July 2014

[d,n]=size(A);

%Mean Center Data
Ac = mean(A')';
Bc = mean(B')';
A = A-repmat(Ac,1,n);
B = B-repmat(Bc,1,n);

%Calculate Optimal Rotation
[u,s,v]=svd(A*B');
R = v*u';
if det(R)<0, disp('Warning: R is a reflection'); end

%Calculate Optimal Translation
t = Bc - R*Ac;

方法2：论文：B.K.P. Horn, H.M. Hilden, S. Negahdaripour, Closed-form solution of absolute orientation using orthonormal matrices, J. Opt. Soc. Am. A 5 (1988) 1127-1135

代码：

function [R,t] = horn(A,B)
% Registers two sets of 3DoF data
% Assumes A and B are d,n sets of data
% where d is the dimension of the system 
% typically d = 2,3
% and n is the number of points
% typically n>3
%
% Mili Shah
% July 2014

[d,n]=size(A);

%Mean Center Data
Ac = mean(A')';
Bc = mean(B')';
A = A-repmat(Ac,1,n);
B = B-repmat(Bc,1,n);

%Calculate Optimal Rotation
M = B*A';
R = M*(M'*M)^(-1/2);
if det(R)<0, disp('Warning: R is a reflection'); end

%Calculate Optimal Translation
t = Bc - R*Ac;
end

方法3：

• B.K.P. Horn, Closed-form solution of absolute orientation using unit quaternions, J. Opt. Soc. Am. A 4 (1987) 629-642.

function [R,t] = hornQuar(A,B)
% Registers two sets of 3DoF data
% Assumes A and B are d,n sets of data
% where d is the dimension of the system 
% typically d = 2,3
% and n is the number of points
% typically n>3
%
% Mili Shah
% July 2014

[d,n]=size(A);

%Mean Center Data
Ac = mean(A')';
Bc = mean(B')';
A = A-repmat(Ac,1,n);
B = B-repmat(Bc,1,n);

%Calculate Optimal Rotation
M = A*B';
N = [M(1,1) + M(2,2) + M(3,3) M(2,3)-M(3,2) M(3,1)-M(1,3) M(1,2)-M(2,1);...
    M(2,3)-M(3,2) M(1,1)-M(2,2)-M(3,3) M(1,2)+M(2,1) M(3,1)+M(1,3);...
    M(3,1)-M(1,3) M(1,2)+M(2,1) -M(1,1)+M(2,2)-M(3,3) M(2,3)+M(3,2);...
    M(1,2)-M(2,1) M(3,1)+M(1,3) M(2,3)+M(3,2) -M(1,1)-M(2,2)+M(3,3)];
[u,v]=eig(N);
[m,ind]=max(diag(v));
r = u(:,ind);
r = [r(2:4);r(1)];
R = (r(4)^2-r(1:3)'*r(1:3))*eye(3) + 2*r(1:3)*r(1:3)'+2*r(4)*[0 -r(3) r(2);r(3) 0 -r(1);-r(2) r(1) 0];
%Calculate Optimal Translation
t = Bc - R*Ac;
end

方法4：M.W. Walker, L. Shao, R.A. Volz, Estimating 3-d location parameters using dual number quaternions, CVGIP: Image Underst. 54 (3) (1991) 358-367.

function [R,t] = walker(A,B)
% Registers two sets of 3DoF data
% Assumes A and B are 3,n sets of data
% and n is the number of points
% typically n>3
%
% Mili Shah
% July 2014

[d,n]=size(A);

C1 = zeros(4,4);
C2 = n*eye(4);
C3 = zeros(4,4);
for i = 1:n
    C1 = C1 + -2*Q([1/2*B(:,i);0])'*W([1/2*A(:,i);0]);
    C3 = C3 + 2*(W([1/2*A(:,i);0])-Q([1/2*B(:,i);0]));
end
A = 1/2*(C3'*inv(C2+C2')*C3-C1-C1');
[u,v]=eig(A);
[m,ind]=max(diag(v));
r = u(:,ind);
s = -inv(C2+C2')*C3*r;
R = (r(4)^2-r(1:3)'*r(1:3))*eye(3) + 2*r(1:3)*r(1:3)'+2*r(4)*[0 -r(3) r(2);r(3) 0 -r(1);-r(2) r(1) 0];
t = W(r)'*s*2;
t = t(1:3);
end

function Q = Q(r)
K = [0 -r(3) r(2);r(3) 0 -r(1);-r(2) r(1) 0];
Q = [r(4)*eye(3)+K r(1:3);-r(1:3)' r(4)];
end
function W = W(r)
K = [0 -r(3) r(2);r(3) 0 -r(1);-r(2) r(1) 0];
W = [r(4)*eye(3)-K r(1:3);-r(1:3)' r(4)];
end

方法5：S. Umeyama, Least-squares estimation of transformation parameters between two point patterns, IEEE Trans. Pattern Anal. Mach. Intell. 13 (4) (1991) 376- 380.

 

function [R,t] = umeyama(A,B)
% Registers two sets of 3DoF data
% Assumes A and B are d,n sets of data
% where d is the dimension of the system 
% typically d = 2,3
% and n is the number of points
% typically n>3
%
% Mili Shah
% July 2014

[d,n]=size(A);

%Mean Center Data
Ac = mean(A')';
Bc = mean(B')';
A = A-repmat(Ac,1,n);
B = B-repmat(Bc,1,n);

%Calculate Optimal Rotation
[u,s,v]=svd(A*B');
R = v*diag([1 1 det(v*u')])*u';

%Calculate Optimal Translation
t = Bc - R*Ac;

第二种情况：输入的数据为位置和姿态，解方程AX=XB

方法1：

• Y. Shiu, S. Ahmad Calibration of Wrist-Mounted Robotic Sensors by Solving Homogeneous Transform Equations of the Form AX = XB. In IEEE Transactions on Robotics and Automation, 5(1):16-29, 1989.

function X = shiu(A,B,X)
% Calculates the least squares solution of
% AX = XB
% From
% Calibration of Wrist-Mounted Robotic Sensors 
% by Solving Homogeneous Transform Equations of the Form AX = XB
% Shiu and Ahmad
%
% Mili Shah
% July 2014

[m,n] = size(A); n = n/4;
AA = zeros(9*(n-1),2*n);
bb = zeros(9*(n-1),1);
%Calculate best rotation R
for i = 1:n
    A1 = logm(A(1:3,4*i-3:4*i-1));
    B1 = logm(B(1:3,4*i-3:4*i-1));
    a1 = [A1(3,2) A1(1,3) A1(2,1)]'; a1 = a1/norm(a1);
    b1 = [B1(3,2) B1(1,3) B1(2,1)]'; b1 = b1/norm(b1);
    v = cross(b1,a1);
    w = atan2(norm(v),b1'*a1);
    v = v/norm(v);
    XP = (eye(3)*cos(w) + sin(w)*skew(v) + (1-cos(w))*v*v');
    [Ai,bi] = shiuMatrix(a1,XP);
    if i == 1
        AA(:,1:2) = repmat(-Ai,n-1,1);
        bb(:,1) = repmat(-bi,n-1,1);
    else
        AA(9*(i-2)+1:9*(i-1),2*i-1:2*i) = Ai;
        bb(9*(i-2)+1:9*(i-1),1) = bb(9*(i-2)+1:9*(i-1),1) + bi;
    end
end
beta = AA\bb;
theta = atan2(beta(2*n),beta(2*n-1));
RA = (eye(3)*cos(theta) + sin(theta)*skew(a1) + (1-cos(theta))*a1*a1');
R = RA*XP;
%Calculate best translation t
C = zeros(3*n,3);
d = zeros(3*n,1);
I = eye(3);
for i = 1:n
    C(3*i-2:3*i,:) = I - A(1:3,4*i-3:4*i-1);
    d(3*i-2:3*i,:) = A(1:3,4*i)-R*B(1:3,4*i);
end
t = C\d;
%Put everything together to form X
X = [R t;0 0 0 1];
end

function [A,b] = shiuMatrix(ka1,X)
A = zeros(9,2);
b = zeros(9,1);


A(1,1) = X(1,1)-ka1(1)*X(:,1)'*ka1;
A(2,1) = X(1,2)-ka1(1)*X(:,2)'*ka1;
A(3,1) = X(1,3)-ka1(1)*X(:,3)'*ka1;

A(4,1) = X(2,1)-ka1(2)*X(:,1)'*ka1;
A(5,1) = X(2,2)-ka1(2)*X(:,2)'*ka1;
A(6,1) = X(2,3)-ka1(2)*X(:,3)'*ka1;

A(7,1) = X(3,1)-ka1(3)*X(:,1)'*ka1;
A(8,1) = X(3,2)-ka1(3)*X(:,2)'*ka1;
A(9,1) = X(3,3)-ka1(3)*X(:,3)'*ka1;

n = cross(X(:,1),ka1);
o = cross(X(:,2),ka1);
a = cross(X(:,3),ka1);

A(1,2) = -n(1);
A(2,2) = -o(1);
A(3,2) = -a(1);

A(4,2) = -n(2);
A(5,2) = -o(2);
A(6,2) = -a(2);

A(7,2) = -n(3);
A(8,2) = -o(3);
A(9,2) = -a(3);

n = X(:,1);
o = X(:,2);
a = X(:,3);

b(1) = -ka1(1)*n'*ka1;
b(2) = -ka1(1)*o'*ka1;
b(3) = -ka1(1)*a'*ka1;

b(4) = -ka1(2)*n'*ka1;
b(5) = -ka1(2)*o'*ka1;
b(6) = -ka1(2)*a'*ka1;

b(7) = -ka1(3)*n'*ka1;
b(8) = -ka1(3)*o'*ka1;
b(9) = -ka1(3)*a'*ka1;
end

方法2：

• R. Tsai, R. Lenz A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration. In IEEE Transactions on Robotics and Automation, 5(3):345-358, 1989.

 

function X = tsai(A,B)
% Calculates the least squares solution of
% AX = XB
% 
% A New Technique for Fully Autonomous 
% and Efficient 3D Robotics Hand/Eye Calibration
% Lenz Tsai
%
% Mili Shah
% July 2014

[m,n] = size(A); n = n/4;
S = zeros(3*n,3);
v = zeros(3*n,1);
%Calculate best rotation R
for i = 1:n
    A1 = logm(A(1:3,4*i-3:4*i-1)); 
    B1 = logm(B(1:3,4*i-3:4*i-1));
    a = [A1(3,2) A1(1,3) A1(2,1)]'; a = a/norm(a);
    b = [B1(3,2) B1(1,3) B1(2,1)]'; b = b/norm(b);
    S(3*i-2:3*i,:) = skew(a+b);
    v(3*i-2:3*i,:) = a-b;
end
x = S\v;
theta = 2*atan(norm(x));
x = x/norm(x);
R = (eye(3)*cos(theta) + sin(theta)*skew(x) + (1-cos(theta))*x*x')';
%Calculate best translation t
C = zeros(3*n,3);
d = zeros(3*n,1);
I = eye(3);
for i = 1:n
    C(3*i-2:3*i,:) = I - A(1:3,4*i-3:4*i-1);
    d(3*i-2:3*i,:) = A(1:3,4*i)-R*B(1:3,4*i);
end
t = C\d;
%Put everything together to form X
X = [R t;0 0 0 1];

方法3：

• C. Wang Extrinsic Calibration of a Vision Sensor Mounted on a Robot. In IEEE Transactions on Robotics and Automation, 8(2): 161-175, 1992.

function X = wang(A,B)
% Calculates the least squares solution of
% AX = XB
% From
% Extrinsic Calibration of a Vision Sensor
% Mounted on a Robot.
% C. Wang
% 
% Mili Shah
% July 2014


[m,n] = size(A); n = n/4;
K = zeros(3,n-1);
theta = zeros(1,n-1);

%Calculate best rotation R
for i = 1:n-1
    A1 = logm(A(1:3,4*i-3:4*i-1)); 
    B1 = logm(B(1:3,4*i-3:4*i-1));
    A2 = logm(A(1:3,4*(i+1)-3:4*(i+1)-1)); 
    B2 = logm(B(1:3,4*(i+1)-3:4*(i+1)-1));
    a1 = [A1(3,2) A1(1,3) A1(2,1)]'; a1 = a1/norm(a1);
    b1 = [B1(3,2) B1(1,3) B1(2,1)]'; b1 = b1/norm(b1);
    a2 = [A2(3,2) A2(1,3) A2(2,1)]'; a2 = a2/norm(a2);
    b2 = [B2(3,2) B2(1,3) B2(2,1)]'; b2 = b2/norm(b2);
    
    if abs(a1'*b1)<.999 && abs(a2'*b2)<.999 && norm(cross((a1-b1),(a2-b2)))~=0
        r = cross((a1-b1),(a2-b2)); r = r/norm(r);
        theta(i) = sign((a1-b1)'*cross(r,a1+b1))*atan(norm(a1-b1)/norm(cross(r,a1+b1)))+...
            sign((a2-b2)'*cross(r,a2+b2))*atan(norm(a2-b2)/norm(cross(r,a2+b2)));
        K(:,i) = r;
    elseif abs(a1'*b1)<.999 && abs(a2'*b2)<.999 && norm(cross((a1-b1),(a2-b2)))==0
        r = -cross(cross(a1,a2),cross(b1,b2)); r = r/norm(r);
        theta(i) = acos(cross(a1,a2)'*cross(b1,b2));
        K(:,i) = r;
    elseif abs(a1'*b1)>.999 && abs(a2'*b2)<.999 
        r = (a1+b1)/2; r = r/norm(r);
        theta(i) = 2*sign((a2-b2)'*cross(r,a2+b2))*atan(norm(a2-b2)/norm(cross(r,a2+b2)));
        K(:,i) = r;
    elseif abs(a1'*b1)<.999 && abs(a2'*b2)>.999 
        r = (a2+b2)/2; r = r/norm(r);
        theta(i) = 2*sign((a1-b1)'*cross(r,a1+b1))*atan(norm(a1-b1)/norm(cross(r,a1+b1)));
        K(:,i) = r;
    else
        r = [1;0;0];
        theta(i) = 0;
        K(:,i) = r;
    end
end
K = [K;theta];
r = mean(K')'; sc = norm(r(1:3)); r = r/sc;
R = eye(3)*cos(r(4)) + (1-cos(r(4)))*r(1:3)*r(1:3)' + skew(r(1:3))*sin(r(4));
%Calculate best translation t
C = zeros(3*n,3);
d = zeros(3*n,1);
I = eye(3);
for i = 1:n
    C(3*i-2:3*i,:) = I - A(1:3,4*i-3:4*i-1);
    d(3*i-2:3*i,:) = A(1:3,4*i)-R*B(1:3,4*i);
end
t = C\d;
%Put everything together to form X
X = [R t;0 0 0 1];

方法4：

• R. Liang, J. Mao Hand-Eye Calibration with a New Linear Decomposition Algorithm. In Journal of Zhejiang University, 9(10):1363-1368, 2008.

 

function [X]=liang(AA,BB)
% Solves the problem AX=XB
% using the formulation of
%
% Hand-Eye Calibration with a 
% New Linear Decomposition Algorithm
% R. Liang, J. Mao 
%
% Mili Shah
% July 2014

[m,n]=size(AA); n = n/4;

A = zeros(9*n,9);
b = zeros(9*n,1);
for i = 1:n
    Ra = AA(1:3,4*i-3:4*i-1);
    Rb = BB(1:3,4*i-3:4*i-1);
    A(9*i-8:9*i,:) = [kron(Ra,eye(3))+kron(-eye(3),Rb')];
end
[u,s,v]=svd(A);
x = v(:,end);
R=reshape(x(1:9),3,3)';
R = sign(det(R))/abs(det(R))^(1/3)*R;
[u,s,v]=svd(R); R = u*v'; if det(R)<0, R = u*diag([1 1 -1])*v'; end
C = zeros(3*n,3);
d = zeros(3*n,1);
I = eye(3);
for i = 1:n
    C(3*i-2:3*i,:) = I - AA(1:3,4*i-3:4*i-1);
    d(3*i-2:3*i,:) = AA(1:3,4*i)-R*BB(1:3,4*i);
end
t = C\d;
%Put everything together to form X
X = [R t;0 0 0 1];

 第三种情况：输入数据为位置和姿态，求解方程AX=YB

方法1：

• A. Li, L. Wang, D. Wu Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product. InInternational Journal of the Physical Sciences, 5(10): 1530-1536, 1995.

 

function [X,Y]=li(AA,BB)
% Solves the problem AX=YB
% using the formulation of
%
% Simultaneous robot-world and hand-eye calibration 
% using dual-quaternions and Kronecker product
% 
% Aiguo Li, Lin Wang and Defeng Wu
%
% Mili Shah
% July 2014

[m,n]=size(AA); n = n/4;

A = zeros(12*n,24);
b = zeros(12*n,1);
for i = 1:n
    Ra = AA(1:3,4*i-3:4*i-1);
    Rb = BB(1:3,4*i-3:4*i-1);
    ta = AA(1:3,4*i);
    tb = BB(1:3,4*i);
    A(12*i-11:12*i-3,1:18) = [kron(Ra,eye(3)) kron(-eye(3),Rb')];
    A(12*i-2:12*i,10:24) = [kron(eye(3),tb') -Ra eye(3)];
    b(12*i-2:12*i) = ta;
end
x = A\b;

X=reshape(x(1:9),3,3)';
[u,s,v]=svd(X); X = u*v'; if det(X)<0, X = u*diag([1 1 -1])*v'; end
X = [X x(19:21);[0 0 0 1]];
Y=reshape(x(10:18),3,3)';
[u,s,v]=svd(Y); Y = u*v'; if det(Y)<0, Y = u*diag([1 1 -1])*v'; end
Y = [Y x(22:24);[0 0 0 1]];

方法2：

• M. Shah, Solving the Robot-World/Hand-Eye Calibration Problem Using the Kronecker Product, ASME Journal of Mechanisms and Robotics, Volume 5, Issue 3 (2013).

function [X,Y]=shah(AA,BB)
% Solves the problem AX=YB
% using the formulation of
%
% Simultaneous Robot/World and Tool/Flange 
% Calibration by Solving Homogeneous Transformation 
% Equations of the form AX=YB
% M. Shah
%
% Mili Shah
% July 2014

[m,n]=size(AA); n = n/4;
A = zeros(9*n,18);
T = zeros(9,9);
b = zeros(9*n,1);
for i = 1:n
    Ra = AA(1:3,4*i-3:4*i-1);
    Rb = BB(1:3,4*i-3:4*i-1);
    T = T + kron(Rb,Ra);
end
[u,s,v]=svd(T);
x = v(:,1);
y = u(:,1);
X = reshape(x,3,3);
X = sign(det(X))/abs(det(X))^(1/3)*X;
[u,s,v]=svd(X); X = u*v';
Y = reshape(y,3,3);
Y = sign(det(Y))/abs(det(Y))^(1/3)*Y;
[u,s,v]=svd(Y); Y = u*v';
A = zeros(3*n,6);
b = zeros(3*n,1);
for i = 1:n
    A(3*i-2:3*i,:) = [-AA(1:3,4*i-3:4*i-1) eye(3)];
    b(3*i-2:3*i,:) = AA(1:3,4*i) - kron(BB(1:3,4*i)',eye(3))*reshape(Y,9,1);
end
t = A\b;

X = [X t(1:3);[0 0 0 1]];
Y = [Y t(4:6);[0 0 0 1]];