%% Prostorová rekonstrukce otáčejícího se objektu 
% Následující skript slouží k otestování metod prostorové rekonstrukce
% aktivní triangulace a streoskopie pro rekonstrukci otáčejícího se objektu. 
% V úvodní části uživatel zvolí vstupní data. Má na výběr ze čtyř vzorových 
% sad a možnost použít vlastní data, která musí mít následující tvar:
%   Popis parametrů je ve tvaru:
%   - 'názevProměnné' = popis (datový typ) || [ jednotka ]
%
% Pro metodu aktivní triangulace:
%   - 'img' = zaznamenané snímky, první snímek zachycuje osu rotace (cell)
%   - 'beta' = triangulační úhel [°]
%   - 'b' = rozměr báze [mm]
%   - 'f' = ohnisková vzdálenost [mm]
%   Nepovinné:
%   - 'imgColor = snímky zaznamenávající texturu (cell)
%
% Pro metodu stereoskopie:
%   - 'imgR' = zaznamenané snímky první kamerou, první snímek
%     zachycuje osu rotace (cell)
%   - 'imgL' = zaznamenané snímky druhou kamerou, první snímek 
%     zachycuje osu rotace (cell)
%   - 'stereoParams' = parametry získané kalibrací kamer (stereoParameters)
%   Nepovinné:
%   - 'imgRcolor = snímky zaznamenávající texturu první kamerou (cell)
%   - 'imgLcolor = snímky zaznamenávající texturu druhou kamerou (cell)
%  
% Program vykoná metodu podle předáných vstupních dat.
%
% Požadované skripty:
%    1. getIndex.m
%    2. calibratedStereoVision.m
%    3. meshGeneration.m (při vykreslovaní modelu pomocí trojúhelníků,
%       nebo čtyřúhelníků)
%
% Požadované toolboxy:
%    1. Computer Vision Toolbox
%
% pozn.: Vzhledem k omezení velikosti příloh jsou vstupní data pro
% rekonstrukci figurky a vázy pomocí stereoskopie 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í data
% Výběr vstupních dat

method = 2;             % 1 = Stereskopie - váza
                        % 2 = Aktivní triangulace - váza
                        % 3 = Stereoskopie - figurka
                        % 4 = Aktivní triangulace - figurka
                        % 5 = Vlastní data

recommended = 1;        % Použít doporučené parametry
                        % 1 - Ano
                        % 0 - Ne
                    
% Definice vlastních parametrů                    
if(~recommended)    
    
    direction = 1;      % Směr otáčení objektu
                        % -1 - V protisměru hodinových ručiček
                        % +1 - Ve směru hodinových ručiček  
                        
    maxVerts = 1600;    % Maximální počet vrcholů modelu tvořeného
                        % čtyřúhelníky (trojúhelníky)
                        % minimálně 2*(n-1)

    rank = 4;           % Řád polynomiální křivky
                        % 0 - Neprokládat body polynomem
                        % n - Řád polynomu
                        
    subRank = 2;        % Řád po částech polynomiální křivky
                        % 0 - Neprokládat body polynomem
                        % n - Řád polynomu
                        
    subPolyCount = 3;   % Počet polynomiálních křivek, které tvoří po 
                        % částech polynomiální křivku

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

    flat = 0;           % Korekce přesahů
                        % 0 - Bez korekce
                        % 1 - Korekce proložením do min/max hodnot
                        % 2 - Korekce průměrováním

    sym = 0;            % Symetrický objekt
                        % 0 - Ne
                        % 1 - Ano    

    fig = [1 0 0 0];    % Způsob vykreslení
                        % 0 - Ne
                        % 1 - Ano
                        % fig(1) - Mračno bodů
                        % fig(2) - Křivky
                        % fig(3) - Trojúhelníkové plochy
                        % fig(4) - Čtyřúhelníkové plochy    
    
    faceColor = [171 104 84]; 
                        % Barva čtyřúhelníkových (trojúhelníkových) ploch
                        % faceColor(1) = mohutnost červené <0;255>
                        % faceColor(2) = mohutnost zelené <0;255>
                        % faceColor(3) = mohutnost modré <0;255>
                        
	savePointCloud = 0; % Uložit mračno bodů ve formátu ply
                        % 1 - Ano
                        % 0 - Ne
                        
    fileName = 'model.ply';
                        % Název souboru ve tvaru: 'nazev.ply' (pouze při
                        % uložení mračna bodů)
                    
