136 lines
3.6 KiB
Matlab
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);
|