clear all;
%%% Programma, per determinare quale matrici fondamentali sono da
%%% calcolare! cerchiamo di minimizzare l'errore, facendo l'operazione
%%% spesso con una telecamera nota.

%% Tutta questa prima parte trova la matrice di adiacenza!
% è utile, perchè dobbiamo sapere tra quali due tc possiamo calcolarci la F

n = 5;                      % numero di tc

A = zeros(n,n);             % matrice di adiacenza

k = [1,4];                  % tc note

% supponiamo per le prove t=5;

t = 5;                      % numero di frame acquisiti da ogni tc

%%% le informazioni per la var frame bisogna trovare con un alg di visione 
%   che spero esista già 


%%% per la simulazione del codice creo dei dati sintetici:
%        - prima riga: indica se il marker si trova in un frame
%        - seconda riga: 1 per tc nota 

%%% frame contiene tutti i frame acquisiti entro un tempo t: quindi i
% frame da 1:n, sono quelli al tempo t=1, n+1:2n, tempo t=2 e cosi via fino
% a n*(t-1):t*n

frame = [1 1 0 0 0 0 1 1 0 0 0 0 1 1 0 0 0 0 1 1 0 0 0 0 0;
         1 0 0 1 0 1 0 0 1 0 1 0 0 1 0 1 0 0 1 0 1 0 0 1 0 ];

% framet è una variabile che contiene due info: 
%           - marker all'istante t è nel frame
%           - la tc è nota


for(h=1:t)
    
    % con questo ciclo scelgo sempre i frame delle 5 tc al variare di t
    framet = [];
    switch h
        case 1
            for(l =h:h+(n-1))
            framet = [framet frame(:,l)];
            end
        case 2
            for(l =((h-1)*n+1):h*n)
            framet = [framet frame(:,l)];
            end
        case 3
            for(l =((h-1)*n+1):h*n)
            framet = [framet frame(:,l)];
            end
        case 4
            for(l =((h-1)*n+1):h*n)
            framet = [framet frame(:,l)];
            end
        case 5
            for(l =((h-1)*n+1):h*n)
            framet = [framet frame(:,l)];
            end
    end
    
    % adesso cerco di estrarre dai frame la matrice di adiacenza
    
    for(i=1:n)
        for(j=1:n)
            if ((framet(1,i) ==1) && (framet(1,j) == 1))
                if (framet(2,i) == 1)
                    A(i,j)=1;                   % se una delle due tc è nota
                else
                    if (framet(2,j) == 1)       % allora asssegniamo un peso minore al arco=1
                        A(i,j)=1;
                    else
                        A(i,j)=3;               % altrimenti provo con 3
                    end
                end
            end
        end
    end
end

A


% Ora salvato tutto in una matrice di adiacenza, che descrive il nostro
% grafo, cerchiamo i cammini minimi tra tutti i vertici (tc)!

% inizializziamo l'alg: la tc 1 è il nostro vertice di partenza

% S insieme dei nodi visitati
% T insieme nodi rimanenti
% J(i) nodo precedente nel cammino minimo
% f(i) peso totale del cammino
% p(ij) peso del arco da i - j

% S = [1];
% T = [2,3,4,5];
% 
% 
% for(i=0; i<n; i++) 
%     d[i] = INT_MAX;
% d[x] = 0;
% for(;;) {
% for(m=INT_MAX, i=0; i<n; i++)
% if (!v[i] && d[i] <= m) m = d[j = i];
% v[j] = 1;
% if (j == y || m == INT_MAX) break;
% for(i=0; i<n; i++)
% if (G[j][i] && d[i] > d[j] + G[j][i])
% 4
% d[i] = d[j] + G[j][i];
% }


%% Generazione del errore per aggiungere al valore esatto dei pti

% Però bisogna dimostrare in qlc modo perchè posso simulare l'errore come
% una gaussiana.....!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
% varianza dell'errore gaussiano
var_x = 10;

% errore bianco da aggiungere ad ogni pto

n = var_x.*randn(3,240);



%% Plot delle traiettorie dei pti scelti

% Pti trovati da due immagini, ma cambiate per avere un errore
% minore del 0.001 sulla F
x1 = [312,260,184,144,170,225,241,255,265,220,190,138; 
      289,311,246,196,176,226,226,190,152,113,133,94;
      1,1,1,1,1,1,1,1,1,1,1,1];
x2 = [335.9,256.8,129.5,39,116.8,179.8,256.8,256.2,256.8,200.5,142.9,59.14;
      263.5,297,268.2,195.18,154.3,201.9,233.4,192.5,138.2,62.5,68.5,16.9;
      1,1,1,1,1,1,1,1,1,1,1,1];


% Punti scelti nel programma pov-ray
%x1_pov = [-3,0,4,5,3,2,0,0,0,1,2,3;-5,-7,-3,1,2,0,-2,0,2,4,4,5;0,0,0,0,0,0,0,0,0,0,0,0];

%plot(x1_pov(1,:),x1_pov(2,:))
%hold on;

 
% provo a dividere e spostare i pti sull'immagine per farli coincidere con 
% quelli scelti in pov-ray

%x11 = x1/35;

% for(i=1:length(x11))
%     x11(1,i)= x11(1,i) -5;
%     x11(2,i)=x11(2,i) -4;
% end
% plot(-x11(1,:),-x11(2,:),'red')

% ora sommo ad ogni pto il rumore bianco
% mi creo 10 coppie di pti con i quali mi calcolo 10 F diverse

for (k=0:9)
    h=k*12;
    for(i=1:12)
        x11(1,h+i) = x1(1,i) + n(1,h+i);
        x11(2,h+i) = x1(2,i) + n(2,h+i);
        x11(3,h+i) = 1;
        x22(1,h+i) = x2(1,i) + n(1,120+h+i);
        x22(2,h+i) = x2(2,i) + n(2,120+h+i);
        x22(3,h+i) = 1;
    end

end  

%% Calcolo della matrice fondamentale
% F=[];
% e1=[];
% e2=[];

F=[];
% adesso mi calcolo le 10 F con le 10 coppie diverse di pti
for (i=0:9)
    h=i*12;
    for(k=1:12)
        t1(1,1)=[x11(1,h+k)];
        t2(1,1)=[x22(1,h+k)];
        for(j=2:3)
           t1(1,j) = [x11(j,h+k)];
           t2(1,j) = [x22(j,h+k)];
        end
        x1_tmp(:,k) = t1';
        x2_tmp (:,k)= t2';
    end
    [G e1 e2]=fundmatrix(x1_tmp,x2_tmp);
F=[F G];
   
end

% controllo se la matrice è esatta
%zero=x1a'*F(1)*x1b
%zero2=(F(1)')*e1

%calcolo la media delle matrici F trovate

M=0*eye(3);
for i=0:3:length(F)-3
    M= M+ [F(:,1+i) F(:,2+i) F(:,3+i)];
    
end    

media=M/(length(F)/3);

% Quindi per migliorare i risultati si potrebbe provare anche a fare un ransac 
% sui punti!