%% Time Varying Systems and Computation
% Klaus Diepold, Sven Gronauer, Simon Schumann
%
%% 2. Applications of SVD
%% 2.1 Pseudoinverse
% SVD  can be used for computing the pseudoinverse of a matrix. The Matlab command "pinv()" also uses this strategy. To find the pseuoinverse of a matrix A, we first find its SVD:

A=randn(5,3)*randn(3,4); %A matrix from the the previous Live-Script
[U,S,V] = svd(A);

% Then we treat the singular values along the diagonal of S that are smaller than tol as zeros, and the representation of A becomes:

tol=1e-4;
d=diag(S);
d(d<=tol)=0;
n=find(d);
A_tilde=U(:,1:n(end))*S(1:n(end),1:n(end))*V(:,1:n(end))';

% Finally, we calculate the pseudoinverse of A as

pseudoinverse= V(:,1:n(end))*inv(S(1:n(end),1:n(end)))*U(:,1:n(end))'

% Comparison with pseudoinverse calulated using Matlab command:

pseudoinverseMatlab=pinv(A,1e-4)
max(max(abs(pseudoinverse-pseudoinverseMatlab))) %maximum absolute difference

%
%% 2.2 Kabsch Algorithm
% The Kabsch Algorithm is is an algorithm that calculates the optimal rotation matrix that minimizes the root mean squared deviation (RMSD) between two sets of points. It is mostly used in cheminformatics for comparing molecular structures and in bioinformatics for comparing protein structures.
% Consider two sets of paired points, P and Q.( Each set of points is represented as an N × 3 matrix.)

Q=[10 1 1;10 2 1; 10 3 1;12 1 1; 12 2 1;12 3 1];
P=[cos(45) sin(45) 0;-sin(45) cos(45) 0;0 0 1]*Q';
disturbance=0.05*(-0.5+rand(size(Q,1),size(Q,2)));
P=P'+disturbance;
Q_pre=Q;
P_pre=P;
plot3(Q(:,1),Q(:,2),Q(:,3),'*')
grid on
xlabel('x')
ylabel('y')
zlabel('z')
hold on
plot3(P(:,1),P(:,2),P(:,3),'*')
zlim([0 2])
hold off

% The following lemma is used in the algorithm.
% Lemma: Let U S V^\top be a singular value decomposition of the matrix Q P^\top. Then the minimum value of || P-RQ||^2 with respect to R is
% \min_{R} || P-RQ||^2 = || P||^2 + ||Q||^2 -2 \text{tr}(S\tilde S),
% where  \[
%    \tilde S = \left\{\begin{array}{lr}
%         I, & \text{if} \: \text{det} (P Q ^\top) \geq 0 \\
%         \text{diag}(1,1,...,-1) & \text{if} \: \text{det} (P Q^\top) < 0
%         \end{array}
%   \] .
% To learn more about it, you can check the papers Least-Squares Estimation of Transformation Parameters Between Two Point Patterns by S. Umeyema and A Purely Algebraic Justification of the Kabsch-Umeyama Algorithm by J. Lawrence, J. Bernal, C. Witzgall.
% The algorithm works in three steps: translation, computation of the covariance matrix, and computation of the optimal rotation matrix.
% Step 1 - Translation:
% Both sets of coordinates must be translated first, so that their centroid coincides with the origin of the coordinate system. This is done by subtracting from the point coordinates of the respective centroid.

Qc=sum(Q,1)/size(Q,1);
Q=Q-Qc;
Pc=sum(P,1)/size(P,1);
P=P-Pc;
plot3(Q(:,1),Q(:,2),Q(:,3),'*')
grid on
xlabel('x')
ylabel('y')
zlabel('z')
hold on
plot3(P(:,1),P(:,2),P(:,3),'*')
zlim([-1 1])
hold off

% Step 2 - Computation of the covariance matrix:

M=Q*P';

% Step 3 - Computation of the optimal rotation matrix:

[U,S,V] = svd(M);
if det(M)>=0
    S_tilde=diag(ones(size(M,1),1));
else
    S_tilde=diag([ones(size(M,1)-1,1);-1]);
end
R_tilde=V*S_tilde*U';
P_tilde=R_tilde*Q;
P_tilde=P_tilde+Pc;
plot3(P_pre(:,1),P_pre(:,2),P_pre(:,3),'*')
hold on
plot3(Q_pre(:,1),Q_pre(:,2),Q_pre(:,3),'*')
plot3(P_tilde(:,1),P_tilde(:,2),P_tilde(:,3),'*')
grid on
zlim([0 2])
xlabel('x')
ylabel('y')
zlabel('z')
hold off

%
%% 2.3 Image compression using SVD
% Consider the following image

rgbImage = imread('peppers.png');
grayImage = rgb2gray(rgbImage);
imshow(grayImage)
X=double(grayImage);

% Compute SVD of the matrix \mathbf{X}.

[U,S,V] = svd(X);

% Reconstruct the image using only the first \mathbf{r} singular values. Think about how much memory could be safed by using this approximation of the image.

r=30;
X_rec = U(:,1:r) * S(1:r,1:r) * V(:,1:r)';
gray_rec=uint8(X_rec);
figure
imshow(gray_rec)

% Change \mathbf{r} to see different approximations. What is the maximum value to which you can set \mathbf{r} to still spare some parameters compared to storing the pixels of the original image?
% Do you know any other applications of SVD?