Skip to content

Commit

Permalink
hand-eye code
Browse files Browse the repository at this point in the history
  • Loading branch information
kpach committed May 25, 2017
0 parents commit 36db4b4
Show file tree
Hide file tree
Showing 24 changed files with 2,399 additions and 0 deletions.
96 changes: 96 additions & 0 deletions HandEye.m
Original file line number Diff line number Diff line change
@@ -0,0 +1,96 @@
function [XL, XR] = HandEye(camera_pose, robot_pose, FLAG_STEREO, FLAG_ALGO, stereoT)
N = size(camera_pose, 3);

camRTcamL = stereoT;

if (FLAG_STEREO) %%Use stereo information (calibrate both camera depenently)%%

AR = zeros(4, 4, 4*N);
AL = zeros(4, 4, 4*N);
B = zeros(4, 4, 4*N);
for i = 1:N
if (i == N)
AR(:, :, i) = camera_pose(:, :, 1, 1)/camera_pose(:, :, i, 1);
AL(:, :, i) = camera_pose(:, :, 1, 2)/camera_pose(:, :, i, 2);
AR(:, :, i + N) = camRTcamL*camera_pose(:, :, 1, 2)/camera_pose(:, :, i, 2)/camRTcamL;
AL(:, :, i + N) = camRTcamL\camera_pose(:, :, 1, 1)/(camera_pose(:, :, i, 1))*camRTcamL;
AR(:, :, i + 2*N) = camRTcamL*camera_pose(:, :, 1, 2)/camera_pose(:, :, i, 1);
AL(:, :, i + 2*N) = camRTcamL\camera_pose(:, :, 1, 1)/camera_pose(:, :, i, 2);
AR(:, :, i + 3*N) = camera_pose(:, :, 1, 1)/camera_pose(:, :, i, 2)/camRTcamL;
AL(:, :, i + 3*N) = camera_pose(:, :, 1, 2)/camera_pose(:, :, i, 1)*camRTcamL;
B(:, :, i) = robot_pose(:, :, 1)\robot_pose(:, :, i);
else
AR(:, :, i) = camera_pose(:, :, i + 1, 1)/camera_pose(:, :, i, 1);
AL(:, :, i) = camera_pose(:, :, i + 1, 2)/camera_pose(:, :, i, 2);
AR(:, :, i + N) = camRTcamL*camera_pose(:, :, i + 1, 2)/camera_pose(:, :, i, 2)/camRTcamL;
AL(:, :, i + N) = camRTcamL\camera_pose(:, :, i + 1, 1)/(camera_pose(:, :, i, 1))*camRTcamL;
AR(:, :, i + 2*N) = camRTcamL*camera_pose(:, :, i + 1, 2)/camera_pose(:, :, i, 1);
AL(:, :, i + 2*N) = camRTcamL\camera_pose(:, :, i + 1, 1)/camera_pose(:, :, i, 2);
AR(:, :, i + 3*N) = camera_pose(:, :, i + 1, 1)/camera_pose(:, :, i, 2)/camRTcamL;
AL(:, :, i + 3*N) = camera_pose(:, :, i + 1, 2)/camera_pose(:, :, i, 1)*camRTcamL;
B(:, :, i) = robot_pose(:, :, i + 1)\robot_pose(:, :, i);
end
end
B(:, :, N + 1:2*N) = B(:, :, 1:N);
B(:, :, 2*N + 1:3*N) = B(:, :, 1:N);
B(:, :, 3*N + 1:4*N) = B(:, :, 1:N);

switch FLAG_ALGO
case 1 %% Our algorithm %%
XR = HandEye_ST(AR, B);
XL = HandEye_ST(AL, B);
case 2 %% Barreto's algorithm (Improved Dual Quaternion, linear) %%
XR = HandEye_IDQ(AR, B);
XL = HandEye_IDQ(AL, B);
case 3 %% Tsai's algorithm (Conventional method) %%
XR = HandEye_Tsai(AR, B);
XL = HandEye_Tsai(AL, B);
case 4 %% Konstantinos' algorithm (Dual Quaternion with quadratic eq.) %%
XR = HandEye_DQ(AR, B);
XL = HandEye_DQ(AL, B);
otherwise
error('Invalid ALGORITHM FLAG.');
end
else %%Not Use stereo information%%

