clear;
close all;

% Choix des images :
dossierImages = 'Fontaine/';
extension = '.png';

% Paramètres intrinsèques de la caméra :
nomFichier = strcat(dossierImages,'matK.mat');
load(nomFichier);
inverse_K = inv(K);

% Nombre d'images utilisées :
n = 4;

% Utilisation des régions d'intérêt :
ROI = 0;

% Lecture des images :
for i = 1:n
	nom = strcat(dossierImages,int2str(i),extension);
	images{i} = imread(nom);
end

% Estimation des n-1 matrices essentielles :
matrices_E = [];
for i = 1:n-1

	% Lecture des images i et i+1 :
	I_1 = images{i};
	I_2 = images{i+1};

	% Conversion en niveaux de gris :
	I_1_nvg = rgb2gray(I_1);
	I_2_nvg = rgb2gray(I_2);

	% Détection des points d'intérêt dans l'image gauche :
	if ROI
		RI_1 = selection_RI(I_1);
		p_1 = detectMinEigenFeatures(I_1_nvg,'ROI',RI_1,'MinQuality',0.0001);
	else
		p_1 = detectMinEigenFeatures(I_1_nvg,'MinQuality',0.0001);
	end

	% Mise en correspondance avec l'image droite :
	traqueur = vision.PointTracker('MaxBidirectionalError',1,'NumPyramidLevels',5);
	p_1 = p_1.Location;
	initialize(traqueur,p_1,I_1);
	[p_2,indices_droite] = step(traqueur,I_2);
	p_1_apparies = p_1(indices_droite,:);
	p_2_apparies = p_2(indices_droite,:);
	
	% Coordonnées homogènes des points image dans les deux repères :
	nb_paires = size(p_1_apparies,1);
	p_1_tilde = [p_1_apparies ones(nb_paires,1)];
	p_2_tilde = [p_2_apparies ones(nb_paires,1)];
	w_1 = transpose(inverse_K*p_1_tilde');
	w_2 = transpose(inverse_K*p_2_tilde');

	% S'il y a trop de paires, on n'en conserve que 10000 :
	nb_paires_majore = min(nb_paires,10000);
	indices = randperm(nb_paires);
	indices_select = indices(1:nb_paires_majore);
	w_1_select = w_1(indices_select,:);
	w_2_select = w_2(indices_select,:);

	% Estimation robuste de E :
	E_estimee = estimation_E_robuste(w_1_select,w_2_select);

	matrices_E = cat(3,matrices_E,E_estimee);
	p_1_tilde_complet{i} = p_1_tilde;
	p_2_tilde_complet{i} = p_2_tilde;
  
end

save donnees_SfM;

input('Tapez Entree pour lancer le SfM !');

% Liste de couleurs :
couleurs = uint8([ 0 255 255 ; 255 0 0 ; 0 255 0 ; 0 0 255 ; 255 255 0 ; 0 0 0 ]);

% Initialisations :
t_1 = zeros(3,1);
R_1 = eye(3);
liste_t = t_1;
liste_R = R_1;
Q = [];
couleurs_points = [];

% SfM à n > 2 images :
for i = 1:n-1

	% Estimation de (t,R) :
	E_estimee = matrices_E(:,:,i);
	p_1_tilde = p_1_tilde_complet{i};
	p_2_tilde = p_2_tilde_complet{i};
	w_1 = transpose(inverse_K*p_1_tilde');
	w_2 = transpose(inverse_K*p_2_tilde');
	[t_4,R_4] = estimation_4_poses(E_estimee);
	[t,R] = estimation_pose(t_4,R_4,w_1,w_2);

	% Reconstruction 3D (méthode "du point milieu", cf. TP1) :
	nouveaux_Q = reconstruction_3D(w_1,w_2,t,R);

	% Points 3D reconstruits, exprimés dans le repère de la caméra i+1 :
	if i==1
		Q = nouveaux_Q;
	else
		Q = [ (R*Q')'+repmat(t',size(Q,1),1) ; nouveaux_Q ];
	end

	% Une couleur différente est affectée à chaque nouveau nuage de points 3D :
	couleurs_points = [ couleurs_points ; repmat(couleurs(i+1,:),size(nouveaux_Q,1),1) ];

	liste_t = cat(2,liste_t,t);
	liste_R = cat(3,liste_R,R);
end

% Réorganisation des caméras pour l'affichage :
couleurs_cameras = [ couleurs(1:n,:) ; 255 255 255 ];
for i = 1:size(liste_t,2)
	if i==1
		liste_t_affichage = t_1;
		liste_R_affichage = R_1;
	else
		liste_R_affichage(:,:,i) = liste_R(:,:,size(liste_t,2)-i+2)'*liste_R_affichage(:,:,i-1);
		liste_t_affichage(:,i) = -liste_R_affichage(:,:,i)*liste_t(:,size(liste_t,2)...
									-i+2)+liste_t_affichage(:,i-1);
	end
end

% Affichage des nuages de points 3D colorés :
figure('Name','Reconstruction 3D par SfM');
affichage_3D_melange(Q,liste_t_affichage,liste_R_affichage,couleurs_points,couleurs_cameras);