%--------------------------------------------------------
% Esperienza di laboratorio 1 - 01/02/2011
% Francesca Odone - odone@disi.unige.it
% basata sul "Structure and Motion Toolkit in Matlab" di P. Torr
%--------------------------------------------------------
% Disponibile su
% Matlab central
% http://www.mathworks.com/matlabcentral/fileexchange/4576-structure-and-motion-toolkit-in-matlab
% e sulla pagina web di Philip Torr
% http://cms.brookes.ac.uk/staff/PhilipTorr/

disp('--------------------------------------------------------------');
disp('Esperienza di laboratorio 1 - 01/02/2011');
disp('Francesca Odone - odone@disi.unige.it');
disp('basata sul "Structure and Motion Toolkit in Matlab" di P. Torr');
disp('--------------------------------------------------------------');

%--------------------------------------------------------
% Settaggio dei parametri
%--------------------------------------------------------

%profile on
clear all;


%--------------------------------------------------------
% Generazione dei dati sintetici
%--------------------------------------------------------

disp('--------------------------------------------------------------');
disp('Generazione di punti sintetici e della ground truth');
disp('--------------------------------------------------------------');

% terza componente
m3 = 256;

% utilizzo valori di default
[true_F,x1,y1,x2,y2,nx1,ny1,nx2,ny2,true_C,true_R,true_TX, true_E, true_X, true_t] = torr_gen_2view_matches;

% calcolo delle matrici di proiezione delle due telecamere
[true_P1, true_P2] = torr_RCT2P(true_C,true_R, true_t);

% visualizza la struttura 3D (reale)
torr_display_structure(true_X, true_P1, true_P2,1);
title('punti reali');

no_matches = length(nx1);

% scelgo se impostare rumore su punti generati 
% no_noise = 1 nessun rumore 
% no_noise = 0 rumore gaussiano con stddev = 1 pixel
no_noise = 1;

if (no_noise)
    nx1 = x1;
    nx2 = x2;
    ny1 = y1;
    ny2 = y2;
    disp('Generati dati privi di rumore');
else disp('Generati dati rumorosi');
end

disp('Matrice delle corrispondenze tra le due viste [x1, y1, x2, y2]');

% matrice dei punti che utilizziamo per la ricostruzione
matches = [nx1,ny1,nx2,ny2]

% matrice dei punti di ground truth (senza rumore)
perfect_matches = [x1,y1,x2,y2];

pause;

%--------------------------------------------------------
% Stimo la matrice fondamentale F
%--------------------------------------------------------

disp('--------------------------------------------------------------');
disp('Stima della matrice fondamentale');
disp('--------------------------------------------------------------');

% Metodo di calcolo della matrice Fondamentale
method = 'mapsac';
%method = 'linear';

disp('Metodo utilizzato:');
method

% parametro impostato a 1 se vogliamo imporre che la matrice F abbia rango
% 2 (utilizzato della stima di F)
set_rank2 = 1;

if (set_rank2==1)
    disp('Impongo il rango della matrice F a 2');
end

% stima di F 
[f, e1, n_inliers,inlier_index,nF] = torr_estimateF( matches, m3, [], method, set_rank2);

disp('Numero di punti e numero di inlier');
no_matches
n_inliers

disp('--------------------------------------------------------------');
disp('Visualizzo la geometria epipolare: ');
disp('Fare click sui punti di un immagine e la linea epipolare corrispondente ');
disp('comparirà sull altra...');
disp('[in rosso le linee epipolari reali - in verde quelle stimate]');
disp('--------------------------------------------------------------');

% visualizzo le due immagini e (interattivamente) le linee epipolari
torr_compare_epipoles(true_F, nF,matches,m3);

disp('--------------------------------------------------------------');
disp('Verifica dell errore di riproiezione');
disp('--------------------------------------------------------------');

%verifica l'errore di riproiezione sul vincolo x'Fx=0
e2 = torr_errf2(f, matches(:,1), matches(:,2), matches(:,3),matches(:,4), length(nx1), m3);

disp('errore medio');
e2

disp('errore minimo');
min(e2)
disp('errore massimo');
max(e2)
disp('errore medio sui punti');
mean(e2)



%-------------------------------------------------------------
% OPZIONALE: Algoritmo di Sampson per la riduzione dell'errore 
%-------------------------------------------------------------
% 
% % correggo i match in modo che rispettino il vincolo x'T F x=0
% [corrected_matches error2] = torr_correctx4F(f, nx1,ny1,nx2,ny2, no_matches, m3);
% 
% %verifica l'errore dopo la correzione
% %i match corretti dovrebbero avere un errore molto piccolo
% e2 = torr_errf2(f, corrected_matches(:,1), corrected_matches(:,2), corrected_matches(:,3),corrected_matches(:,4), length(nx1), m3);
% 
% disp('errore medio dopo la correzione dei match');
% e2

