-
Notifications
You must be signed in to change notification settings - Fork 3
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
0 parents
commit 36db4b4
Showing
24 changed files
with
2,399 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
Oops, something went wrong.