clear;
close all;

load donnees_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 du couple (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);

	% Estimation de l'échelle :
	if i>1
		% Paires de points d'intérêt associées à la paire d'images {I_{i-1},I_i}} :
		p_1_tilde_i_moins_1 = p_1_tilde_complet{i-1};
		[~,indices_i_moins_1,indices_i] = intersect(round(p_1_tilde_i_moins_1),round(p_1_tilde),'rows');

		% Estimation du facteur d'échelle :
		alpha_estime = estimation_echelle(Q_i_moins_1(indices_i_moins_1,:),w_2(indices_i,:),t,R);
		t = alpha_estime*t;
	end

	% 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
	Q_i_moins_1 = nouveaux_Q;

	% La couleur d'un point 3D est sa couleur dans l'image gauche :
	I_1 = images{i};
	for j = 1:size(p_1_tilde,1)
		currentColor = I_1(round(p_1_tilde(j,2)),round(p_1_tilde(j,1)),:);
		couleurs_points = [ couleurs_points ; ipermute(currentColor,[1,3,2])];
	end

	% Concaténation des changements de pose :
	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);