clc
clear

[PPMcalib mcalib Mcalib]=PPMcalib()
%[PPMrel mrel Mrel]=PPMrel()
%[PPMsec msec Msec]=PPMseconda()
mrel=puntirel()
msec=puntiseconda()

%PPM(:,:,1)=PPMrel
%PPM(:,:,2)=PPMsec
%m(:,:,1)=mrel
%m(:,:,2)=msec
%S = intersect_le(PPM,m)




%[Krel Rrel trel]=art(PPMrel);

%[Kseconda Rseconda tseconda]=art(PPMsec);
[Kcalib Rcalib tcalib]=art(PPMcalib);
Kcalib=[687.4010 0 491;0 687.4010 276;0 0 1]

Kseconda=Kcalib
Krel=Kcalib

%Rcalib=Rrel  %%%%%%%%%%%%%%%%%%%%%%importantissimo
%tcalib=trel
%Krelcalc=Krel
%Kseccalc=Kseconda

%f=161
%Krel=[6.521*f 0 491;0 6.521*f 276;0 0 1]
%Kseconda=Krel
%Krel=Krelcalc
%Kseconda=Kseccalc
%Krel=(Kseccalc+Krelcalc)/2
%Kseconda=Krel
%Kgiusta=[638 0 491;0 638 276;0 0 1]
%%%%%%%%%%%%%%%%%%%%%%%%Metto Rcalib perfetta e anche tcalib perfetta
%  angle(1)=pi/4
%  angle(2)=0
%  angle(3)=-pi/2
%  Rcalib=eul(angle,'RPY')
%  
%  tcalib(1)=0
%  tcalib(2)=4
%  tcalib(3)=sqrt(2)*23
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[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')
Gcalib=[Rcalib tcalib;0 0 0 1];


F=fm(mrel,msec)
[Rot,tras]=sr(F,Krel,Kseconda,mrel,msec) %la traslazione è trovata a  meno di un fattore di scala




%%%%%%%%%%%%%%calcolo Baseline
E=Kseconda'*F*Krel

base=0.5*trace(E*E')*eye(3)-E*E'
riga=base(1,:)/sqrt(base(1,1))
%basex=sqrt(base(1,1))
%basey=sqrt(base(2,2))
%basez=sqrt(base(3,3))
%tracciarad=sqrt(base(1,1)+base(2,2)+base(3,3))
norm(riga)
%base
% cros12=cross(E(:,1),E(:,2))
% cros31=cross(E(:,3),E(:,1))
% cros23=cross(E(:,2),E(:,3))
% base1=(cros12/sqrt(cros12'*cros12))*sqrt(0.5*trace(E*E'))
% base2=(cros31/sqrt(cros31'*cros31))*sqrt(0.5*trace(E*E'))
% base3=(cros23/sqrt(cros23'*cros23))*sqrt(0.5*trace(E*E'))
% norm(base1)
% norm(base2)
% norm(base3)
%%%%%%%%%%%%%%%%%%%%%%%%Metto Rot perfetta e tras perfetta
%   angle(1)=0
%   angle(2)=pi/4
%   angle(3)=pi/4
%   Rot=eul(angle,'RPY')
% 
%  %Per tornare indietro
% C_2nuo=[3;1;-29]/sqrt(613)
% C_2nuo(1)=-1/sqrt(613)
% C_2=Rcalib*C_2nuo
% tras=-Rot*C_2
% 
% %tras=tras*sqrt(613)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Gtrac=[Rot tras;0 0 0 1]

%  [acalib,b]=ieul(Rot);
%  disp('calibrata')
%  strcat('H',num2str(acalib(1)),'P',num2str(acalib(2)),'B',num2str(acalib(3)),'X',num2str(tras(1)),'Y',num2str(tras(2)),'Z',num2str(tras(3)),'E')

%%%%%%calcolo fattore di scala




%hrel=11.765
%hsec=5.947
%esatte
hrel=8  %169 zoom
hsec=16 %147 zoom
difh=hsec-hrel

%CONTI GIUSTI
% fattsc=3.927/tras(1)
% fattsc=33.7
% tras=tras*fattsc


%METODO RAGAZZO
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)) %che sarebbe la radicequadrata della traccia della base

[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')


%sqrt(((trel(1)-tseconda(1))^2)+((trel(2)-tseconda(2))^2)+((trel(3)-tseconda(3))^2))
%t+RC_2=0 allora t=-RC_2 è chiaro che C_2 è conosciuto a meno di una costante visto che lo è t.Io voglio trovare C_2 solidale con il primo sistema di riferimento solidale alla telecamera calibrata
%mentre le altezze sono date rispetto al riferimento del pavimento. 
%allineando dunque i sistemi di riferimento delle 2 telecamere secondo l'asse Y parallelo a quello del pavimento 
% Gtraci=inv(Gtrac)
% %METODO MATEMATICA B
%   C_2=inv(Gtrac)*[0;0;0;1]
%   C_2(4,:)=[] %calcolo c_2 nelle coordinate della camera1
%   Gtraci=Rtilde*C_2
%  c_x=Gtraci(1,1)
%  c_y=Gtraci(2,1)
%  c_z=Gtraci(3,1)
%  X=-difh
%  Y=(X*c_y)/c_x
%  Z=(X*c_z)/c_x
%  fin=-Rot*(inv(Rtilde)*[X;Y;Z])
%  distanza=sqrt((X^2)+(Y^2)+(Z^2))
 
 %[prim]=Rcalib*[X;Y;Z]
 %X=prim(1)
 %Y=prim(2)
 %Z=prim(3)
 
 %baseline=[X;Y;Z]
 %  baseline=[sqrt(base(1,1));sqrt(base(2,2));sqrt(base(3,3))]
%Rot*baseline
   
   %    [Rraffin,traffin] = horn(Krel,Kseconda,mrel,msec,Rot,baseline)
% %  
%    traffin=traffin*norm(baseline)
% %   
% Rraffin=real(Rraffin)
%    [araffin,b]=ieul(Rraffin);
%  disp('finraffin')
%  strcat('H',num2str(araffin(1)),'P',num2str(araffin(2)),'B',num2str(araffin(3)),'X',num2str(traffin(1)),'Y',num2str(traffin(2)),'Z',num2str(traffin(3)),'E')
% % 




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%per passare dalle telecamere a
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%riferimento camera calibrata
[arisp,b]=ieul(Rot*Rcalib);
%angolirel = convang(arot,'rad','deg')
disp('ripetto calibrato')
strcat('H',num2str(arisp(1)),'P',num2str(arisp(2)),'B',num2str(arisp(3)),'X','0','Y','0','Z','0','E')

rotazione=[-pi/2 0 0]
Rig = eul(rotazione,'RPY')
[arig,b]=ieul(Rig*Rot*Rcalib);
%angolirel = convang(arot,'rad','deg')
disp('ruotato ancora')
strcat('H',num2str(arig(1)),'P',num2str(arig(2)),'B',num2str(arig(3)),'X','0','Y','0','Z','0','E')