%% Houghova transformace -- Poèítání schodù

%%
% Úkolem dne¹ního cvièení bude naimplementovat a vyu¾ít Houghovu
% transformaci k vyøe¹ení netriviálního úkolu spoètení jezdících schodù
% na stanici Námìstí Míru z jedné fotografie.

%%
% Nejprve si naètìte vstupní *obrázek.* stairs.jpg
img_big = rgb2gray(imread('stairs.jpg'));
img_gray = imresize(img_big, 1/3, 'bilinear');

%%
% Budeme potøebovat najít pøímky v obraze, musíme tedy napøed detekovat
% hrany
img_bw = edge(img_gray, 'canny', 0.3, 1);
imshow(img_bw);

%%
% Odfiltrujeme souvislé komponenty, které nejsou dostateèné "protáhlé"
[img_bw_lines, lines] = find_lines(img_bw, 10, 0.05);

%%
% Vizualizace nalezených úseèek, jejich normál (modøe) a smìrových
% vektorù (èervenì)
vis_lines(img_bw_lines, lines);

%%
% Houghova transformace na vybraných pøímkách;
acc = hough_vanish(lines, size(img_bw_lines));

%%
% Rozmazání akumulátoru
H = fspecial('gaussian');
acc_blurred = imfilter(double(acc), H, [5 5], 3);
figure; imagesc(acc_blurred);

%%
% Nalezení nejvy¹¹ího lokálního maxima -- úbì¾níku
vanish_point = findmax(acc_blurred, 1, [30 30 0.2]);

%%
% Vizualizace nalezeného úbì¾níku
figure;
imshow(img_bw_lines);
hold on;
plot(vanish_point(2), vanish_point(1), 'gx');

% ----------------------------------------------------------

figure; imshow(img_bw_lines);
bbox = ginput(2);
img_bw_part = img_bw_lines(bbox(1, 2):bbox(2, 2),:);
imshow(img_bw_part);

theta = (80:100)';
[R,xp] = radon(img_bw_part,theta);	% Hough transform
figure;imagesc(theta, xp, R);

stairs_R = findmax(R, 2, [20 10 0.2]);
stairs_R(2, :) = theta(stairs_R(2, :));
stairs_R(1, :) = xp(stairs_R(1, :));
stairs_R = sortrows(stairs_R')';

hold on
plot(stairs_R(2, 1), stairs_R(1, 1), 'rx')
plot(stairs_R(2, 2), stairs_R(1, 2), 'rx')

bb_centre = [size(img_gray, 2)/2; (bbox(2,2)-bbox(1,2)-1)/2 + bbox(1,2)];

% bacha na to minus!
stairs_points = [vanish_point(2)*ones(2, 1), bb_centre(2) - stairs_R(1, :)'];

figure;
imshow(img_bw_lines);
hold on;
plot(vanish_point(2), vanish_point(1), 'gx');
plot(stairs_points(:, 1), stairs_points(:, 2), 'rx');


figure;
imshow(img_gray);
stairs_up = ginput(1);
stairs_up(1) = vanish_point(2);

% -------------------------------

vp = [vanish_point(2); vanish_point(1)];

P_inf = vp;
P_0 = stairs_points(1, :)';
P_I = stairs_points(2, :)';
P_x = stairs_up';

num_stairs(P_inf, P_0, P_I, P_x)

% ----------------------------------------------------
%% Kontrolní otázky:
% Jak by se dal vyu¾ít RANSAC na hledání úbì¾níku?
% Jiné hranové detektory...


%-------------------------------
% HTML vystupy

figure
imshow(img_gray_small)
hold on
plot([P_x(1)/2, P_x(1)/2], [1, size(img_gray_small, 1)], 'w')
plot(vanish_point(2)/2, vanish_point(1)/2, 'gx');
plot(vanish_point(2)/2, vanish_point(1)/2, 'bx');
plot(stairs_points(:, 1)/2, stairs_points(:, 2)/2, 'rx');
plot(P_x(1)/2, P_x(2)/2, 'gx');