A = zeros(4, 4, N);
B = zeros(4, 4, N);
for i = 1:N
if (i == N)
A(:, :, i) = camera_pose(:, :, 1)/camera_pose(:, :, i);
B(:, :, i) = robot_pose(:, :, 1)\robot_pose(:, :, i);
else
A(:, :, i) = camera_pose(:, :, i + 1)/camera_pose(:, :, i);
B(:, :, i) = robot_pose(:, :, i + 1)\robot_pose(:, :, i);
end
end
switch FLAG_ALGO
case 1 %% Our algorithm %%
XR = HandEye_ST(A, B);
case 2 %% Barreto's algorithm (Improved Dual Quaternion, linear) %%
XR = HandEye_IDQ(A, B);
case 3 %% Tsai's algorithm (Conventional method) %%
XR = HandEye_Tsai(A, B);
case 4 %% Konstantinos' algorithm (Dual Quaternion with quadratic eq.) %%
XR = HandEye_DQ(A, B);
otherwise
error('Invalid ALGORITHM FLAG.');
end
XL = stereoT\XR;
end

% The non-linear optimisation is for real data where a lot of local
% minima can be found.
% h = optimoptions(@lsqnonlin, 'Algorithm', 'levenberg-marquardt', 'MaxFunEvals', 30000);
%
%
% x_init = [rodrigues(XR(1:3, 1:3)); XR(1:3, 4)];
% [refine_X, ~, ~, ~, ~, ~, J] = lsqnonlin(@(x) optimAXXB(x, AR, B), x_init, [], [], h);
%
% XR = [rodrigues(refine_X(1:3)), refine_X(4:6);0 0 0 1];
%
% x_init = [rodrigues(XL(1:3, 1:3)); XL(1:3, 4)];
% [refine_X, ~, ~, ~, ~, ~, J] = lsqnonlin(@(x) optimAXXB(x, AL, B), x_init, [], [], h);
%
% XL = [rodrigues(refine_X(1:3)), refine_X(4:6);0 0 0 1];
end
93 changes: 93 additions & 0 deletions HandEye_DQ.m
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
function X = HandEye_DQ(A, B)
%Our quaternions are like this (q1 q2 q3,s )
%Get n
n = size(A, 3);
%Make movements (a,B) which are interposition transformations
%(marker2wordl and cam2grid)
%transform A,B into dual quaternions
for i=1:n
[q,qprime] = getDualQ(A(1:3,1:3,i),A(1:3,4,i));
Qa(i).q = q;
Qa(i).qprime = qprime;
[q,qprime] = getDualQ(B(1:3,1:3,i),B(1:3,4,i));
Qb(i).q = q;
Qb(i).qprime = qprime;
end

%The dual quaternion is (Q.q + epsilon*Q.prime)
%a = Qa.q, a' = Qa.prime idem for b
S = [];
for i=1:n
S(:,:,i) = [Qa(i).q(1:3)-Qb(i).q(1:3) skew3(Qa(i).q(1:3)+Qb(i).q(1:3)) zeros(3,1) zeros(3,3);...
Qa(i).qprime(1:3)-Qb(i).qprime(1:3) skew3(Qa(i).qprime(1:3)+Qb(i).qprime(1:3)) Qa(i).q(1:3)-Qb(i).q(1:3) skew3(Qa(i).q(1:3)+Qb(i).q(1:3))];
end

