% =========================================================================
% Exercise 8
% =========================================================================

clc;
close all;
clear all;

% Initialize VLFeat (http://www.vlfeat.org/)

%K Matrix for house images (approx.)
K = [  670.0000     0     393.000
         0       670.0000 275.000
         0          0        1];

%Load images
imgName1 = '../data/house.000.pgm';
imgName2 = '../data/house.004.pgm';

img1 = single(imread(imgName1));
img2 = single(imread(imgName2));

%extract SIFT features and match
[fa, da] = vl_sift(img1);
[fb, db] = vl_sift(img2);

%don't take features at the top of the image - only background
filter = fa(2,:) > 100;
fa = fa(:,find(filter));
da = da(:,find(filter));

[matches_1, scores] = vl_ubcmatch(da, db);

showFeatureMatches(img1, fa(1:2, matches_1(1,:)), img2, fb(1:2, matches_1(2,:)), 20);

% the initial set of points used in fundamental matrix estimation
x1s = makehomogeneous(fa(1:2, matches_1(1,:)));
x2s = makehomogeneous(fb(1:2, matches_1(2,:)));
%% Compute essential matrix and projection matrices and triangulate matched points

%use 8-point ransac or 5-point ransac - compute (you can also optimize it to get best possible results)
%and decompose the essential matrix and create the projection matrices

Ps{1} = eye(4);
[E, inliers_1] = ransacfitfundmatrix(x1s, x2s, 0.001);

% keep descriptors for the features used in triangularion
da_inliers = da(:, inliers_1);
db_inliers = db(:, inliers_1);

x1s_in = makehomogeneous(x1s(1:2, inliers_1));
x2s_in = makehomogeneous(x2s(1:2, inliers_1));

x1_calibrated = inv(K) * x1s_in;
x2_calibrated = inv(K) * x2s_in;
showFeatureMatches(img1, x1s_in(1:2, :), img2, x2s_in(1:2, :), 30);

Ps{2} = decomposeE(E, x1_calibrated, x2_calibrated);

%triangulate the inlier matches with the computed projection matrix
[XS_12, err] = linearTriangulation(Ps{1}, x1_calibrated, Ps{2}, x2_calibrated);
err

%% Add an addtional view of the scene

imgName3 = '../data/house.001.pgm';
img3 = single(imread(imgName3));
[fc, dc] = vl_sift(img3);

[matches_3, scores] = vl_ubcmatch(da_inliers, dc);
%match against the features from image 1 that where triangulated

%find the 2D-3D corresponding matched point pairs
XS_matched = XS_12(:, matches_3(1, :));
x1_calibrated_3 = x1_calibrated(:, matches_3(1, :));

x3s = makehomogeneous(fc(1:2, matches_3(2,:)));
x3_calibrated = inv(K) * x3s;

%run 6-point ransac
[Ps{3}, inliers_3] = ransacfitprojmatrix(x3_calibrated, XS_matched, 0.05);
if (det(Ps{3}) < 0)
    Ps{3}(1:3,1:3) = -Ps{3}(1:3,1:3);
    Ps{3}(1:3, 4) = -Ps{3}(1:3, 4);
end

x1_calibrated_3_in = x1_calibrated_3(:, inliers_3);
x3_calibrated_in = x3_calibrated(:, inliers_3);

%triangulate the inlier matches with the computed projection matrix
[XS_13, err] = linearTriangulation(Ps{2}, x1_calibrated_3_in, Ps{3}, x3_calibrated);
err

x1_3 = K * x1_calibrated_3_in;
x3 = K * x3_calibrated_in;

showFeatureMatches(img2, x1_3(1:2, :), img3, x3(1:2, :), 40);

%% Add more views...

imgName4 = '../data/house.002.pgm';
img4 = single(imread(imgName4));

[fd, dd] = extractSIFT(img4);
[matches_4, scores] = vl_ubcmatch(da_inliers, dd);
%match against the features from image 1 that where triangulated

XS_matched = XS_12(:, matches_4(1, :));
x1s_calibrated_4 = x1_calibrated(:, matches_4(1, :));
x4s = makehomogeneous(fd(1:2, matches_4(2,:)));
x4_calibrated = inv(K) * x4s;


%run 6-point ransac
[Ps{4}, inliers_4] = ransacfitprojmatrix(x4_calibrated, XS_matched, 0.05);

if (det(Ps{4}) < 0)
    Ps{4}(1:3,1:3) = -Ps{4}(1:3,1:3);
    Ps{4}(1:3, 4) = -Ps{4}(1:3, 4);
end

x1_calibrated_4 = x1s_calibrated_4(:, inliers_4);
x4_calibrated = x4_calibrated(:, inliers_4);

%triangulate the inlier matches with the computed projection matrix
[XS_14, err] = linearTriangulation(Ps{1}, x1_calibrated_4, Ps{4}, x4_calibrated);

x1_4 = (K * x1_calibrated_4);
x4 = (K * x4_calibrated);
showFeatureMatches(img1, x1_4(1:2, :), img4, x4(1:2, :), 50);

%% Plot stuff
close all
fig = 10;
figure(fig);

%use plot3 to plot the triangulated 3D points
XS_12_ = makeinhomogeneous(XS_12);
XS_13_ = makeinhomogeneous(XS_13);
XS_14_ = makeinhomogeneous(XS_14);
plot3(XS_12_(1, :), XS_12_(2, :), XS_12_(3, :), "*r");
hold on;
plot3(XS_13_(1, :), XS_13_(2, :), XS_13_(3, :), "*b");
hold on;
plot3(XS_14_(1, :), XS_14_(2, :), XS_14_(3, :), "*g");
% draw cameras
drawCameras(Ps, fig);