function mvg_ex08_prac

[K,R,T,images] = readcalibration('templeCalib.txt');
nImages = size(K,3);

% Relative projections with reference frame 1:
P = zeros(4,4,nImages);
g1 = inv([K(:,:,1) * R(:,:,1) K(:,:,1) * T(:,1) ; 0 0 0 1]);
for i=1:nImages
    P(:,:,i) = [K(:,:,i) * R(:,:,i) K(:,:,i) * T(:,i) ; 0 0 0 1] * g1;
    R(:,:,i) = P(1:3,1:3,i);
    T(:,i) = P(1:3,4,i);
end

% Get corresponding image points:
nPoints = 6;
[x,y] = getcoordinates(images,nPoints);

% Compute depths:
lambda = zeros(nPoints,1);
for j=1:nPoints
    sum1 = 0;
    sum2 = 0;
    x1 = [x(1,j);y(1,j);1];
    for i=2:nImages
        xhat = hat([x(i,j);y(i,j);1]);
        xhatT = xhat * T(:,i);
        sum1 = sum1 - xhatT' * xhat * R(:,:,i) * x1;
        sum2 = sum2 + norm(xhatT)^2;
    end
    lambda(j) = sum2 / sum1;
end
display(lambda);

% Plot 2D points to image:
imshow(uint8(images(:,:,1)));
hold on;
plot(x(1,:),y(1,:));
hold off;

% Plot 3D points:
figure;
plot3(x(1,:),y(1,:),lambda);

end

function A = hat(v)
A = [0 -v(3) v(2) ; v(3) 0 -v(1) ; -v(2) v(1) 0];
end

function [x,y] = getcoordinates(images,nPoints)

[w,h,nImages] = size(images);
x = zeros(nImages,nPoints);
y = zeros(nImages,nPoints);
for i=1:nPoints
    for n=1:nImages
        imshow(uint8(images(:,:,n)));
        [xx,yy] = ginput(1);
        x(n,i) = double(xx);
        y(n,i) = double(yy);
    end
end
end

function [x,y] = getcoordinates2()
x = [
   232   385   375   206   210   370
   235   384   375   206   207   369
   238   386   375   207   206   368
   238   390   376   208   205   365
   240   389   376   209   204   366
   ];
y = [
   127   129   242   234   309   311
   137   136   231   222   306   308
   148   147   220   212   302   305
   157   158   209   201   289   298
   170   171   199   191   281   292
   ];
end