function varargout = Rekonstrukce(varargin)
% REKONSTRUKCE MATLAB code for Rekonstrukce.fig
%      REKONSTRUKCE, by itself, creates a new REKONSTRUKCE or raises the existing
%      singleton*.
%
%      H = REKONSTRUKCE returns the handle to a new REKONSTRUKCE or the handle to
%      the existing singleton*.
%
%      REKONSTRUKCE('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in REKONSTRUKCE.M with the given input arguments.
%
%      REKONSTRUKCE('Property','Value',...) creates a new REKONSTRUKCE or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before Rekonstrukce_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to Rekonstrukce_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help Rekonstrukce

% Last Modified by GUIDE v2.5 17-May-2017 20:09:34

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @Rekonstrukce_OpeningFcn, ...
                   'gui_OutputFcn',  @Rekonstrukce_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT


% --- Executes just before Rekonstrukce is made visible.
function Rekonstrukce_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to Rekonstrukce (see VARARGIN)

% Choose default command line output for Rekonstrukce
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes Rekonstrukce wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = Rekonstrukce_OutputFcn(hObject, eventdata, handles) 
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;


% --- Executes on button press in radiobutton1.
function radiobutton1_Callback(hObject, eventdata, handles)
% hObject    handle to radiobutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hint: get(hObject,'Value') returns toggle state of radiobutton1
if get(handles.radiobutton1,'Value')==1
    set(handles.radiobutton2,'Value',0);
else
    set(handles.radiobutton2,'Value',1);
end

% --- Executes on button press in radiobutton2.
function radiobutton2_Callback(hObject, eventdata, handles)
% hObject    handle to radiobutton2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hint: get(hObject,'Value') returns toggle state of radiobutton2
if get(handles.radiobutton2,'Value')==1
    set(handles.radiobutton1,'Value',0);
else
    set(handles.radiobutton1,'Value',1);
end

% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global adresar 
StaryAdresar=pwd;
cd(adresar) 
load cameraParams.mat;
cd(StaryAdresar)
pohledy = imageDatastore(adresar);

% zobrazit pohledy, podle aktualniho poctu projekci
global volba
if volba==1 
figure
montage(pohledy.Files, 'Size', [3, 5]);
elseif volba==2
    figure
montage(pohledy.Files, 'Size', [5, 6]);
elseif volba==3
figure
montage(pohledy.Files, 'Size', [5, 9]);
end

% Prevod vsech pohledu do sedotonu
obrazy = cell(1, numel(pohledy.Files));
for i = 1:numel(pohledy.Files)
    I = readimage(pohledy, i);
    obrazy{i} = rgb2gray(I);
end

% % % prirazeni pozice kamer k poloze vyznamnych bodu

% korekce distorze
I = undistortImage(obrazy{1}, cameraParams); 

% Stanoveni hranic detekce v poheldech
border = 45;
roi = [border, border, size(I, 2)- 2*border, size(I, 1)- 2*border];


if get(handles.radiobutton1,'Value')==1
    
% vyhledani vyznamnych bodu, vyber typu detektoru 
vyznamne_body = detectHarrisFeatures(I,  'ROI', roi);
else
    vyznamne_body   = detectSURFFeatures(I,'MetricThreshold', 990, 'ROI', roi);
end

% urceni deskriptory vyzmnych bodu
deskriptory = extractFeatures(I, vyznamne_body);
% vytvoreni vsetu pro ukladani parametru
vSet = viewSet;

% pridani prvniho pohledu
viewId = 1;
vSet = addView(vSet, viewId, 'Points', vyznamne_body, 'Orientation', ...
    eye(3, 'like', vyznamne_body.Location), 'Location', ...
    zeros(1, 3, 'like', vyznamne_body.Location));

% vypocet pro ostatni pohledy
for i = 2:numel(obrazy)
    % korekce distorze
    I = undistortImage(obrazy{i}, cameraParams);
    
    % vyhledani vyznamnych bodu
if get(handles.radiobutton1,'Value')==1
    vyznamne_body_nove = detectHarrisFeatures(I, 'ROI', roi);
else
    vyznamne_body_nove   = detectSURFFeatures(I,'MetricThreshold', 990,  'ROI', roi);
end

% urceni deskriptoru
    deskriptory_nove = extractFeatures(I, vyznamne_body_nove); 
    
%    vyhledani shod z deskriptoru
    indexPairs = matchFeatures(deskriptory, deskriptory_nove, 'MaxRatio', 0.65);
    
    % Stanoveni shod vyznamnych bodu
    shody_1 = vyznamne_body(indexPairs(:, 1));
    shody_2 = vyznamne_body_nove(indexPairs(:, 2));
    
%     stanoveni pozice kamery
    [relativeOrient, relativeLoc, inlierIdx] = helperEstimateRelativePose(...
        shody_1, shody_2, cameraParams);
    
    % pridani vyznamnych bodu do iteho pohledu
    vSet = addView(vSet, i, 'Points', vyznamne_body_nove);
    
    % ulozeni shod mezi i+1 pohledy
    vSet = addConnection(vSet, i-1, i, 'Matches', indexPairs(inlierIdx,:));
    
%    ziskani pozice a orientace z prvniho pohledu
    predchozi_pozice = poses(vSet, i-1);
    predchozi_orientace_kamery = predchozi_pozice.Orientation{1};
    predchzi_pozice_kamery    = predchozi_pozice.Location{1};
        
%     VYpocet relativni orientace kamer
    orientace_kamery = relativeOrient * predchozi_orientace_kamery;
    pozice_kamery    = predchzi_pozice_kamery + relativeLoc * predchozi_orientace_kamery;
    vSet = updateView(vSet, i, 'Orientation', orientace_kamery,'Location', pozice_kamery);
    
%   zisk shodnych bodu   
    tracks = findTracks(vSet);

%    zisk pozic kamery
    camPoses = poses(vSet);

%    vypocet triangulace
    body_xy = triangulateMultiview(tracks, camPoses, cameraParams);
    
%    Zpresneni pozice kamer vuci bodum
    [~, camPoses] = bundleAdjustment(body_xy,tracks, camPoses, cameraParams, 'FixedViewId', 1, 'PointsUndistorted', true);

%    ulozeni presnych pozic kamery
    vSet = updateView(vSet, camPoses);

    deskriptory = deskriptory_nove;
    vyznamne_body   = vyznamne_body_nove;  
end

% vypocet 3d bodu pomoci sledovani bodu point tarcking
I = undistortImage(obrazy{1}, cameraParams); 

% nalezeni vyznamnych bodu

if get(handles.radiobutton1,'Value')==1
vyznamne_body = detectHarrisFeatures(I);
else
    vyznamne_body   =  detectSURFFeatures(I, 'MetricThreshold', 990);
end

% vytvoreni point trackeru
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 4);

