function        [dist,fit_err] = subpixeledges(edginess,sintheta,costheta)

%SUBPIXELEDGES          Locate edges to within subpixel accuracy
%                       Makes use of parabola fitting algorithm
%
%                       [dist,fit_err] = subpixeledges(edginess,sintheta,costheta)
%
%


%       (c) Copyright Pietro Perona, 
%       California Inst. of Technology
%               July 9 1993
%       

[m,n] = size(edginess);

beta = 2 * sintheta.^2 .* costheta.^2 + 1;
deltainv = 1 ./ (6 * (3*beta-2));

ker_x = -[-1 0 1; -1 0 1; -1 0 1];
ker_y = ker_x';
ker_x2= ker_x.^2;
ker_y2= ker_y.^2;
ker_xy= ker_x .* ker_y;
ker_1 = ones(3);

slop_x = conv2(edginess,ker_x); slop_x = slop_x(2:m+1,2:n+1);
slop_y = conv2(edginess,ker_y); slop_y = slop_y(2:m+1,2:n+1);
slop = - slop_x .* sintheta + slop_y .* costheta; 
clear slop_x slop_y

curv_x = conv2(edginess,ker_x2); curv_x = curv_x(2:m+1,2:n+1);
curv_y = conv2(edginess,ker_y2); curv_y = curv_y(2:m+1,2:n+1);
twist  = conv2(edginess,ker_xy); twist = twist(2:m+1,2:n+1);
curv = curv_x .* sintheta.^2 + curv_y .* costheta.^2 - 2 * twist .* sintheta .* costheta;
clear curv_x curv_y twist 

avrg  = conv2(edginess,ker_1); avrg = avrg(2:m+1,2:n+1);


a = deltainv .* ((3 * curv) - (2 * avrg));
b = slop / 6;
c = deltainv .* ((2 * beta .* avrg) - 2 * curv);


dist = 0.5 * b ./ a; %- 0.5 * (3 * beta - 2) .* slop ./ ( (3 * curv) - (2 * avrg));

fit2norm = 6 * (beta .* (a.^2) + (b.^2) ) + 9 * (c.^2) + 12 * a .* c;

grad2norm = conv2((edginess.^2),ker_1); grad2norm = grad2norm(2:m+1,2:n+1);

fit_err = ones(m,n);

fit_err(grad2norm~=0) = sqrt(1 - fit2norm(grad2norm~=0) ./ grad2norm(grad2norm~=0));

fit_err = real(fit_err);

