%Test di Stato dell'arte

%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}'];
%raccolgo i dati dai vari piani immagine
Dati=[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}']; %Inizializzo con i dati della prima telecamera

%specifico dimensioni stanza
dim_stanza_x=10000;
dim_stanza_y=10000;
dim_stanza_z=10000;

for i=2:length(a)
   tmp=[a{i}(1,:)' b{i}(1,:)' c{i}(1,:)' d{i}(1,:)' a{i}(2,:)' b{i}(2,:)' c{i}(2,:)' d{i}(2,:)' camera_pos(i,1)*ones(length(a{i}(1,:)),1) camera_pos(i,2)*ones(length(a{i}(1,:)),1) camera_pos(i,3)*ones(length(a{i}(1,:)),1) x_p_as{i}' y_p_as{i}' z_p_as{i}']; %aggiungo i dati delle altre telecamere
   Dati=[Dati ; tmp]; 
end


disp('Tempo esecuzione approccio centralizzato')
tic
punti_stato_arte=stato_arte_ricostruzione_vero(Dati,epsilon,dim_stanza_x,dim_stanza_y,dim_stanza_z,d_max_var);
tmpo=toc %Valuto il tempo di esecuzione



Punti_tmp = pulisci2(punti_stato_arte);

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

