%Estimating rotations and translations
%Alyssa Novelia (5/01/2013)
%Revised by Daniel Kawano (12/13/2017)

%Implementation of the four methods used to estimate rotations and
%translations using tracking data from four material points on the object.

clear; clc; close all;

%%  Preliminaries

%Define book's dimension:
l = 8;  %length
w = 6;  %width
b = 1;  %thickness

%Choose 4 material points (landmarks) in reference configuration to track:
r1 = 0.5*[-l; w; -b];
r2 = 0.5*[l; w; -b];
r3 = 0.5*[-l; -w; -b];
r4 = 0.5*[-l; w; b];

%Parameterize rotation using a set of 3-2-1 Euler angles. L1, L2, L3 are
%matrix components of rotation tensors:
L3 = [cos(pi/4) sin(pi/4) 0; -sin(pi/4) cos(pi/4) 0; 0 0 1];
L2 = [cos(pi/6) 0 -sin(pi/6); 0 1 0; sin(pi/6) 0 cos(pi/6)];
L1 = [1 0 0; 0 cos(pi/4) sin(pi/4); 0 -sin(pi/4) cos(pi/4)];

%Take the product of the matrices and transpose it to obtain the rotation
%matrix:
Q = (L1*L2*L3)';

%Define translation:
d = [1; 1; -10];

%Calculate the positions of landmarks in the current configuration: 
p1 = Q*r1 + d;
p2 = Q*r2 + d;
p3 = Q*r3 + d;
p4 = Q*r4 + d;

%Noise will be modeled using a random number generator within the interval
%defined below:
noise_mag = 0;  %set to 0 for noiseless case
noise_int = noise_mag*[-1 1];

%In this example we will only add noise to the current configuration's
%measurements. The measured positions in the current configuration are as
%follows:
m1 = p1 + noise_int(1) + (noise_int(2)-noise_int(1)).*rand(3,1);
m2 = p2 + noise_int(1) + (noise_int(2)-noise_int(1)).*rand(3,1);
m3 = p3 + noise_int(1) + (noise_int(2)-noise_int(1)).*rand(3,1);
m4 = p4 + noise_int(1) + (noise_int(2)-noise_int(1)).*rand(3,1);

f = figure; 
set(f, 'Name', 'Visualizing estimated rotation error. Blue: estimated rotation, yellow: actual rotation');
set(f, 'color', 'w');

%%  Method 1: Naive method

%Create augmented M (m') and R (r') matrices:
augM = [m1 m2 m3 m4]; augM = [ones(1,4); augM];
augR = [r1 r2 r3 r4]; augR = [ones(1,4); augR];

%Solve for the augmented solution:
augX = augM*augR^(-1);

%Extract rotation matrix and translation vector:
R_method1 = augX(2:end, 2:end);
d_method1 = augX(2:end,1);

%Plot comparison of results from the 4 methods here:
subplot(2,2,1); title('Naive method');
draw_book(Q,d,l,w,b,'y');
draw_book(R_method1,d_method1,l,w,b,'b');
xlabel('\bfE\rm_1'); ylabel('\bfE\rm_2'); zlabel('\bfE\rm_3     ', 'rotation', 0);
grid on;
axis equal;
view(3);

%%  Method 2: TRIAD method

%There are 24 possible ways to construct vectors V_i and v_i.
%One way to see this is to imagine a cuboid where 4 of its vertices are
%r/m_1,2,3,4. We can calculate the remainder of the vertices of the cuboid
%from r/m_1,2,3,4.

%The vertex on each face of the cuboid is a possible origin for the triad
%formed by {V_i} or {v_i}. The triad will have two vectors {V_1,2 or v_1,2}
%parallel to that particular face of the cuboid and one vector {V_3 or v_3}
%that is normal to the face of the cuboid. The vectors parallel to the face
%of the cuboid will be based on the two edges that meet at that particular
%vertex.

%Since there are 6 faces of the cube, and for each face there are 4
%vertices, there are 6 x 4 = 24 possible triads to be constructed.

%Construct a cell array to store the vertices on each face. There are 6
%faces and 4 vertices on each face, hence we have a 6 x 4 cell array:
Rvertices = {r1 r2 r2+r4-r1 r4;
            r2 r2+r3-r1 r2+r3+r4-2*r1 r2+r4-r1;
            r2+r3-r1 r3 r3+r4-r1 r2+r3+r4-2*r1;
            r1 r3 r3+r4-r1 r4;
            r4 r2+r4-r1 r2+r3+r4-2*r1 r3+r4-r1;
            r1 r2 r2+r3-r1 r3};

Mvertices = {m1 m2 m2+m4-m1 m4;
            m2 m2+m3-m1 m2+m3+m4-2*m1 m2+m4-m1;
            m2+m3-m1 m3 m3+m4-m1 m2+m3+m4-2*m1;
            m1 m3 m3+m4-m1 m4;
            m4 m2+m4-m1 m2+m3+m4-2*m1 m3+m4-m1;
            m1 m2 m2+m3-m1 m3};

%Append all the position vectors together:
R = [r1 r2 r3 r4];
M = [m1 m2 m3 m4];

%Prepare variables to store error, calculated R matrices and d vectors:
err_list = [];
R_list = zeros(3,3,size(Rvertices,1)*size(Rvertices,2));
d_list = [];