end

switch method
    case 1 % Stereskopie - váza
        load vazaStereoVision
        n=length(imgL);
        
% Doporučené parametry
        if recommended 
            flat = 1;
            sym = 1;
            maxVerts = 2*(n-1)*2^4;
            filter = 10;
            rank = 4;
            subRank = 0;
            fig = [1 1 1 1];
            direction = 1;
            savePointCloud = 0;
            faceColor = [171 104 84];
        end

    case 2 % Aktivní triangulace - váza
        load vazaActiveTriangulation
        n=length(img);
% Doporučené parametry    
        if recommended 
            flat = 2;
            sym = 1;
            maxVerts = 2*(n-1)*2^4;
            filter = 20;
            rank = 4;
            subRank = 0;
            fig = [1 1 1 1];
            direction = 1;
            savePointCloud = 0;
            faceColor = [171 104 84];
        end

    case 3 % Stereoskopie - figurka
        load figurkaStereoVision
        if length(imgL) == length(imgR)
            n=length(imgL);
        end
        
% Doporučené parametry
        if recommended 
            flat = 0;
            sym = 0;
            filter = 30; 
            rank = 0;
            subRank = 0;
            fig = [1 0 0 0];
            direction = 1;
            savePointCloud = 0;
        end
        
    case 4 % Aktivní triangulace - figurka
            load figurkaActiveTriangulation
            n=length(img);
% Doporučené parametry        
            if recommended 
                flat = 0;
                sym = 0;
                filter = 40;
                rank = 0;
                subRank = 0;
                fig = [1 0 0 0];
                direction = 1;
                savePointCloud = 0;
            end
end
%% Metody prostorové rekonstrukce
% Společné pro všechny metody
if rank
    verts = floor(maxVerts/(n-1));            
    aproxPoints = zeros(verts*(n-1),3);
    polynomial = zeros(n-1,rank+1);
end
if subRank
        subPoly = zeros(n-1,rank+1);
        subAproxPoints = zeros(subPolyCount-1,verts*(n-1),3);
        nPoints = zeros(1,n-1);
end
if flat == 2
    minY = []; maxY = [];
end

if exist('imgRcolor','var') || exist('imgLcolor','var') || exist('imgColor','var')
    C=[];
end
pointsCloud=[];

% Volba metody podle předaných parametrů

% Stereoskopie
if exist('stereoParams','var') && exist('imgR','var') && exist('imgL','var')

% Zjištění souřadnic osy rotace
    oRot = calibratedStereoVision(imgR{1},imgL{1},stereoParams,filter);
    zRot = mean(oRot(:,3));
    minRot = max(oRot(:,2));
    
    for i=2:n
        if exist('imgRcolor','var') && exist('imgLcolor','var')
            [points,color] = calibratedStereoVision(imgR{i},imgL{i},stereoParams,filter,imgRcolor{i-1},imgLcolor{i-1});
        elseif exist('imgRcolor','var')
            [points,color] = calibratedStereoVision(imgR{i},imgL{i},stereoParams,filter,imgRcolor{i-1},1);
        elseif exist('imgLcolor','var')
            [points,color] = calibratedStereoVision(imgR{i},imgL{i},stereoParams,filter,imgLcolor{i-1},2);    
        else
            [points] = calibratedStereoVision(imgR{i},imgL{i},stereoParams,filter);
        end
        if numel(points) >= 3
            points(:,1) = direction*2*(i-2)*pi/(n-1);
            points(:,3) = points(:,3) - zRot;
            