% pridani prvni sady vyznamnych bodu
vyznamne_body = vyznamne_body.Location;
initialize(tracker, vyznamne_body, I);

% ulozeni vyznamnych bodu
vSet = updateConnection(vSet, 1, 2, 'Matches', zeros(0, 2));
vSet = updateView(vSet, 1, 'Points', vyznamne_body);

% vypocet pro ostatni pohledy
for i = 2:numel(obrazy)
%     korekce distorze
    I = undistortImage(obrazy{i}, cameraParams); 
    
%    sledovani bodu
    [vyznamne_body_nove, validIdx] = step(tracker, I);
    
%     vymazani starych korespondenci
    if i < numel(obrazy)
        vSet = updateConnection(vSet, i, i+1, 'Matches', zeros(0, 2));
    end
    vSet = updateView(vSet, i, 'Points', vyznamne_body_nove);
    
%   ulozeni novych shodnych bodu
    matches = repmat((1:size(vyznamne_body, 1))', [1, 2]);
    matches = matches(validIdx, :);        
    vSet = updateConnection(vSet, i-1, i, 'Matches', matches);
end

% zisk shodnych bodu   
tracks = findTracks(vSet);

% zisk pozic kamery
camPoses = poses(vSet);

% nalezení 3d bodu
body_xyz = triangulateMultiview(tracks, camPoses,...
    cameraParams);

% zpresneni postaveni kamery a 3D bodu
[body_xyz, camPoses] = bundleAdjustment(body_xyz, tracks, camPoses, cameraParams, 'FixedViewId', 1,'PointsUndistorted', true);


% vizualizace
figure;

% Vykreslení a nastavení prametru pro vykreslení
pcshow(body_xyz, 'VerticalAxis', 'z', 'VerticalAxisDir', 'down','MarkerSize', 30);


% Zadání veikosti zobrazovaného pole
pozice = camPoses.Location{1};
 xlim([pozice(1)-5, pozice(1)+3]);
ylim([pozice(2)-5, pozice(2)+3]);
zlim([pozice(3)-1, pozice(3)+20]);
title('Rekonstrukce');


% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global adresar 
adresar=uigetdir(pwd,'Vyber adresare s obrazky');