%Construct T
T = [];
for i=1:n
T = [T S(:,:,i)'];
end

T = T';
%SVD
[U,S,V] = svd(T);

%Solution, right null vectors of T
v7 = V(:,7);
v8 = V(:,8);

u1 = v7(1:4);
v1 = v7(5:8);

u2 = v8(1:4);
v2 = v8(5:8);
%Now lambda1*v7+lambda2*v8 = [q;qprime]
%
%or other:
%
%lambda1^2*u1'*u1+2*lambda1*lambda2*u1'*u2+lambda2^2*u2'*u2 = 1
%and
%lambda1^2*u1'*v1 + lambda1*lambda2*(u1'*v2+u2'*v1)+lambda2*u2'*v1 = 0
%Setting lambda1/lambda2 = s
%lambda1^2/lambda2^2*u1'*v1 + lambda1*lambda2/lambda2^2*(u1'*v2+u2'*v1)+lambda2^2/lambda2^2*u2'*v1 = 0
%s^2*u1'*v1 + s*(u1'*v2+u2'*v1)+u2'*v1 = 0
%s^2*u1'*v1 + s*(u1'*v2+u2'*v1)+u2'*v1 = 0
a = u1'*v1;
b = (u1'*v2+u2'*v1);
% c = u2'*v1;
c = u2'*v2 ;
s = roots([a b c]);

s = real(s);

%insert into equation
val1 = s(1)^2*u1'*u1+2*s(1)*u1'*u2+u2'*u2;
val2 = s(2)^2*u1'*u1+2*s(2)*u1'*u2+u2'*u2;
%Take bigger value
if(val1>val2)
s = s(1);
val = val1;
else
s = s(2);
val = val2;
end
%Get lambdas
lambda2 = sqrt(1/val);
lambda1 = s*lambda2;

%This algorithm gives quaternion with the form of (s, q1 q2
%q3)->contrary to the notation we used above (q1 q2 q3,s )
%Therefore we must rearrange the elements!
qfinal = lambda1*v7+lambda2*v8;
q = [qfinal(2:4);qfinal(1)];
qprime = [qfinal(6:8);qfinal(5)];

%Extract transformation
R = q2dcm(q)';
t = 2*qmult(qprime,qconj(q));
t = t(1:3);

%Assign output arguments
X = [R t;[0 0 0 1]];
err=[];
end
35 changes: 35 additions & 0 deletions HandEye_IDQ.m
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
function X = HandEye_IDQ(A, B)
%Estimate Camera to robot gripper transformation from the equation
%Ta: Camera to grid transformation in the form of (4x4xN)
%Tb: Gripper to robot base transformation in the form on (4x4xN)
%, where N is the number of captured movement.
%AX = XB
N = size(A, 3);
%Construct L and Lp matrix
L = zeros(4*N, 4);
Lp = zeros(4*N, 4);
for j = 1:N
[a, ap] = getDualQ(A(1:3, 1:3, j), A(1:3, 4, j));
[b, bp] = getDualQ(B(1:3, 1:3, j), B(1:3, 4, j));
x = a - b;
y = a + b;
z = ap - bp;
w = ap + bp;
L(4*j-3:4*j, :) = [x(4), -x(1:3)';x(1:3), skew3(y(1:3))+x(4)*eye(3)];
Lp(4*j-3:4*j, :) = [z(4), -z(1:3)';z(1:3), skew3(w(1:3))+z(4)*eye(3)];
end

%Determine real part and dual part of matrix X
[~, ~, V] = svd(L);
q = V(:, 4);
qprime = L\(-Lp*q);

q = [q(2:4); q(1)];
qprime = [qprime(2:4); qprime(1)];

%Determine eye to hand tranformation
R = q2dcm(q)';
t = 2*qmult(qprime,qconj(q));
t = t(1:3);
X = [R, t;0 0 0 1];
end
84 changes: 84 additions & 0 deletions HandEye_ST.m
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
function X = HandEye_ST(A, B)
converge_thres = 10;
counter_rotation = 0;
counter_translation = 0;
max_ite = 50;

N = size(A, 3);

v_a = zeros(3, N);
v_b = zeros(3, N);
om_a = zeros(3, N);
om_b = zeros(3, N);
K = zeros(8*N, 4);

X = eye(4);

LHS = zeros(3*N, 3);
RHS = zeros(3*N, 1);

for j = 1:N
a = logm(A(:, :, j));
b = logm(B(:, :, j));
v_a(:, j) = a(1:3 ,4);
v_b(:, j) = b(1:3, 4);
om_a(:, j) = rodrigues(A(1:3, 1:3, j));
om_b(:, j) = rodrigues(B(1:3, 1:3, j));
LHS(3*j - 2:3*j, :) = skew3(om_a(:, j));
[a, ap] = getDualQ(A(1:3, 1:3, j), A(1:3, 4, j));
[b, bp] = getDualQ(B(1:3, 1:3, j), B(1:3, 4, j));
x = a - b;
y = a + b;

K(8*j - 7:8*j - 4, 1:4) = [x(4), -(x(1:3))'; x(1:3), skew3(y) + x(4)*eye(3)];
end

counter = 0;

while (((counter_rotation < converge_thres) || (counter_translation < converge_thres)) && (counter < max_ite))

R_init = X(1:3, 1:3);
t_init = X(1:3, 4);

for j = 1:N
vec_buf = v_a(:, j) - cross(t_init, om_a(:, j));
K(8*j - 3:8*j, 1) = [vec_buf - v_b(:, j); 0];
K(8*j - 3:8*j, 2:4) = [skew3(vec_buf + v_b(:, j)); (-vec_buf + v_b(:, j))'];
end

[~, ~, v_basis] = svd(K);
v_basis = v_basis(:, 4);

qR = [v_basis(2:4); v_basis(1)];

X(1:3, 1:3) = q2dcm(qR)';

for j = 1:N
LHS(3*j - 2:3*j, :) = skew3(om_a(:, j));
RHS(3*j - 2:3*j, 1) = X(1:3, 1:3)*v_b(:, j) - v_a(:, j);
end


X = [X(1:3, 1:3) LHS\RHS;0 0 0 1];

diff = X/[R_init, t_init;0 0 0 1];

if (norm(rodrigues(diff(1:3, 1:3))) < 1e-3)
counter_rotation = counter_rotation + 1;
else
counter_rotation = 0;
end

if (norm(diff(1:3, 4)) < 1e-3)
counter_translation = counter_translation + 1;
else
counter_translation = 0;
end

counter = counter + 1;
ccc = rodrigues(X(1:3, 1:3));
C(1:3, counter) = ccc;
C(4:6, counter) = X(1:3, 4);
end

end
33 changes: 33 additions & 0 deletions HandEye_Tsai.m
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
function X = HandEye_Tsai(A, B)
%Estimate Camera to robot gripper transformation from the equation
%TaL: Left camera to grid transformation in the form of (4x4xN)
%Tb: Gripper to robot base transformation in the form of (4x4xN)
%, where N is the number of captured movement.
X = eye(4);
N = size(A, 3);
RLHS = zeros(3*N, 3);
RRHS = zeros(3*N, 1);
for j = 1:N
rA = rodrigues(A(1:3, 1:3, j));
rB = rodrigues(B(1:3, 1:3, j));
thetaA = norm(rA);
thetaB = norm(rB);
rAn = rA/thetaA;
rBn = rB/thetaB;
pA = 2*sin(thetaA/2)*rAn;
pB = 2*sin(thetaB/2)*rBn;
RLHS(3*j - 2:3*j, :) = skew3(pA + pB);
RRHS(3*j - 2:3*j, :) = pB - pA;
end

axisRR = RLHS\RRHS;
axisRR = 2*axisRR/sqrt(1 + norm(axisRR)^2);
X(1:3, 1:3) = (1-norm(axisRR)*norm(axisRR)/2)*eye(3)+0.5*(axisRR*axisRR'+sqrt(4-norm(axisRR)*norm(axisRR))*skew3(axisRR));
LHS = zeros(3*N, 3);
RHS = zeros(3*N, 1);
for j = 1:N
LHS(3*j - 2:3*j, :) = A(1:3, 1:3, j) - eye(3);
RHS(3*j - 2:3*j, :) = X(1:3, 1:3)*B(1:3, 4, j) - A(1:3, 4, j);
end
X(1:3, 4) = LHS\RHS;
end
15 changes: 15 additions & 0 deletions addDistortion.m
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
function [xd, xp] = addDistortion(x, cc, kc)

m = length(x);

r2 = x(1, :).^2 + x(2, :).^2;
a = x(1, :).*x(2, :);
b = 2*x(1, :).^2;
c = 2*x(2, :).^2;

dx = [2*kc(3)*a + kc(4)*(r2 + b);
kc(3)*(r2 + c) + 2*kc(4)*a];
xd = repmat((1 + kc(1)*r2 + kc(2)*r2.^2 + kc(5)*r2.^3), 2, 1).*x(1:2, :) + dx + repmat([cc(1); cc(2)], 1, m);
xp = x + repmat([cc(1); cc(2); 1], 1, m);

end
Loading

0 comments on commit 36db4b4

Please sign in to comment.