% Proložení bodů polynomem 
            if rank
                polynomial(i-1,:) = polyfit(points(:,2),points(:,3),rank);
                if (min(points(:,2)) ~= max(points(:,2))) && ~flat
                    aproxPoints(verts*(i-2)+1:verts*(i-1),1) = direction*2*(i-2)*pi/(n-1);
                    aproxPoints(verts*(i-2)+1:verts*(i-1),2) = linspace(min(points(:,2)),max(points(:,2)),verts);
                    aproxPoints(verts*(i-2)+1:verts*(i-1),3) = polyval(polynomial(i-1,:),linspace(min(points(:,2)),max(points(:,2)),verts));
                end
                if flat == 2
                    minY = [minY min(points(:,2))];
                    maxY = [maxY max(points(:,2))];
                end
            end
            if subRank
               nPoints(i-1) =  size(points,1);
            end
            pointsCloud = [pointsCloud; points];
            if exist('C','var')
                C = [C; color];
            end
        end
        points = [];
    end
    
% Aktivní triangulace    
elseif exist('img','var') && exist('beta','var')
    
% Zjištění souřadnic osy rotace
        laser = imfill(img{1}(:,:,1) >= filter*255/100, 'holes');
        uRot = []; vRot = [];
        for v=1:size(laser,1)
            u = getIndex(laser(v,:),1);
            if u
                uRot = [uRot u];
                vRot = [vRot v*b/f];
            end
        end 
        uRot = mean(uRot);
        minRot = max(vRot);
        m=1;
        for i=2:n
            
            laser = imfill(img{i}(:,:,1) > filter*255/100, 'holes');
            len = size(laser,1);
            k=1;
            for v=1:len 
                 u = getIndex(laser(v,:),1);
                 if u
                     points(k,1) = direction*2*(i-2)*pi/(n-1);
                     points(k,3) = (u-uRot)*tand(beta)*b/f;
                     points(k,2) = v*b/f;
                     if exist('imgColor','var')
                         red = imgColor{i-1}(:,:,1);
                         green = imgColor{i-1}(:,:,2);
                         blue = imgColor{i-1}(:,:,3);
                         C(m,:) = [red(v,u) green(v,u) blue(v,u)];
                         m=m+1;
                     end
                     k=k+1;
                 end
            end
                     
% Proložení bodů polynomem
            if rank
                if (min(points(:,2)) ~= max(points(:,2)))
                    polynomial(i-1,:) = polyfit(points(:,2),points(:,3),rank);
                    if ~flat
                        aproxPoints(verts*(i-2)+1:verts*(i-1),1) = 2*(i-2)*pi/(n-1);
                        aproxPoints(verts*(i-2)+1:verts*(i-1),2) = linspace(min(points(:,2)),max(points(:,2)),verts);
                        aproxPoints(verts*(i-2)+1:verts*(i-1),3) = polyval(polynomial(i-1,:),linspace(min(points(:,2)),max(points(:,2)),verts));
                    elseif flat == 2
                        minY = [minY min(points(:,2))];
                        maxY = [maxY max(points(:,2))];
                    end 
                end
            end
            if subRank
               nPoints(i-1) =  size(points,1);
            end
            pointsCloud = [pointsCloud ;points];
            points = [];
        end
else
    fprintf('Nebyly předány parametry potřebné k použití jedné z metod.\n');
    return
end

