% 4.4 Likelihood function for an edge map

clear;
close all;

% The original image
I = imread('Image.bmp');
figure;
imshow(I);

% The Canny Edge Map
%E = edge(I,'canny');
E = imread('Canny.bmp');
E2 = imread('Show.bmp');
figure;
imshow(E2);

l = 40;

% Best matching
s = [70;118;32];

% Not perfect matching
%s = [50;47;0];

% Intermediate case
%s = [50;47;30];

% Worst case (no mathing points)
%s = [50;63;0];


x1 = s(1);
y1 = s(2);
alpha = s(3)*pi/180;

q1 = [x1;y1];

q2 = [x1+l*cos(alpha);y1+l*sin(alpha)];

J = 10;
jv(2:2:2*J+1) = -(1:J);
jv(3:2:2*J+2) = 1:J;
jv(1) = 0;

siz = size(E);
rx = siz(2);
ry = siz(1);

sig = 8;
Lik = 1;
ind = 0;
for i=1:2:l
    ind = ind+1;
    k = (i-1)/(l-1);
    q = k*q1+(1-k)*q2;
    n = [-sin(alpha);cos(alpha)];
    d_ok = J;
    j=1;
    found(ind) = 0;
    while(j<=length(jv))
        d = jv(j);
        qp = round(q+d*n);
        if((qp(1)>0)&(qp(1)<=rx)&(qp(2)>0)&(qp(2)<=ry))
            if(E(qp(2),qp(1))~=0)
                d_ok = norm(qp-q)*sign(d);
                j = length(jv)+1;
                found(ind) = 1;
            end
        end
        j = j+1;
    end
    q_ok(:,ind) = q+d_ok*n;
    
    Lik = Lik *exp(-0.5*d_ok^2/sig^2);
end

Lik

figure;
imshow(E2);
hold;
plot([q1(1),q2(1)],[q1(2),q2(2)],'r');
for i=1:ind
    if(found(i)==1)
        plot(q_ok(1,i),q_ok(2,i),'r.');
    else
        plot(q_ok(1,i),q_ok(2,i),'b.');
    end
end

