clear;
close all;

load donnees_appariees;
load E_estimee;

% Coordonnées homogènes des pixels appariés :
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');

% Estimation de la pose (4 solutions) :
[t_4,R_4] = estimation_4_poses(E_estimee);

% Reconstruction 3D (4 solutions) :
figure('Name','Reconstruction 3D : 4 solutions');
for i = 1:4
	t = t_4(:,i);
	R = R_4(:,:,i);
	Q = reconstruction_3D(w_1,w_2,t,R);
	affichage_3D(I_1,p_1_apparies,Q,t,R,i);
end

input('Tapez Entree pour afficher la bonne solution !');

% Détermination de la "bonne" pose :
[t,R] = estimation_pose(t_4,R_4,w_1,w_2);

% Reconstruction 3D de la "bonne" solution :
Q = reconstruction_3D(w_1,w_2,t,R);

% Affichage de la "bonne" solution :
figure('Name','Reconstruction 3D');
affichage_3D(I_1,p_1_apparies,Q,t,R);