79 lines
2.2 KiB
Mathematica
79 lines
2.2 KiB
Mathematica
|
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);
|