%% Prostorová rekonstrukce při změně pozice měřicího systému
% Následující skript slouží k otestování metody prostorové rekonstrukce
% streoskopie zaznamenávající objekt při změně pozice optického systému. V 
% úvodní části uživatel zvolí vstupní data. Může použít vzorová data
% nebo použít vlastní.
%   Popis parametrů je ve tvaru:
%   - 'názevProměnné' = popis (datový typ) || [ jednotka ]
%
%   Povinné:
%   - 'imgR' = zaznamenané snímky první kamerou, první snímek
%     zachycuje známý vzor a druhý nasvícený objekt pro zajištění textur
%     (cell)
%   - 'imgL' = zaznamenané snímky druhou kamerou, první snímek
%     zachycuje známý vzor a druhý nasvícený objekt pro zajištění textur
%     (cell)
%   - 'cameraParamsR' = parametry první kamery získané kalibrací kamery
%     (cameraParameters)
%   - 'cameraParamsL' = parametry druhé kamery získané kalibrací kamery
%     (cameraParameters)
%   - 'set' = venktor definující počet snímků zaznamenaný každou změnou
%     pohledu
%   - 'rot' = venktor definující natočení pohledů určitých sad snímků (pro
%     správnou funkčnost jsou postačující obecné hodnoty 0, 90, 180, 
%     270) [°] 
%   - 'tolerance' = vektor definující toleranci epipolárního omezení pro
%     určité sady snímků [-]
%   - 'matchingRange' = vektor definující rozsah bodů považovaných za 
%      kandidáty na korespondující body pro určité sady snímků [-]
%   - 'ofset' pozice referenčního bodu (počátek kalibrační mřížky) ve
%     světovém souřadném systému [-]
%
%   Nepovinné:
%   - 'filtration' - filtrace odlehlých hodnot (bool)
%   - 'algorithm' - metoda k filtraci odlehlých hodnot ('RANSAC' nebo
%                   'MSAC')
% Požadované skripty:
%    1. getIndex.m
%
% Požadované toolboxy:
%    1. Computer Vision Toolbox
%
% pozn.: Vzhledem k omezení velikosti příloh jsou vstupní data pro
% rekonstrukci kovadliny umístěna pouze na CD, které je součástí tištěné
% verze.
%
% Vytvořeno v rámci bakalářské práce na téma Vytvoření 3D modelu za pomoci 
% laserového vzoru.
% Martin Krátký
% Vysoké učení technické v Brně
% Květen 2019
% Práce je dostupná na webové adrese:
% https://www.vutbr.cz/studenti/zav-prace/detail/119313 
%

clear variables
%% Vstupní parametry

% Použít vzorová, nebo vlastní data
sample = 1;     % 1 - Použít vzorová data
                % 0 - Použít vlastní data

% Definice vlastních dat
if ~sample
                    % Načtení sad snímků
load('vstupniData.mat')                
    
check=0;            % Kontrola korespondujících bodů
                    % 1 - Ano
                    % 0 - Ne

filter = [0];       % Intenzita filtru 0-100 [%]

tolerance = [0];    % Tolerance epipolárního omezení (doporučená hodnota 
                    % je v rozmezí 0-4)

matchingRange = [0];% Rozsah hledání potenciálních korespondujících bodů 

ofset = [0];        % Pozice referenčního bodu

rot = [0];          % Natočení pohledu

square = 25;        % Délka stran čtverce vzoru [mm]

filtration=0;       % Filtrace odlehlých hodnot
                    % 1 - Ano
                    % 0 - Ne
                
algorithm = 'RANSAC'; 
                    % Algoritmus pro filtraci odlehlých hodnot
                    % 'RANSAC' - Ransac
                    % 'MSAC'   - Mlesac