for iii = 1:size(Rvertices,1)   %loop over faces
    for jjj = 1:size(Rvertices,2)   %loop over vertices

        %Choose an origin:
        pivot = jjj;
        %Choose two other vertices that will define the two edges on the
        %plane:
        next = jjj+1; if next > 4, next = next - 4; end
        prev = jjj-1; if prev < 1, prev = prev + 4; end

        %Define r/m_1,2,3 used to calculate V/v_1,2,3:
        Rpivot = Rvertices{iii,pivot};
        Rnext = Rvertices{iii,next};
        Rprev = Rvertices{iii,prev};

        Mpivot = Mvertices{iii,pivot};
        Mnext = Mvertices{iii,next};
        Mprev = Mvertices{iii,prev};

        %Calculate V_1,2,3 and v_1,2,3:
        V1 = (Rnext-Rpivot); V1 = V1/norm(V1);
        V2 = cross(V1,(Rprev-Rpivot)); V2 = V2/norm(V2);
        V3 = cross(V1,V2); V3 = V3/norm(V3);

        v1 = (Mnext-Mpivot); v1 = v1/norm(v1);
        v2 = cross(v1,(Mprev-Mpivot)); v2 = v2/norm(v2);
        v3 = cross(v1,v2); v3 = v3/norm(v3);

        %Append V_1,2,3 and v_1,2,3:
        V = [V1 V2 V3];
        v = [v1 v2 v3];

        %Calculate rotation matrix:
        R2 = v*V';
        
        %Calculate translation:
        d2 = (m1 + m2 + m3)./3 - R2*((r1 + r2 + r3)./3);

        %Calculate error:
        err_square = norm(M - R2*R + [d2 d2 d2 d2]);

        %Append errors, calculated R and d for each iteration:
        err_list = [err_list; err_square];
        d_list = [d_list d2];
        R_list(:,:,size(d_list,2)) = R2;
    end
end

%Find the rotation and translation with minimum error and assign the value:
[dummy idx] = min(err_list);
R_method2 = R_list(:,:,idx);
d_method2 = d_list(:,idx);

%Plot comparison of results from 4 methods here:
subplot(2,2,2); title('TRIAD method');
draw_book(Q,d,l,w,b,'y');
draw_book(R_method2,d_method2,l,w,b,'b');
xlabel('\bfE\rm_1'); ylabel('\bfE\rm_2'); zlabel('\bfE\rm_3     ', 'rotation', 0);
grid on;
axis equal;
view(3);

%%  Method 3: Singular value decomposition method

%Sum the appended matrices R and M across the columns, and divide by the
%column size of the matrices to calculate the mean positions:
meanR = sum(R,2)./size(R,2);   
meanM = sum(M,2)./size(M,2);

%Calculate and append position vectors relative to mean positions:
C = M - repmat(meanM,1,4);
D = R - repmat(meanR,1,4);

%Calculate S:
S = C*D';

%Note that the third matrix output by the SVD function is the transpose
%of R2:
[R1 LAM R2T] = svd(S);
R2 = R2T';  %calculate R2

%Calculate the rotation matrix and translation vector:
R_method3 = R1*[1 0 0; 0 1 0; 0 0 det(R1*R2)]*R2;
d_method3 = meanM - R_method3*meanR;

%Plot comparison of results from the 4 methods here:
subplot(2,2,3); title('SVD method');
draw_book(Q,d,l,w,b,'y');
draw_book(R_method3,d_method3,l,w,b,'b');
xlabel('\bfE\rm_1'); ylabel('\bfE\rm_2'); zlabel('\bfE\rm_3     ', 'rotation', 0);
grid on;
axis equal;
view(3);

%%  Method 4: q-method

%Assign weights to each position vector and calculate weighted position
%vectors. If all position vectors have equal weight, we expect the q-method
%to yield the same result as the SVD method:
alpha = [1 1 1 1];
weightR = alpha*R'; weightR = weightR'./sum(alpha);
weightM = alpha*M'; weightM = weightM'./sum(alpha);

%Define B matrix:
B = zeros(3,3);
for iii = 1:size(B,1)   %loop over rows, iii = i
    for mmm = 1:size(B,1)   %loop over columns, mmm = m
        B1 = zeros(size(alpha));
        %Calculate the term in bracket, call that B1:
        for kkk = 1:size(alpha,2)   %to over alpha to calculate summation, kkk = k
            B1(kkk) = alpha(kkk)*M(iii,kkk)*R(mmm,kkk);
        end
        B1 = sum(B1,2)./sum(alpha);
        B(iii,mmm) = B1 - weightM(iii)*weightR(mmm);
    end
end

%Define z matrix:
z2 = zeros(3,size(alpha,2));
for kkk = 1:size(alpha,2)   %loop over alpha to calculate summation, kkk = k
    z2(:,kkk) = cross(alpha(kkk)*M(:,kkk), R(:,kkk));
end
z2 = sum(z2,2)./sum(alpha);
z = cross(weightM,weightR) - z2;

%Define augmented matrix K:
K = [B + B' - trace(B)*eye(size(B)) z];
K = [K; z' trace(B)];

%Find maximum eigenvalue of K and corresponding eigenvector:
[dum idx] = max(eig(K));
[eigV lam] = eig(K);
q = eigV(:,idx);    %where q = [e1 e2 e3 e0]

%Reconstruct rotation matrix using Euler-Rodrigues parameters:
R_method4 = (q(4)^2 - q(1)^2 - q(2)^2 - q(3)^2)*eye(3,3) + ...
            2*q(1:3)*q(1:3)' + ...
            2*[0 -q(4)*q(3) q(4)*q(2); q(4)*q(3) 0 -q(4)*q(1); -q(4)*q(2) q(4)*q(1) 0];

%Calculate translation:
d_method4 = weightM - R_method4*weightR;    

%Plot comparison of results from the 4 methods here:
subplot(2,2,4); title('q-method');
draw_book(Q,d,l,w,b,'y');
draw_book(R_method4,d_method4,l,w,b,'b');
xlabel('\bfE\rm_1'); ylabel('\bfE\rm_2'); zlabel('\bfE\rm_3     ', 'rotation', 0);
grid on;
axis equal;
view(3);