TP-probleme-inverse-3D/TP2/sfm.m
2023-06-25 16:38:01 +02:00

136 lines
3.6 KiB
Matlab

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);