% Authors: Steven Su and Saurabh Vyas
% Date: May 17, 2011
% Interventional Photoacoustic Registration
% 600.446: CIS II, Spring 2011
% The Johns Hopkins University

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Peforms a point cloud transform between two sets of points using Arun's
% method. Put the coordinate vectors for each cloud in a vertical fashion.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [R,p] = PointCloudTransform(matAi, matBi)

matAidim = size(matAi); %Determines dimesions, MxN, of input matrix (point cloud) matAi 
N_ai = matAidim(1,2); %Determines the number, N_ai, of a_i vectors in the first input point cloud, matAi
a_bar = (1/N_ai).*sum(matAi,2); %Computes the mean of all the a_i vectors in the input point cloud, matAi

matBidim = size(matBi); %Determines dimesions, MxN, of input matrix (point cloud) matBi 
N_bi = matBidim(1,2); %Determines the number, N_bi, of b_i vectors in the second input point cloud, matBi
b_bar = (1/N_bi).*sum(matBi,2);%Computes the mean of all the b_i vectors in the input point cloud, matBi

Ltotal = zeros(3, 3);

%Computes the difference between each individual vector a_i in input point
%cloud, matAi, and the average of the vectors, a_bar
for i=1:1:N_ai
    a_tilde_mat(:,i) = matAi(:,i)-a_bar;  
end
%Computes the difference between each individual vector b_i in input point
%cloud, matBi, and the average of the vectors, b_bar
for i=1:1:N_bi
    b_tilde_mat(:,i) = matBi(:,i)-b_bar;
end

%Instantiates a matrix containing a sum of the corresponding products of these a_tilde and b_tilde
%which will be used to compute the SVD
for i = 1:1:N_ai
    for k = 1:3
        L(:,k) = a_tilde_mat(:,i).*b_tilde_mat(k,i);
    end
    Ltotal = Ltotal + L;
end
%Computes the SVD using standard Matlab library
[U S V]=svd(Ltotal);

%Computes the rotation matrix, R
R = V*U';

%Checks to make sure the determinant of R is 1. If it is p is calculated,
%or else an error message is shown and the V prime matrix is created.
if round(det(R))==1
    p = b_bar-R*a_bar;
    disp('det(R)=1');
else
    disp('Error, Deteriminant of R not 1, Error Handling, Please Wait...');
    V_prime=zeros(3,3);
    V_prime(:,1:2)=V(:,1:2);
    V_prime(:,3)=-1.*V(:,3);
    R=V_prime*U';
    p = b_bar-R*a_bar;
end