clear all 
clc
cams(1).setmarker=[1:12];
cams(2).setmarker=[4:16];
size(cams(1).setmarker)

K=[1 0 0 0;0 1 0 0;0 0 1 0];
R=eye(3);
T=[0 0 0]';
X_hat=[91 105 152 171 271 311 327 356 357 545 600 600;
    63 177 320 438 105 207 241 241 347 97 350.60 356.603;
    1 1 1 1 1 1 1 1 1 1 1 1];
dP=[30 30 30 30 20 40 40 40 35 30 50 50];   %dP deviazione standard non varianza!!
camera=1;


[m,n]=size(cams(camera).setmarker);%number of markers seen by the camera
num_marker=n;
A=zeros(2,num_marker);
%Sarebbe meglio avere già un campo cams.R e cams.T, per non dovergli ogni
%volta calcolare da cams.angles, e cams.K . Ancora meglio cams.P

P=K*[R T;0 0 0 1];
idealmarker(1).center=[];
idealmarker(1).uncrt=[];
idealmarker(1).blob=[];

for i=1:num_marker
    temp=P*[X_hat(:,cams(1).setmarker(i));1]; %'seen' markers projection on image plane
    idealmarker(i).center=[(temp(1)/temp(3)); (temp(2)/temp(3))];
    idealmarker(i).uncrt=(norm((P*[0;0;dP(:,i);1])));      %uncertainty in pixel
    
end

%Estract the blobs from a image.
originalImage = imread('img4.png');
%originalImage = imread('img4.png'); % Read in standard MATLAB demo image
level=graythresh(originalImage);
binaryImage=im2bw(originalImage,level); % Threshold to binary
labeledImage = bwlabel(binaryImage, 8); % Label each blob so can do calc on it
blobMeasurements = regionprops(labeledImage, 'Centroid'); % Get all the blob properties.
numberOfBlobs = size(blobMeasurements, 1);
bfromim = inf(numberOfBlobs,num_marker+1);  %blob vs ideal markers and a counter
bfromim(:,end)=zeros(numberOfBlobs,1);
% xybfromxym = inf(2*numberOfBlobs,num_marker+1); %x-y blob vs ideal markers x-y

imagesc(originalImage); colormap(gray(256))
hold on
scatter(X_hat(1,:),X_hat(2,:),'green');
hold on

blob=zeros(2,numberOfBlobs);
for i=1:numberOfBlobs
    blob(:,i)=blobMeasurements(i).Centroid;
end
scatter(blob(1,:),blob(2,:),'red');

A=zeros(2,num_marker);

for i=1:numberOfBlobs
    sum=0;
    for j=1:num_marker
%         disp('cntrd')
%         blobMeasurements(i).Centroid
%         disp('mrkr')
%         idealmarker(j).center'
%         disp('dis')
        distance=norm(idealmarker(j).center-(blobMeasurements(i).Centroid)');
%         disp('uncrt')
         idealmarker(j).uncrt
        if distance<=3*idealmarker(j).uncrt
            bfromim(i,j)=distance/idealmarker(j).uncrt;
            bfromim(i,end)=bfromim(i,end)+1;
            sum=sum+bfromim(i,j);
%             xybfromxym(2*i-1,j)=abs((idealmarker(j).center(1)-blobMeasurements(i).Centroid(1))/idealmarker(j).uncrt);
%             xybfromxym(2*i,j)=abs((idealmarker(j).center(2)-blobMeasurements(i).Centroid(2))/idealmarker(j).uncrt);
        end
    end
    for j=1:num_marker
        bfromim(i,j)=bfromim(i,j)/sum;
    end
end

B=bfromim;

for i=1:numberOfBlobs
    if bfromim(i,end)==1
        count=1;
        while bfromim(i,count)==inf
            count=count+1;
        end
        A(:,count)=[1 1]';  %the index-th marker is seen with probability 1
    
        for j=1:numberOfBlobs
            bfromim(j,count)=Inf;
        end
    end        

    if bfromim(i,end)>1
        min=Inf;
        index=0;
        for j=1:num_marker
            if bfromim(i,j)<min
                min=bfromim(i,j);
                index=j;
            end
        end
        if index>0
          	A(1,index)=1;
            for j=1:numberOfBlobs
                bfromim(j,index)=Inf;
            end
%             ErrProb=0;
%             for j=1:num_marker
%                 if bfromim(i,j)<Inf
%                     ErrProb=ErrProb+erfc(bfromim(i,j)/2^0.5)/2;
%                 end
%             end
            A(2,index)=1-min;
        end
    end
    
    
%     if bfromim(i,end)>1
%         min=Inf;
%         index=0;
%         indeces=[];
%         for j=1:num_marker
% %             if bfromim(i,j)<Inf
% %                 indeces=[indeces,j];
% %             end
%             if bfromim(i,j)<min
%                 min=bfromim(i,j);
%                 index=j;
%             end
%         end
%         ErrProb=0;
%         for j=1:num_marker
%             if bfromim(i,j)<inf
%                 ErrProb=ErrProb+erfc(xybfromxym(2*i-1,j)/2^0.5)/2+erfc(xybfromxym(2*i,j)/2^0.5)/2;
%             end  
%         end
%         A(:,count)=[1 1-ErrProb]';
%     end
end
%A=im_overlap(points,dP,K,R,T,1,'img4','png');