%Ricostruisco i punti con MINIMI QUADRATI ELIMINANDO RAGGI 

%Costrusco le telecamere

T1=[a{1}(1,:)' b{1}(1,:)' c{1}(1,:)' d{1}(1,:)' a{1}(2,:)' b{1}(2,:)' c{1}(2,:)' d{1}(2,:)' camera_pos(1,1)*ones(length(a{1}(1,:)),1) camera_pos(1,2)*ones(length(a{1}(1,:)),1) camera_pos(1,3)*ones(length(a{1}(1,:)),1) x_p_as{1}' y_p_as{1}' z_p_as{1}'];
T2=[a{2}(1,:)' b{2}(1,:)' c{2}(1,:)' d{2}(1,:)' a{2}(2,:)' b{2}(2,:)' c{2}(2,:)' d{2}(2,:)' camera_pos(2,1)*ones(length(a{2}(1,:)),1) camera_pos(2,2)*ones(length(a{2}(1,:)),1) camera_pos(2,3)*ones(length(a{2}(1,:)),1) x_p_as{2}' y_p_as{2}' z_p_as{2}'];
T3=[a{3}(1,:)' b{3}(1,:)' c{3}(1,:)' d{3}(1,:)' a{3}(2,:)' b{3}(2,:)' c{3}(2,:)' d{3}(2,:)' camera_pos(3,1)*ones(length(a{3}(1,:)),1) camera_pos(3,2)*ones(length(a{3}(1,:)),1) camera_pos(3,3)*ones(length(a{3}(1,:)),1) x_p_as{3}' y_p_as{3}' z_p_as{3}'];
T4=[a{4}(1,:)' b{4}(1,:)' c{4}(1,:)' d{4}(1,:)' a{4}(2,:)' b{4}(2,:)' c{4}(2,:)' d{4}(2,:)' camera_pos(4,1)*ones(length(a{4}(1,:)),1) camera_pos(4,2)*ones(length(a{4}(1,:)),1) camera_pos(4,3)*ones(length(a{4}(1,:)),1) x_p_as{4}' y_p_as{4}' z_p_as{4}'];
T5=[a{5}(1,:)' b{5}(1,:)' c{5}(1,:)' d{5}(1,:)' a{5}(2,:)' b{5}(2,:)' c{5}(2,:)' d{5}(2,:)' camera_pos(5,1)*ones(length(a{5}(1,:)),1) camera_pos(5,2)*ones(length(a{5}(1,:)),1) camera_pos(5,3)*ones(length(a{5}(1,:)),1) x_p_as{5}' y_p_as{5}' z_p_as{5}'];
T6=[a{6}(1,:)' b{6}(1,:)' c{6}(1,:)' d{6}(1,:)' a{6}(2,:)' b{6}(2,:)' c{6}(2,:)' d{6}(2,:)' camera_pos(6,1)*ones(length(a{6}(1,:)),1) camera_pos(6,2)*ones(length(a{6}(1,:)),1) camera_pos(6,3)*ones(length(a{6}(1,:)),1) x_p_as{6}' y_p_as{6}' z_p_as{6}'];
T7=[a{7}(1,:)' b{7}(1,:)' c{7}(1,:)' d{7}(1,:)' a{7}(2,:)' b{7}(2,:)' c{7}(2,:)' d{7}(2,:)' camera_pos(7,1)*ones(length(a{7}(1,:)),1) camera_pos(7,2)*ones(length(a{7}(1,:)),1) camera_pos(7,3)*ones(length(a{7}(1,:)),1) x_p_as{7}' y_p_as{7}' z_p_as{7}'];
T8=[a{8}(1,:)' b{8}(1,:)' c{8}(1,:)' d{8}(1,:)' a{8}(2,:)' b{8}(2,:)' c{8}(2,:)' d{8}(2,:)' camera_pos(8,1)*ones(length(a{8}(1,:)),1) camera_pos(8,2)*ones(length(a{8}(1,:)),1) camera_pos(8,3)*ones(length(a{8}(1,:)),1) x_p_as{8}' y_p_as{8}' z_p_as{8}'];
T9=[a{9}(1,:)' b{9}(1,:)' c{9}(1,:)' d{9}(1,:)' a{9}(2,:)' b{9}(2,:)' c{9}(2,:)' d{9}(2,:)' camera_pos(9,1)*ones(length(a{9}(1,:)),1) camera_pos(9,2)*ones(length(a{9}(1,:)),1) camera_pos(9,3)*ones(length(a{9}(1,:)),1) x_p_as{9}' y_p_as{9}' z_p_as{9}'];
T10=[a{10}(1,:)' b{10}(1,:)' c{10}(1,:)' d{10}(1,:)' a{10}(2,:)' b{10}(2,:)' c{10}(2,:)' d{10}(2,:)' camera_pos(10,1)*ones(length(a{10}(1,:)),1) camera_pos(10,2)*ones(length(a{10}(1,:)),1) camera_pos(10,3)*ones(length(a{10}(1,:)),1) x_p_as{10}' y_p_as{10}' z_p_as{10}'];
T11=[a{11}(1,:)' b{11}(1,:)' c{11}(1,:)' d{11}(1,:)' a{11}(2,:)' b{11}(2,:)' c{11}(2,:)' d{11}(2,:)' camera_pos(11,1)*ones(length(a{11}(1,:)),1) camera_pos(11,2)*ones(length(a{11}(1,:)),1) camera_pos(11,3)*ones(length(a{11}(1,:)),1) x_p_as{11}' y_p_as{11}' z_p_as{11}'];
T12=[a{12}(1,:)' b{12}(1,:)' c{12}(1,:)' d{12}(1,:)' a{12}(2,:)' b{12}(2,:)' c{12}(2,:)' d{12}(2,:)' camera_pos(12,1)*ones(length(a{12}(1,:)),1) camera_pos(12,2)*ones(length(a{12}(1,:)),1) camera_pos(12,3)*ones(length(a{12}(1,:)),1) x_p_as{12}' y_p_as{12}' z_p_as{12}'];
T13=[a{13}(1,:)' b{13}(1,:)' c{13}(1,:)' d{13}(1,:)' a{13}(2,:)' b{13}(2,:)' c{13}(2,:)' d{13}(2,:)' camera_pos(13,1)*ones(length(a{13}(1,:)),1) camera_pos(13,2)*ones(length(a{13}(1,:)),1) camera_pos(13,3)*ones(length(a{13}(1,:)),1) x_p_as{13}' y_p_as{13}' z_p_as{13}'];
T14=[a{14}(1,:)' b{14}(1,:)' c{14}(1,:)' d{14}(1,:)' a{14}(2,:)' b{14}(2,:)' c{14}(2,:)' d{14}(2,:)' camera_pos(14,1)*ones(length(a{14}(1,:)),1) camera_pos(14,2)*ones(length(a{14}(1,:)),1) camera_pos(14,3)*ones(length(a{14}(1,:)),1) x_p_as{14}' y_p_as{14}' z_p_as{14}'];
T15=[a{15}(1,:)' b{15}(1,:)' c{15}(1,:)' d{15}(1,:)' a{15}(2,:)' b{15}(2,:)' c{15}(2,:)' d{15}(2,:)' camera_pos(15,1)*ones(length(a{15}(1,:)),1) camera_pos(15,2)*ones(length(a{15}(1,:)),1) camera_pos(15,3)*ones(length(a{15}(1,:)),1) x_p_as{15}' y_p_as{15}' z_p_as{15}'];
T16=[a{16}(1,:)' b{16}(1,:)' c{16}(1,:)' d{16}(1,:)' a{16}(2,:)' b{16}(2,:)' c{16}(2,:)' d{16}(2,:)' camera_pos(16,1)*ones(length(a{16}(1,:)),1) camera_pos(16,2)*ones(length(a{16}(1,:)),1) camera_pos(16,3)*ones(length(a{16}(1,:)),1) x_p_as{16}' y_p_as{16}' z_p_as{16}'];




%Inizializzo

R11a2=cell(1,2);
R21a2=cell(1,2);
R31a2=cell(1,2);
R41a2=cell(1,2);
R51a2=cell(1,2);
R61a2=cell(1,2);
R71a2=cell(1,2);
R81a2=cell(1,2);

%Associo le telecamere ricordando che al primo passo nn vi sono associazioni

R11a2{1}=T1;
R11a2{2}=T2;
R21a2{1}=T3;
R21a2{2}=T4;
R31a2{1}=T5;
R31a2{2}=T6;
R41a2{1}=T7;
R41a2{2}=T8;
R51a2{1}=T9;
R51a2{2}=T10;
R61a2{1}=T11;
R61a2{2}=T12;
R71a2{1}=T13;
R71a2{2}=T14;
R81a2{1}=T15;
R81a2{2}=T16;

%Inizializzo i punti
P11a2=[-1 -10 -1];

%dimensione stanza
dim_stanza_x=10000;
dim_stanza_y=10000;
dim_stanza_z=10000;

d_max=d_max_var;
disp('secondo passo dell albero')
tic
[R12a2 P12a2]=associa2(R11a2, R21a2, P11a2, P11a2,epsilon,dim_stanza_x,dim_stanza_y,dim_stanza_z,d_max);
t1=toc %Valuto tempo esecuzione
%set(S.tP1R4,'string',t1,'visible','on')
tic
[R22a2 P22a2]=associa2(R31a2, R41a2, P11a2, P11a2,epsilon,dim_stanza_x,dim_stanza_y,dim_stanza_z,d_max);
 t2=toc
% set(S.tP1R3,'string',t2,'visible','on')
 tic
[R32a2 P32a2]=associa2(R51a2, R61a2, P11a2, P11a2,epsilon,dim_stanza_x,dim_stanza_y,dim_stanza_z,d_max);
t3=toc
% set(S.tP1R2,'string',t3,'visible','on')
tic
[R42a2 P42a2]=associa2(R71a2, R81a2, P11a2, P11a2,epsilon,dim_stanza_x,dim_stanza_y,dim_stanza_z,d_max);
t4=toc
%set(S.tP1R1,'string',t4,'visible','on')

tmp=max([t1 t2 t3 t4]);
disp('terzo passo dell albero')
tic
[R13a2 P13a2]=associa2(R12a2, R22a2, P12a2, P22a2,epsilon,dim_stanza_x,dim_stanza_y,dim_stanza_z,d_max);
t2=toc
%set(S.tP2R2,'string',t2,'visible','on')
tic
[R23a2 P23a2]=associa2(R32a2, R42a2, P32a2, P42a2,epsilon,dim_stanza_x,dim_stanza_y,dim_stanza_z,d_max);
t1=toc
%set(S.tP2R1,'string',t1,'visible','on')
tmp = tmp + max([t1 t2]);

disp('quarto passo dell albero')

tic
[R14a2 P14a2]=associa2(R13a2, R23a2, P13a2, P23a2,epsilon,dim_stanza_x,dim_stanza_y,dim_stanza_z,d_max);
tmpo=toc
%set(S.tP3R1,'string',tmpo,'visible','on')

disp('Tempo totale')
tmp=tmp+tmpo
%set(S.Ttot,'string',tmp,'visible','on') %Calcolo tempo esecuzione massimo considerando approccio decentralizzato ad albero

Punti_tmp = pulisci2(P14a2);

Punti = media_punti_3D(100,Punti_tmp');

figure(2);
for i = 1 : n
plot3(pt_3D(1,i),pt_3D(2,i),pt_3D(3,i),'ro','MarkerFaceColor','b');
hold on
grid on
axis([-1000 11000 -1000 11000 -1000 11000]);
xlabel('Asse x [mm]'); ylabel('Asse y [mm]'); zlabel('Asse z [mm]');
title('Posizione Marker Reale','Fontsize',15);
box on
end

figure(3);
for i = 1 : length(Punti)
plot3(Punti(1,i),Punti(2,i),Punti(3,i),'ro','MarkerFaceColor','r');
hold on
grid on
axis([-1000 11000 -1000 11000 -1000 11000]);
xlabel('Asse x [mm]'); ylabel('Asse y [mm]'); zlabel('Asse z [mm]');
title('Posizione Marker Ricostruiti','Fontsize',15);
box on
end