% Po částech polynomiální křivky
if subRank
    pointsCloud(:,2)=-pointsCloud(:,2);
    vec = linspace(min(pointsCloud(:,2)),max(pointsCloud(:,2)),subPolyCount);
    subVerts = zeros(1,subPolyCount-1);
  
    for j=1:subPolyCount-1
        subAproxPoints = [];

        for i=1:n-1
            subPointsCloud = [];
            fi = direction*2*(i-2)*pi/(n-1);
            mask = (pointsCloud(sum(nPoints(1:i))-nPoints(i)+1:sum(nPoints(1:i)),2)<=vec(j+1)).*(pointsCloud(sum(nPoints(1:i))-nPoints(i)+1:sum(nPoints(1:i)),2)>=vec(j));
            subPointsCloud(:,1) = nonzeros(pointsCloud(sum(nPoints(1:i))-nPoints(i)+1:sum(nPoints(1:i)),2).*mask);
            subPointsCloud(:,2) = nonzeros(pointsCloud(sum(nPoints(1:i))-nPoints(i)+1:sum(nPoints(1:i)),3).*mask);

            if (size(subPointsCloud(:,1),1) > rank) && (size(subPointsCloud(:,2),1) > rank)
                subVerts(j) = subVerts(j) + 1;
                subPoly = polyfit(subPointsCloud(:,1),subPointsCloud(:,2),rank);

                subAproxPoints((verts)*(i-1)+1:(verts)*(i),1) = fi;
                subAproxPoints((verts)*(i-1)+1:(verts)*(i),2) = linspace(vec(j),vec(j+1),verts);
                subAproxPoints((verts)*(i-1)+1:(verts)*(i),3) = polyval(subPoly,linspace(min(subPointsCloud(:,1)),max(subPointsCloud(:,1)),verts));            
            end
        end
        [subAproxPoints(:,1),subAproxPoints(:,2),subAproxPoints(:,3)] = pol2cart(subAproxPoints(:,1),subAproxPoints(:,3),subAproxPoints(:,2));
        figure(10)
        meshGeneration(subAproxPoints, verts, subVerts(j))
        hold on
        figure(11)
        scatter3(subAproxPoints(:,1),subAproxPoints(:,2),subAproxPoints(:,3))
        hold on
        if fig(4)
            figure(10)
            meshGeneration(subAproxPoints, verts, subVerts(j))
            xlabel('x [mm]') 
            ylabel('y [mm]') 
            zlabel('z [mm]') 
            set(gca,'FontSize',14)
            grid on
            daspect([1 1 1])
        end
    end
    if fig(2)
        figure(11)
        scatter3(subAproxPoints(:,1),subAproxPoints(:,2),subAproxPoints(:,3),'Marker','.')
        grid on
        xlabel('x [mm]') 
        ylabel('y [mm]') 
        zlabel('z [mm]') 
        set(gca,'FontSize',14)
        daspect([1 1 1])
    end
    pointsCloud(:,2)=-pointsCloud(:,2);
end

%% Korekce 
% Korekce přesahů
if flat == 1
    for i = 2:n
        aproxPoints(verts*(i-2)+1:verts*(i-1),1) = 2*(i-2)*pi/(n-1);
        aproxPoints(verts*(i-2)+1:verts*(i-1),2) = linspace(min(pointsCloud(:,2)),max(pointsCloud(:,2)),verts);
        aproxPoints(verts*(i-2)+1:verts*(i-1),3) = polyval(polynomial(i-1,:),linspace(min(pointsCloud(:,2)),max(pointsCloud(:,2)),verts));
    end
elseif flat == 2    
    for i = 2:n
        aproxPoints(verts*(i-2)+1:verts*(i-1),1) = 2*(i-2)*pi/(n-1);
        aproxPoints(verts*(i-2)+1:verts*(i-1),2) = linspace(mean(minY(:,2)),mean(maxY(:,2)),verts);
        aproxPoints(verts*(i-2)+1:verts*(i-1),3) = polyval(polynomial(i-1,:),linspace(mean(minY(:,2)),mean(maxY(:,2)),verts));
    end 
end

% Korekce chybně určené osy rotace
if sym 
    minimal = zeros(1,verts); maximal = zeros(1,verts);
    
    for j = 1:verts
           minimal(j) = min(aproxPoints(j:verts:(n-1)*verts-verts+(verts-j+1),3));
           maximal(j) = max(aproxPoints(j:verts:(n-1)*verts-verts+(verts-j+1),3)); 
    end
    
    exc = (minimal+maximal)/2;
    repmat(exc,1,n-1);
    symPoints = aproxPoints;
    symPoints(:,3) = repmat((minimal+maximal)/2,1,n-1);

    [symPoints(:,1),symPoints(:,2),symPoints(:,3)] = pol2cart(symPoints(:,1),symPoints(:,3),symPoints(:,2));
    symPoints(:,3)=flipud(symPoints(:,3)-min(symPoints(:,3)));
end