else
% Definice vzorových dat
    load kovadlina
    filter = [50 20 10 10 20 10 10 20 ];
    tolerance = [1 1 1 2 2 .5 0.8 1];
    matchingRange = [40 40 40 30 60 60 20 20 ];
    ofset = [ 175 170 0; -230 120 0; -205 120 0 ; -175 -160 0; -175 -150 0; 225 -105 0; 225 -105 0 ;175 170 0]; 
    rot = [180 270 270 0 0 90 90 180];
    square = 25;
    filtration = 0;
    check = 0;
end


%% Rekonstrukce
C=[]; pointsCloud = [];
for setIndex=1:length(set)

    n = set(setIndex);
    color = [];
   
% Body z referenčního snímku
    [imagePointsR,~] = detectCheckerboardPoints(imgR{sum(set(1:setIndex-1))+1});
    [imagePointsL,boardSize] = detectCheckerboardPoints(imgL{sum(set(1:setIndex-1))+1});
    
    k=1;
    worldPoints = generateCheckerboardPoints(boardSize,square);

% Úprava referenčních bodů, aby respektovali natočení kamer 
     if (rot(setIndex)>=90) && (rot(setIndex)<180)
        worldPoints(:,3) = -worldPoints(:,2);
        worldPoints(:,2) = worldPoints(:,1);
        worldPoints(:,1) = 0;
    elseif (rot(setIndex)>=180) && (rot(setIndex)<270)
        worldPoints(:,3) = -worldPoints(:,2);
        worldPoints(:,1) = worldPoints(:,1);
        worldPoints(:,2) = 0;
    elseif (rot(setIndex)>=270) && (rot(setIndex)<360)
        worldPoints(:,3) = -worldPoints(:,2);
        worldPoints(:,2) = -worldPoints(:,1);        
        worldPoints(:,1) = 0;
    else %0-90
        worldPoints(:,3) = -worldPoints(:,2);
        worldPoints(:,1) = worldPoints(:,1);
        worldPoints(:,2) = 0;
     end
% Výpočet fundamentální matice
    F = estimateFundamentalMatrix(imagePointsR,imagePointsL,'Method','Norm8Point');

    [worldOrientationR,worldLocationR] = estimateWorldCameraPose(imagePointsR,worldPoints,cameraParamsR);
    [worldOrientationL,worldLocationL] = estimateWorldCameraPose(imagePointsL,worldPoints,cameraParamsL);
    
% Vnější parametry kamer
    [R, t] = cameraPoseToExtrinsics(worldOrientationR,worldLocationR);
    cameraMatrixR = cameraMatrix(cameraParamsR, R, t);
    [R, t] = cameraPoseToExtrinsics(worldOrientationL,worldLocationL);
    cameraMatrixL = cameraMatrix(cameraParamsL, R, t);
    
% Hledání korespondujících bodů na snímcích sloužících k rekonstrukci
    correspondencesR = [0 0];
    correspondencesL = [0 0];
    kC=1;
    
    for i=3+sum(set(1:setIndex-1)):sum(set(1:setIndex))
        line1R = imfill(imgR{i}(:,:,1) >= filter(setIndex)*255/100, 'holes');
        line1L = imfill(imgL{i}(:,:,1) >= filter(setIndex)*255/100, 'holes');
        
        colorR = imgR{2+sum(set(1:setIndex-1))};
        colorL = imgL{2+sum(set(1:setIndex-1))};
        
        redR = colorR(:,:,1);
        greenR = colorR(:,:,2);
        blueR = colorR(:,:,3);

        redL = colorL(:,:,1);
        greenL = colorL(:,:,2);
        blueL = colorL(:,:,3);
        
        detectedPointsR = [];
        detectedPointsL = [];
        epipolarLinesR = [];
        epipolarLinesL = [];
        kR=1; kL=1;
        for v = 1:size(line1R,1)
            u = getIndex(line1R(v,:),0);
            if u
                detectedPointsR(kR,1) = u;
                detectedPointsR(kR,2) = v;
                kR=kR+1;
            end
            u = getIndex(line1L(v,:),0);
            if u
                detectedPointsL(kL,1) = u;
                detectedPointsL(kL,2) = v;
                kL=kL+1;
            end
        end
        
