function X = daniilidis(A,B)
% Calculates the least squares solution of
% AX = XB
%
% The dual quaternion approach to hand-eye calibration
% Daniildis, Bayro-Corrochano
%
% Mili Shah
% July 2014
%
% Uses hom2quar.m and quar2hom.m

[m,n] = size(A); n = n/4;
T = zeros(6*n,8);

for i = 1:n
    A1 = (A(:,4*i-3:4*i));
    B1 = (B(:,4*i-3:4*i));
    a = hom2quar(A1);
    b = hom2quar(B1);
    T(6*i-5:6*i,:) = ...
        [a(2:4,1)-b(2:4,1)  skew(a(2:4,1)+b(2:4,1)) zeros(3,4);...
        a(2:4,2)-b(2:4,2) skew(a(2:4,2)+b(2:4,2)) a(2:4,1)-b(2:4,1)  skew(a(2:4,1)+b(2:4,1))];
end
[u,s,v]=svd(T);
u1 = v(1:4,7);
v1 = v(5:8,7);
u2 = v(1:4,8);
v2 = v(5:8,8);

a = u1'*v1;
b = u1'*v2+u2'*v1;
c = u2'*v2;

s1 = (-b+sqrt(b^2-4*a*c))/2/a;
s2 = (-b-sqrt(b^2-4*a*c))/2/a;

s  = [s1; s2];
[val,in] = max(s.^2*(u1'*u1) + 2*s*(u1'*u2) + u2'*u2);
s = s(in);
L2 = sqrt(1/val);
L1 = s*L2;

q = L1*v(:,7) + L2*v(:,8);
X = quar2hom([q(1:4) q(5:8)]);
end
function dq = hom2quar(H)
% Converts 4x4 homogeneous matrix H
% to a dual quaternion dq represented as
% dq(:,1) + e*dq(:,2)
%
% Mili Shah

R = H(1:3,1:3);
t = H(1:3,4);

R = logm(R);
r = [R(3,2) R(1,3) R(2,1)]';
theta = norm(r);
l = r/norm(theta);

q = [cos(theta/2); sin(theta/2)*l];
qprime = .5*qmult([0;t],q);
dq=[q qprime];
end
function H = quar2hom(dq)
% Converts dual quaternion dq represented as
% dq(:,1) + e*dq(:,2)
% to a 4x4 homogeneous matrix H
%
% Uses the m-file
% qmult.m
%
% Mili Shah

q = dq(:,1); qe = dq(:,2);

R = [1-2*q(3)^2-2*q(4)^2 2*(q(2)*q(3)-q(4)*q(1)) 2*(q(2)*q(4)+q(3)*q(1));...
    2*(q(2)*q(3)+q(4)*q(1)) 1-2*q(2)^2-2*q(4)^2 2*(q(3)*q(4)-q(2)*q(1));...
    2*(q(2)*q(4)-q(3)*q(1)) 2*(q(3)*q(4)+q(2)*q(1)) 1-2*q(2)^2-2*q(3)^2];

q(2:4) = -q(2:4);
t = 2*qmult(qe,q);

H = [R t(2:4);0 0 0 1];
end