clear all
clc
% X_hat=[10 3;1 3;4 3];
profile on
space=[5 5 5];
X_hat=marker_generator(space,100);
dP=(0.05)^2*ones(1,size(X_hat,2));
cams=auto_position(4,space);
lato=400;
[cams voxels]=space_partition(cams,space,pi/3,0.2,0.5,5);
[cams voxels]=mark2vox(X_hat,cams,voxels,space,0.2);
for i=1:size(cams,2)
    cams(i).intrinsic=[1 0 lato/2;0 1 lato/2;0 0 1];
end
latovoxel=0.2;
X=real_marker(X_hat,dP);

[cams,Real,rays,uv_hat]= im_simulation(X_hat,X,dP,cams,1,lato,0.1,pi/3);
color=jet(size(uv_hat,2));

% figure;
% camso=camso(1);
% geps=camso(1);
% % figure(2)
% % grid on
% % plotcam3(geps,space,voxels,latovoxel,[1 1 1 0 0 0 0])
% % view([75,30])
% % grid on
% % axis equal
% % hold on
% for i=1:size(X_hat,2)
% % DrawCuboid([.2 .2 .2], mark(:,i),[0 0 0],'m',1)
% plot3(X_hat(1,i),X_hat(2,i),X_hat(3,i),'go')
% hold on
% end

[A,blob] = im_overlap(X_hat,dP,cams,1,Real,lato,pi/3);
% %
% % for i=1:size(camso(1).setmarkers,2)
% % % DrawCuboid([.2 .2 .2], mark(:,i),[0 0 0],'m',1)
% % plot3(X_Hat(1,camso(1).setmarkers(i)),X_Hat(2,camso(1).setmarkers(i)),X_Hat(3,camso(1).setmarkers(i)),'go')
% % hold on
% % end
% % 
% 

figure(60);
imshow(Real);
hold on
for i=1:size(uv_hat,2)
%     PlotCircle(uv_hat(1,i),uv_hat(2,i),rays(i),10000,color(i,:))
    PlotCircle(uv_hat(1,i),uv_hat(2,i),floor(rays(i)),10000,'r');
    hold on;
end

