%%%%%%%  SINTESI DI UNA CAMERA  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% Generiamo un modello sintetico di camera:
%

% Genero N punti casuali in uno spazio di scena [-5000 < x,y,z < +5000] 
N = 10;   % lavoro con 10 punti
P = ( rand(3,N) - 0.5 ) * 10000 

% Vediamo i punti nel loro spazio di scena
figure(1);
plot3(P(1,:),P(2,:),P(3,:), 'o'); grid;
xlabel('asse X'); ylabel('asse Y'); zlabel('asse Z');

% Definisco una camera allineata alla terna di scena (niente rotazione) e
% distante 12000 pixel lungo l'asse Z (tz = 12000)
R = eye(3) % Matrice identità 3x3
T = [ 0 ; 0 ; 12000 ]

% A partire da R e T costruisco la matrice estrinseca E
E = [ R T ; 0 0 0 1]

% Costruisco la matrice di calibrazione intrinseca K, scegliendo f = 2000
% (pixel) e immaginando una piano immagine di 3000x2000 pixel, con il
% centro ottico nel mezzo, quindi: xc = 1500, yc = 1000 (pixel)
K = [2000 0 1500 0; 0 2000 1000 0; 0 0 1 0]

% Costruisco la matrice di proiezione M, prodotto di K ed E
M = K*E

% Mediante la matrice M, proietto i punti di scena P, generando i punti
% immagine pim. Per farlo, devo passare prima in coordinate omogenee (che 
% chiamo Ptilde e pimtilde)
Ptilde = [ P; ones(1,N)]   % punti di scena in coordinate omogenee
pimtilde = M * Ptilde       % proietto
pim = [ pimtilde(1,:)./pimtilde(3,:) ;  pimtilde(2,:)./pimtilde(3,:)] 
                            % coordinate immagine euclidee

% controllo la proiezione: genero l'ipotetica immagine degli N punti e
% traccio anche il rettangolo che rappresenta il campo visivo della camera
figure(2);
plot( pim(1,:),pim(2,:),'x' , [0;3000;3000;0;0],[0;0;2000;2000;0],'r'); 


%%%%%%%  CALIBRAZIONE  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% Proviamo ora a calibrare utilizzando l'algoritmo lineare. 
% Partiamo dalla conoscenza di P e pim e cerchiamo di determinare M
%

% Algoritmo lineare: costruisco la matrice del sistema lineare 2N x 12 (che
% chiamo Psol) come da algoritmo:
Psol = []; 
for i = 1:N, 
    Psol=[ Psol; ...
           Ptilde(:,i)'  [0 0 0 0]       -pim(1,i)*Ptilde(:,i)'; ...
           [0 0 0 0]     Ptilde(:,i)'    -pim(2,i)*Ptilde(:,i)' ]; 
end;

% Calcolo il vettore 12x1 msol, soluzione del sistema Psol msol = 0, i cui
% 12 elementi sono le 3 righe della matrice incognita M [3x4]
[w,d]=eig(Psol'*Psol)
msol = w(:,1)

% Ricompongo la matrice M ottenuta dal calcolo, a partire dal vettore msol
Mcal = [ msol(1:4,1)' ; msol(5:8,1)'; msol(9:12,1)']

% Mcal deve risultare uguale alla matrice di partenza, M, a meno di un
% fattore di scala. Per confrontare quindi M con Mcal, le devo normalizzare
% a un fattore di scala comune. Scelgo, ad esempio, di riscalarle entrambe
% in modo da avere M(1,4)=1000
Mcal * 100 ./ Mcal(1,4)
M * 100 ./ M(1,4)

pause;

%%%%%%%  ESTRAZIONE dei PARAMETRI di CAMERA  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
%

% Estraggo A e b dalla matrice M stimata (Mcal)
A = Mcal(:,1:3)
b = Mcal(:,4)

% Calcolo rho e la terza riga di R, r3:
rho = 1 / norm( A(3,:) )
r3 = rho * A(3,:)

% Calcolo i parametri intrinseci: f, xc, yc
xc = rho^2 * dot(A(1,:),A(3,:))
yc = rho^2 * dot(A(2,:),A(3,:))
f  = rho^2 * norm( cross(A(2,:),A(3,:)) )

% Calcolo i parametri estrinseci: matrice di rotazione R
r1 = rho^2 / f * cross(A(2,:),A(3,:))
r2 = -rho^2 / f * cross(A(1,:),A(3,:))
R = [ r1 ; r2 ; r3 ]

% Calcolo i parametri estrinseci: vettore di traslazione t
tz = b(3) * rho
tx = 1/f * (rho*b(1) - xc*tz)
ty = 1/f * (rho*b(2) - yc*tz)
t = [tx ; ty ; tz]
