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];