% Exercise Sheet 4, Part II
%
% ========================================
% Image Formation, Exercise 1
% ========================================
%
% (a) -----------------------------------------
function W = projection_perspective(V,f)

% Projection matrix:
K = [f 0 0 0 ; 0 f 0 0 ; 0 0 1 0];

nVertices = size(V,1);

% Conversion to homogenous coordinates:
Vh = [V,ones(nVertices,1)];

% Translate the vertices to the camera coordinate system:
T = [1 0  0 0 ; 0 1 0 0 ; 0 0 1 1 ; 0 0 0 1];
Vh = T * Vh';

% Project the vertices:
Wh_t = K * Vh;
Wh = Wh_t';

% Back transform from homogenous to 3D coordinates:
for i=1:nVertices
    W(i,1) = Wh(i,1) / Wh(i,3);
    W(i,2) = Wh(i,2) / Wh(i,3);
end;

% (b) -----------------------------------------
function W = projection_parallel(V,f)

% Projection matrix:
K = [f 0 0 0 ; 0 f 0 0 ; 0 0 0 0];

nVertices = size(V,1);

% Conversion to homogenous coordinates:
Vh = [V,ones(nVertices,1)];

% Translate the vertices to the camera coordinate system:
T = [1 0  0 0 ; 0 1 0 0 ; 0 0 1 1 ; 0 0 0 1];
Vh = T * Vh';

% Project the vertices:
Wh_t = K * Vh;
Wh = Wh_t';

% Back transform from homogenous to 3D coordinates:
W = Wh(:,1:2);

% (c) -----------------------------------------
function W = projection_spherical(V,f)

% Projection matrix:
K = [f 0 0 0 ; 0 f 0 0 ; 0 0 1 0];

nVertices = size(V,1);

% Conversion to homogenous coordinates:
Vh = [V,ones(nVertices,1)];

% Translate the vertices to the camera coordinate system:
T = [1 0  0 0 ; 0 1 0 0 ; 0 0 1 1 ; 0 0 0 1];
Vh = T * Vh';

% Project the vertices:
Wh_t = K * Vh;
Wh = Wh_t';

% Back transform from homogenous to 3D coordinates:
W = zeros(nVertices,3);
for i=1:nVertices
    norm = sqrt(Wh(i,1)*Wh(i,1) + Wh(i,2)*Wh(i,2) + Wh(i,3)*Wh(i,3));
    W(i,1) = Wh(i,1) / norm;
    W(i,2) = Wh(i,2) / norm;
    W(i,3) = Wh(i,3) / norm;
end;


% ========================================
% The Lucas-Kanade method, Exercise 1
% ========================================
% 
function [u,v] = exercise05(threshold,windowsize)

% Read input data:
I1 = double(imread('street1.pgm'));
I2 = double(imread('street2.pgm'));

[w,h] = size(I1);

% Compute gradients:
Ix  = I1(:, [2:end end]) - I1(:, [1 1:end-1]) / 2; % (I(x+1,y)-I(x-1,y)) / 2
Iy  = I1([2:end end], :) - I1([1 1:end-1], :) / 2; % (I(x,y+1)-I(x,y-1)) / 2
It  = I2 - I1;

% Elements of the structure tensor:
Ixx = Ix.*Ix;
Iyy = Iy.*Iy;
Ixy = Ix.*Iy;
Itx = It.*Ix;
Ity = It.*Iy;

for y = windowsize + 1  :  h - windowsize
for x = windowsize + 1  :  w - windowsize

    T = zeros(2,2);
    a = zeros(2,1);
    
    n = 1/(2 * windowsize + 1)^2;
    
    % Compute integral of structure tensors:
    for j = -windowsize:windowsize
    for i = -windowsize:windowsize
        xi = x + i;
        yj = y + j;
        
        T(1,1) = T(1,1) + Ixx(xi,yj) * n;
        T(1,2) = T(1,2) + Ixy(xi,yj) * n;
        T(2,2) = T(2,2) + Iyy(xi,yj) * n;

        a(1,1) = a(1,1) + Itx(xi,yj) * n;
        a(2,1) = a(2,1) + Ity(xi,yj) * n;
    end
    end
    T(2,1) = T(1,2);
    
    % Compute velocity:
    if det(T) > threshold
        b = -inv(T) * a;
        u(x,y) = b(1);
        v(x,y) = b(2);
    end
end
end

figure; colormap('gray'); imagesc(I1);
figure; colormap('gray'); imagesc(I2);
figure; colormap('gray'); imagesc(u);
figure; colormap('gray'); imagesc(v);