%function [ X, q ] = foerstnerScan ( img, WndSize, StpSize, Mode )

%  [ X, q ] = foerstnerScan ( img, WndSize, StpSize, Mode )
%
%  Foerstner corner finder (interest operator)
%
%      X    - coordinates of corners
%      q    - condition number and gradient magnitude
%      ima  - image
%      win  - x_min, x_max, y_min, y_max of subimage where the search is started
%
%
%  (c) Pietro Perona, Apr 26, 1999
%
%  California Institute of Technology

[SizeY SizeX] = size(img);

Gx = conv2(img, [1 0 -1] , 'same');
Gy = conv2(img, [1 0 -1]', 'same');

if strcmp(Mode, 'circle'),
	tmp = Gy;
	Gy = -Gx;
	Gx =  tmp;
end 

WndSize = WndSize + 1 - rem(WndSize, 2);

WndSizeH = floor(WndSize / 2);

NPix = prod(WndSize);

Xidx = -WndSizeH(1) : WndSizeH(1);
Yidx = -WndSizeH(2) : WndSizeH(2);

[Xgrd, Ygrd] = meshgrid(Xidx, Yidx);

figure(1); clf; imagesc(img); colormap gray; hold on; zoom on;

for y = WndSizeH(2) + 1 : StpSize(2) : SizeY - WndSizeH(2),
  for x = WndSizeH(1) + 1 : StpSize(1) : SizeX - WndSizeH(1),

    ptch = img(Yidx + y, Xidx + x);

		Gx2 = Gx(Yidx + y, Xidx + x) .^ 2;
		Gxy = Gx(Yidx + y, Xidx + x) .* Gy(Yidx + y, Xidx + x);
		Gy2 = Gy(Yidx + y, Xidx + x) .^ 2;

		px = Xgrd + x;
		py = Ygrd + y;

		M11 = sum(sum(Gx2));

		M22 = sum(sum(Gy2));

		M12 = sum(sum(Gxy));

		M = [M11 M12; M12 M22];
	
		[U S V] = svd(M);

		c = cond(M);
		gm = sqrt(M(1, 1) + M(2, 2));
		q = [c, gm];

		if S(2,2) > 1e1 & gm/NPix > 10,
			V = [ sum(sum(Gx2 .* px + Gxy .* py)); ...
			      sum(sum(Gxy .* px + Gy2 .* py)) ];
			xp = inv(M) * V;
			sig = sum(sum((Gx(Yidx + y, Xidx + x) .* (xp(1) - px) + ...
			   	     Gy(Yidx + y, Xidx + x) .* (xp(2) - py)) .^ 2));
			sig = sig / (NPix - 2);

			if sqrt(sig) < 10,
				plot(xp(1), xp(2), 'g.');
				fprintf(1, 'Cond: %5.2f \t Grad: %5.2f \t Dist: %5.2f \n', S(2,2), gm/NPix, sqrt(sig));
			end

		end

	end
end