%% Wczytanie obrazów do przestrzeni roboczej
I1 = imread('C:\Users\Piotr\Documents\MATLAB\Image47.png');
I2 = imread('C:\Users\Piotr\Documents\MATLAB\Image48.png');
imshowpair(I1, I2, 'montage');
title('Para orginalnych obrazów');

%% Wczytanie parametrów kamery
load moja_kamera.mat
cameraParams = parametryKamery;

%% Usunięcie zniekształceń z obrazów
[I1, newOrigin1] = undistortImage(I1, cameraParams, 'OutputView', 'full');
[I2, newOrigin2] = undistortImage(I2, cameraParams, 'OutputView', 'full');
figure;
imshowpair(I1, I2, 'montage');
title('Obrazy po usunięciu zniekształceń');


%% Obliczenie macierzy parametrów zewnętrznych i macierzy kamery dla obrazów
squareSize = 23;
[refPoints1, boardSize] = detectCheckerboardPoints(I1);
% Przekształcenie współrzędnych wykrytych punktów do układu orginalnego
% obrazu
refPoints1 = bsxfun(@plus, refPoints1, newOrigin1);

worldPoints = generateCheckerboardPoints(boardSize, squareSize);
% wyznaczenie położenia i orientacji kamery względem układu globalnego
[R1, t1] = extrinsics(refPoints1, worldPoints, cameraParams);

[refPoints2, boardSize] = detectCheckerboardPoints(I2);
refPoints2 = bsxfun(@plus, refPoints2, newOrigin2);
[R2, t2] = extrinsics(refPoints2, worldPoints, cameraParams);

% obliczenie macierzy dla obu kamer
cameraMatrix1 = cameraMatrix(cameraParams, R1, t1);
cameraMatrix2 = cameraMatrix(cameraParams, R2, t2);

%% Ekstrakcja Feature Points

% Detekcja feature points
imagePoints1 = detectSURFFeatures(rgb2gray(I1), 'MetricThreshold', 600);
imagePoints2 = detectSURFFeatures(rgb2gray(I2), 'MetricThreshold', 600);
% Ekstrakcja feature descriptors
features1 = extractFeatures(rgb2gray(I1), imagePoints1);
features2 = extractFeatures(rgb2gray(I2), imagePoints2);

% wyświetlenie 1500 najlepszych punktów
figure;
imshow(I1);
title('1500 najsilniejszych punktów');
hold on;
plot(selectStrongest(imagePoints1, 1500));

%% Dopasowanie features pomiędzy obrazami za pomocą funkcji matchFeatures

indexPairs = matchFeatures(features1, features2, 'MaxRatio', 0.5);
matchedPoints1 = imagePoints1(indexPairs(:, 1));
matchedPoints2 = imagePoints2(indexPairs(:, 2));

% Wizualozacja powiązanych punktów
figure;
showMatchedFeatures(I1, I2, matchedPoints1, matchedPoints2);
title('Powiązane punkty');

% Transformacja punktów do globalnego układu współrzędnych
matchedPoints1.Location = bsxfun(@plus, matchedPoints1.Location, newOrigin1);
matchedPoints2.Location = bsxfun(@plus, matchedPoints2.Location, newOrigin2);

%% Reconstrukcja obiektu z dopasowanych features
% Wyznaczenie lokacji punktów w 3D przy użyciu funkcji
% triangulate implementującej algorytm DLT (Direct Linear Transformation)
% Błąd, będący różnicą między punktami na obrazie a odwzorowaniem
% jest użyty do eliminacji fałszywych dopasowań.

[points3D, reprojErrors] = triangulate(matchedPoints1, matchedPoints2, ...
    cameraMatrix1, cameraMatrix2);

% Eliminacja błędnych punktów
errorDists = max(sqrt(sum(reprojErrors .^ 2, 2)), [], 3);
validIdx = errorDists < 1;

points3D = points3D(validIdx, :);
validPoints1 = matchedPoints1(validIdx, :);
validPoints2 = matchedPoints2(validIdx, :);

figure;
showMatchedFeatures(I1, I2, validPoints1,validPoints2);
title('Dopasowane punkty po usunięciu błędów');


%% Wykres chmury punktów w 3D
% Using the scatter3 function, draw a point cloud representation of each
% feature location and its corresponding colors.

% rysowanie kamer
figure;
plotCamera('Location', -t1 * R1', 'Orientation', R1', 'Size', 10, ...
    'Color', 'r', 'Label', '1');
hold on
grid on
plotCamera('Location', -t2 * R2', 'Orientation', R2', 'Size', 10, ...
    'Color', 'b', 'Label', '2');

% wyznaczenie koloru odwzorowywanych punktów
validPoints1 = round(validPoints1.Location);
numPixels = size(I1, 1) * size(I1, 2);
allColors = reshape(im2double(I1), [numPixels, 3]);
colorIdx = sub2ind([size(I1, 1), size(I1, 2)], validPoints1(:,2), ...
    validPoints1(:, 1));
color = allColors(colorIdx, :);

% dodanie zielonego punktu reprezentującego początek układu współrzędnych
points3D(end+1,:) = [0,0,0];
color(end+1,:) = [0,1,0];

% rysowanie chmury punktów
showPointCloud(points3D, color, 'VerticalAxisDir', 'down', 'MarkerSize', 45);
xlabel('x (mm)');
ylabel('y (mm)');
zlabel('z (mm)')
title('Rekonstrukcja chmury punktów');

hold off;