clc
clear

%%%%%calibrazione prima telecamera
[PPMcalib mcalib Mcalib]=PPMcalib()
%recupero punti coiniugati
mrel=puntirel()
msec=puntiseconda()



%%%%%%calcolo parametri intrinseci ed estrinseci telecamera calibrata
[Kcalib Rcalib tcalib]=art(PPMcalib);

%%%%%% supponiamo telecamere con stessi parametri intrinseci
Kseconda=Kcalib
Krel=Kcalib
%%%%%%simulazione telecamera calibrata
[acalib,b]=ieul(Rcalib);
angolicalib = convang(acalib,'rad','deg')
disp('calibrata')
strcat('H',num2str(acalib(1)),'P',num2str(acalib(2)),'B',num2str(acalib(3)),'X',num2str(tcalib(1)),'Y',num2str(tcalib(2)),'Z',num2str(tcalib(3)),'E')

%matrice decalibrata degli intrinseci
Gcalib=[Rcalib tcalib;0 0 0 1];
%%%%%%calcolo e decomposizione matice fondamentale
F=fm(mrel,msec)
[Rot,tras]=sr(F,Krel,Kseconda,mrel,msec) %la traslazione è trovata a  meno di un fattore di scala


%%%%%%%%%%%%%%calcolo Baseline metodo di horn
%E=Kseconda'*F*Krel
%base=0.5*trace(E*E')*eye(3)-E*E'

%estrinseci tra calibrata e non calibrata a meno di un fattore di scala
Gtrac=[Rot tras;0 0 0 1]




%%%%%altezze telecamere

hrel=8  %169 zoom
hsec=16 %147 zoom
difh=hsec-hrel

%%%%%%%%%%%%CALCOLO BASELINE

C_2=inv(Gtrac)*[0;0;0;1]
C_2(4,:)=[] %calcolo g_2 nelle coordinate della camera1

Rtilde=inv(Rcalib)
  
%C_2=-inv(Rot)*tras %oppure C_2 si puo calcolare anche cosi
C_2nuo=Rtilde*C_2
fatsc=difh/C_2nuo(1)
C_2ver=C_2nuo*fatsc
C_2verin=inv(Rtilde)*C_2ver
tras=-Rot*C_2verin
%tras=tras*sqrt(trace(base)) % SE SI UTILIZZA HORN

%Simulazione telecamera non calibrata rispetto calibrata
[arot,b]=ieul(Rot);
angolirel = convang(arot,'rad','deg')
disp('fin')
strcat('H',num2str(arot(1)),'P',num2str(arot(2)),'B',num2str(arot(3)),'X',num2str(tras(1)),'Y',num2str(tras(2)),'Z',num2str(tras(3)),'E')

