This repository has been archived by the owner on Mar 26, 2021. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 52
/
hand_eye_dual_quaternion.m
143 lines (127 loc) · 4.39 KB
/
hand_eye_dual_quaternion.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
%This computes the hand-eye calibration using the method described in
%"Hand-Eye Calibration Using Dual Quaternions" from
%Konstantinos Daniilidis
%
%Input:
%Hmarker2world a 4x4xNumber_of_Views Matrix of the form
% Hmarker2world(:,:,i) = [Ri_3x3 ti_3x1;[ 0 0 0 1]]
% with
% i = number of the view,
% Ri_3x3 the rotation matrix
% ti_3x1 the translation vector.
% Defining the transformation of the robot hand / marker
% to the robot base / external tracking device
%Hgrid2cam a 4x4xNumber_of_Views Matrix (like above)
% Defining the transformation of the grid to the camera
%
%Output:
%Hcam2marker_ The transformation from the camera to the marker /
% robot arm
%err The residuals from the least square processes
%
%Christian Wengert
%Computer Vision Laboratory
%ETH Zurich
%Sternwartstrasse 7
%CH-8092 Zurich
%www.vision.ee.ethz.ch/cwengert
function [Hcam2marker_, err] = hand_eye_dual_quaternion(Hmarker2world, Hgrid2cam)
%Our quaternions are like this (q1 q2 q3,s )
%Get n
n = size(Hmarker2world,3);
%Make movements (a,B) which are interposition transformations
%(marker2wordl and cam2grid)
%transform A,B into dual quaternions
for i=1:n-1
A = inv(Hmarker2world(:,:,i+1))*Hmarker2world(:,:,i);
B = Hgrid2cam(:,:,i+1)*inv(Hgrid2cam(:,:,i));
[q,qprime] = getDualQuaternion(A(1:3,1:3),A(1:3,4));
Qa(i).q = q;
Qa(i).qprime = qprime;
[q,qprime] = getDualQuaternion(B(1:3,1:3),B(1:3,4));
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-1
S(:,:,i) = [Qa(i).q(1:3)-Qb(i).q(1:3) crossprod(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) crossprod(Qa(i).qprime(1:3)+Qb(i).qprime(1:3)) Qa(i).q(1:3)-Qb(i).q(1:3) crossprod(Qa(i).q(1:3)+Qb(i).q(1:3))];
end
%Construct T
T = [];
for i=1:n-1
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]);
%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
Hcam2marker_ = [R -R*t;[0 0 0 1]]^-1;
err=[];
%Creates a dual quaternion from a rotation matrix and a translation vector
function [q,qprime] = getDualQuaternion(R,t)
%Conversion from R,t to the screw representation [d,theta,l,m]
r = rodrigues(R);
theta = norm(r);
l = r/norm(theta);
%Pitch d
d = l'*t;
%Make point c
c = .5*(t-d*l)+cot(theta/2)*cross(l,t);
%moment vector
m = cross(c,l);
%Rotation quaternion
%(q1 q2 q3,s )
q = [sin(theta/2)*l; cos(theta/2)];
%Get dual
qprime = [.5*(q(4)*t+cross(t,q(1:3)));-.5*q(1:3)'*t];