% Prochází podle rozsahu, testování kandidátů pomocí epipolárního omezení
        range = matchingRange(setIndex);
        for j = 1:size(detectedPointsR,1)
            epipolarConstraint = inf*ones(1,2*range+1);
            candidates = zeros(2*range+1,2);
            for k=1:1+2*range
                if (j-range+k-1) > 0 && (j-range+k-1) <= size(detectedPointsL,1)
                    epipolarConstraint(k) = abs([detectedPointsR(j,1) detectedPointsR(j,2) 1]*F*[detectedPointsL(j-range+k-1,1) detectedPointsL(j-range+k-1,2) 1]');%+abs([detectedPointsR(j,1) detectedPointsR(j,2) 1]*F1*[detectedPointsL(j-range+k-1,1) detectedPointsL(j-range+k-1,2) 1]');
                    candidates(k,1) = detectedPointsL(j-range+k-1,1);
                    candidates(k,2) = detectedPointsL(j-range+k-1,2);
                end    
            end
            if min(epipolarConstraint) < tolerance(setIndex)
                k=find(epipolarConstraint == min(epipolarConstraint));
                    correspondencesR(kC,1) = detectedPointsR(j,1);
                    correspondencesR(kC,2) = detectedPointsR(j,2);
                    correspondencesL(kC,1) = candidates(k,1);
                    correspondencesL(kC,2) = candidates(k,2);
                    R = redR(detectedPointsR(j,2),detectedPointsR(j,1));
                    G = greenR(detectedPointsR(j,2),detectedPointsR(j,1));
                    B = blueR(detectedPointsR(j,2),detectedPointsR(j,1));
                    color(kC,:) = [R G B];
                    kC=kC+1;
            end
        end
    end
    
% Filtrace odlehlých hodnot
if filtration
        [~, epipolarInliers, ~] = estimateFundamentalMatrix(...
        correspondencesR, correspondencesL, 'Method', algorithm, ...
        'NumTrials', 1000, 'DistanceThreshold', 10, 'Confidence', 50);

        correspondencesR = correspondencesR(epipolarInliers, :);
        correspondencesL = correspondencesL(epipolarInliers, :);
        color = color(epipolarInliers, :);
end
    
    
if ~isempty(correspondencesR)
    % Kontrola korespondujících bodů
    if check
        figure(setIndex)
        showMatchedFeatures(line1R,line1L,correspondencesR,correspondencesL,'montage','PlotOptions',{'ro','go','y--'});
        hold on
    end

    % Výpočet bodů v prostoru
    points = triangulate(correspondencesR,correspondencesL,cameraMatrixR,cameraMatrixL);
    fprintf('%d', setIndex);
end

% Posun bodů v prostoru, aby respektovaly referenční bod

if (rot(setIndex)>=180) && (rot(setIndex)<270)
    points(:,1) = -points(:,1);
    points(:,2) = -points(:,2);
end

points(:,1) = points(:,1) + ofset(setIndex,1);
points(:,2) = points(:,2) + ofset(setIndex,2);
points(:,3) = points(:,3) + ofset(setIndex,3);
    
pointsCloud = [pointsCloud; points];
C = [C; color];
points = [];

end

%% Vykreslení modelu
pointsCloud(:,3)=pointsCloud(:,3) - min(pointsCloud(:,3));

figure(setIndex+1)
ptCloud = pointCloud([pointsCloud(:,1),pointsCloud(:,2),pointsCloud(:,3)],'Color',uint8(C));
pcshow(ptCloud)    

xlabel('x [mm]') 
ylabel('y [mm]') 
zlabel('z [mm]') 
grid on
daspect([1 1 1])
  