%B. Vandeportaele 2015
%based on http://www.diegm.uniud.it/fusiello/demo/rect/
%http://www.diegm.uniud.it/fusiello/papers/00120016.pdf

close all
%calcul des matrices caméra
c_1=calc_c(Tc_1,omc_1,fc,cc)
c_2=calc_c(Tc_2,omc_2,fc,cc)
%calcul des matrices rotations et caméra intrinsèque 
R1=rodrigues(omc_1);
R2=rodrigues(omc_2);
KK=[fc(1),0,cc(1),0;0, fc(2),cc(2),0;0,0,1,0];

%calcul des positions des centres optiques, idem à CalcBaseline.m
C3_1=c_1(:,1:3); % recupere les 3 premieres colones, sous matrice 3*3 a gauche de c_1
c4_1=c_1(:,4); % 4� colone de c_1
C3_2=c_2(:,1:3); % recupere les 3 premieres colones, sous matrice 3*3 a gauche de c_2
c4_2=c_2(:,4); % 4� colone de c_2
L1=-inv(C3_1)*c4_1
L2=-inv(C3_2)*c4_2

% new x axis (= direction of the baseline)
v1 = (L1-L2);
% new y axes (orthogonal to new x and old z)
%en utilisant le vecteur z d'une seule caméra
%v2 = cross(R1(3,:)',v1);
%on calcule un vecteur moyen Z des 2 caméras
zmoy=(R1(3,:) + R2(3,:)) /2;
zmoy=zmoy/norm(zmoy,2)
v2 = cross(zmoy',v1);

% new z axes (orthogonal to baseline and y)
v3 = cross(v1,v2);

% new extrinsic parameters
R = [v1'/norm(v1)
     v2'/norm(v2)
     v3'/norm(v3)];
% translation is left unchanged
% new intrinsic parameters (arbitrary)
%calcul des 2 matrices d'homographie pour générer des images rectifiées,
%sans tenir compte des offsets dans l'image
[H1,H2]=computeHomographiesForEpipolarRectification(2500,400,300,0,0,0,R,L1,L2,c_1,c_2);
 
%lit les images d'entrée
nomimin1='imaobj1_rect.jpg'; %nom du fichier image contenant l'image 1 sans les distorsions optiques
imin1=imread(nomimin1); 
imin1=imin1(:,:,1);
[hautin,largin]=size(imin1); 

nomimin2='imaobj2_rect.jpg'; %nom du fichier image contenant l'image 2 sans les distorsions optiques
imin2=imread(nomimin2); 
imin2=imin2(:,:,1); 

%adaptation taille d'image et offset pour calculer les images rectifiées
p2D=[1,1; largin,1; largin,hautin;1,hautin]
listeP2D2=[];
for nbpt=1:4
p2=H1^-1*[p2D(nbpt,1);p2D(nbpt,2);1];
p2=p2/p2(3);
listeP2D2=[listeP2D2,p2];
end
for nbpt=1:4
p2=H2^-1*[p2D(nbpt,1);p2D(nbpt,2);1];
p2=p2/p2(3);
listeP2D2=[listeP2D2,p2];
end

%listeP2D2
maxu=max(listeP2D2(1,:))
maxv=max(listeP2D2(2,:))
maxu1=max(listeP2D2(1,1:4))
maxu2=max(listeP2D2(1,5:8))

minu=min(listeP2D2(1,:))
minv=min(listeP2D2(2,:))
minu1=min(listeP2D2(1,1:4))
minu2=min(listeP2D2(1,5:8))

%paire rectifiée,  
%calcul des tailles pour les images 
hautout=ceil(maxv-minv)
largout1=ceil(maxu1-minu1)
largout2=ceil(maxu2-minu2)

%calcul des offsets pour les homographies
offv=-minv
offu1=-minu1
offu2=-minu2

%calcul des 2 matrices d'homographie pour générer des images rectifiées,
%avec ajout des offsets aux homographies
[H1,H2]=computeHomographiesForEpipolarRectification(2500,400,300,offu1,offu2,offv,R,L1,L2,c_1,c_2);
 
 
%genere la paire d'images rectifiées épipolairement
imout1=zeros(hautout,largout1);
imout1=CalculeImageHomographie(H1,imout1,hautout,largout1,imin1,hautin,largin) ; 
imout2=zeros(hautout,largout2);
imout2=CalculeImageHomographie(H2,imout2,hautout,largout2,imin2,hautin,largin) ; 
%création d'une image composite
imcompose=[imout1, imout2];
%affichage de lignes horizontales noires pour vérification visuelle
for v=1:50:hautout
    imcompose(v,:)=zeros(1,largout1+largout2);
end
imwrite(uint8(imcompose),'imcompose.jpg','jpg');


%affichage
figure
subplot(2,2,1);
imagesc(imin1)
subplot(2,2,2);
imagesc(imin2)
subplot(2,2,3);
imagesc(imout1)
axis equal
axis ij
colormap('Gray')
subplot(2,2,4);
imagesc(imout2)
axis equal
axis ij
colormap('Gray')

%les zones de l'image ne respectant pas la contrainte épipolaire
%correspondent aux zone n'ayant pas subit le même mouvement que la mire et
%l'objet.