% Transformace do kartézského souřadného systému
[pointsCloud(:,1),pointsCloud(:,2),pointsCloud(:,3)] = pol2cart(pointsCloud(:,1),pointsCloud(:,3),pointsCloud(:,2));

% Zahodit vše, co je pod osou rotace
pointsCloud(:,1)=pointsCloud(:,1).*(pointsCloud(:,3)<minRot);
pointsCloud(:,2)=pointsCloud(:,2).*(pointsCloud(:,3)<minRot);
pointsCloud(:,3)=pointsCloud(:,3).*(pointsCloud(:,3)<minRot);
pointsCloud((pointsCloud(:,3) == 0),1) = NaN;
pointsCloud((pointsCloud(:,3) == 0),2) = NaN;
pointsCloud((pointsCloud(:,3) == 0),3) = NaN;
if exist('C','var')
    C(:,1)=C(:,1).*(pointsCloud(:,3)<minRot);
    C(:,2)=C(:,2).*(pointsCloud(:,3)<minRot);
    C(:,3)=C(:,3).*(pointsCloud(:,3)<minRot);
    C((pointsCloud(:,3) == 0),1) = NaN;
    C((pointsCloud(:,3) == 0),2) = NaN;
    C((pointsCloud(:,3) == 0),3) = NaN;
end
pointsCloud(:,3)=-pointsCloud(:,3)-min(-pointsCloud(:,3));
if rank
    [aproxPoints(:,1),aproxPoints(:,2),aproxPoints(:,3)] = pol2cart(aproxPoints(:,1),aproxPoints(:,3),aproxPoints(:,2));
    aproxPoints(:,3)=flipud(aproxPoints(:,3)-min(aproxPoints(:,3)));
end

%% Vykreslení modelu
if fig(1) % Mračno bodů
    figure(1)
    if exist('C','var')
        ptCloud = pointCloud([pointsCloud(:,1),pointsCloud(:,2),pointsCloud(:,3)],'Color',uint8(C));
        pcshow(ptCloud,'MarkerSize',14)    
    else
        scatter3(pointsCloud(:,1),pointsCloud(:,2),pointsCloud(:,3),'Marker','.')
    end
    
    grid on
    xlabel('x [mm]') 
    ylabel('y [mm]') 
    zlabel('z [mm]') 
    set(gca,'FontSize',14)
    daspect([1 1 1])
end
if fig(2) % Aproximované body
    figure(2)
    if sym
        scatter3(symPoints(:,1),symPoints(:,2),symPoints(:,3),'Marker','.')
    else
        scatter3(aproxPoints(:,1),aproxPoints(:,2),aproxPoints(:,3),'Marker','.')
    end
    
    grid on
    xlabel('x [mm]') 
    ylabel('y [mm]') 
    zlabel('z [mm]') 
    set(gca,'FontSize',14)
    daspect([1 1 1])
end
if fig(3) % Model tvořený trojúhelníky
    if sym
        meshGeneration(symPoints, verts, n-1, 'tri')
    else
        meshGeneration(aproxPoints, verts, n-1, 'tri')
    end
    
    xlabel('x [mm]') 
    ylabel('y [mm]') 
    zlabel('z [mm]') 
    set(gca,'FontSize',14)
    grid on
    daspect([1 1 1])
end
if fig(4) % Model tvořený čtyřúhelníky
    figure(4)
    if sym
        meshGeneration(symPoints, verts, n-1, 'quad')
    else
        meshGeneration(aproxPoints, verts, n-1, 'quad')
    end
    
    xlabel('x [mm]') 
    ylabel('y [mm]') 
    zlabel('z [mm]') 
    set(gca,'FontSize',14)
    grid on
    daspect([1 1 1])
end
if savePointCloud
    if ~exist('ptCloud','var')
        if exist('C','var')
            ptCloud = pointCloud([pointsCloud(:,1),pointsCloud(:,2),pointsCloud(:,3)],'Color',uint8(C)); 
        else
            ptCloud = pointCloud([pointsCloud(:,1),pointsCloud(:,2),pointsCloud(:,3)]);
        end
    end
    pcwrite(ptCloud,fileName)
end
