function varargout = StereopairCalibration(varargin)
% STEREOPAIRCALIBRATION MATLAB code for StereopairCalibration.fig

gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @StereopairCalibration_OpeningFcn, ...
                   'gui_OutputFcn',  @StereopairCalibration_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

function StereopairCalibration_OpeningFcn(hObject, eventdata, handles, varargin)

handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

function varargout = StereopairCalibration_OutputFcn(hObject, eventdata, handles) 

varargout{1} = handles.output;

function LeftPhotoSelect_Callback(hObject, eventdata, handles)

%Vyber leve kalibracni fotografie, jeji zobrazeni

clear global namel

global namel
global Imgl

[namel,PathName] = uigetfile({'*.png';'*.jpg';'*.jpeg';'*.raw'},'MultiSelect','on');

namel = char(namel);

addpath(PathName);

Imgl=imread(namel); 
axes(handles.axes1);
imshow(Imgl);

function RightPhotoSelect_Callback(hObject, eventdata, handles)

%Vyber prave kalibracni fotografie, jeji zobrazeni

clear global namer

global namer
global Imgr

[namer,PathName] = uigetfile({'*.png';'*.jpg';'*.jpeg';'*.raw'},'MultiSelect','on');

namer = char(namer);

addpath(PathName);

Imgr=imread(namer); 
axes(handles.axes2);
imshow(Imgr);

function Canny_Callback(hObject, eventdata, handles)

%Zobrazi Cannyho algoritmem upravene fotografie

global Imgl
global Imgr

axes(handles.axes1);
sens = str2double(get(handles.Sensitivity,'String'));
w=im2double(rgb2gray(Imgl)).*255; 
Img = edge(w,'Canny',sens);
imshow(Img);

axes(handles.axes2);
sens = str2double(get(handles.Sensitivity,'String'));
w=im2double(rgb2gray(Imgr)).*255; 
Img = edge(w,'Canny',sens);
imshow(Img);

function Compute_Callback(hObject, eventdata, handles)

%POCATECNI NASTAVENI NEKTERYCH PROMENNYCH

h = waitbar(0,'Hledani kalibracnich bodu');                                 %progressbar

global namel                                                                %jmena leveho a praveho snimku
global namer
global name
name = [namel;namer];

%PRIPRAVA GLOBALNICH PROMENNYCH

global MatchCell
global xCell
global yCell
global XCell
global YCell
global ZCell
global xcorCell
global ycorCell
global K1G
global K2G
global K3G
global P1G
global P2G
global C1G
global C2G
global DETG
global CameraParams

%NASTAVENI KRITERII PRO VYTRIDENI ELIPS Z OBRAZU

Small = str2double(get(handles.Small,'String'));                            %dolni hranice delky vedlejsich poloos hledanych elips
Bigmin = str2double(get(handles.DiameterMin,'String'));                     %dolni hranice delky hlavnich poloos
Bigmax = str2double(get(handles.DiameterMax,'String'));                     %horni hranice delky hlavnich poloos hledanych elips
Distortion = str2double(get(handles.Distortion,'String'));                  %maximalni zkresleni - zavisi na uhlu, pod kterym je elipsa zachycena
Sens = str2double(get(handles.Sensitivity,'String'));                       %citlivost Cannyho algoritmu
Convar = str2double(get(handles.Convar,'String'));                          %vyplnena oblast deleno konvexni oblast - odstrani nekonvexni utvary
Tolerance = 1;                                                              %maximalni zbytek nejmensich ctvercu po prolozeni bodu hranice elipsy obecnou rovnici elipsy
CutOff = 250;                                                               %"orez" snimku - elipticke oblasti se stredem, ktery je bliz jakekoli hrane nez toto cislo [pix], nebude dale uvazovan

%NASTAVENI PARAMETRU PRO STAR OPERATOR, SKENOVANI A ROZPOZNAVANI KODU

Star = 8;                                                                   %pocet paprsku star operatoru deleno dvema (je jich 2x tolik) (= pocet primek)                                                                  %Krok na primce (vzdálenost dvou nainterpolovaných pixel? po primce)                                                                             %Bude nastaveno podle spolehlivosti algoritmu dekodujici znacky
Steps = linspace(7,38,20);                                                  %radialni "rozliseni", kroky od stredu, v kterych bude skenovano                                                                                 
MaxHamming = 0.1;                                                           %maximalni Hammingova vzdalenost rozpoznaneho kodu od kodu v knihovne

%NACTENI DOKUMENTU

knihovna = transpose(xlsread(get(handles.Library,'String')));               %knihovna kodu kalibracnich znacek
souradnice = xlsread(get(handles.Coordinates,'String'));                    %knihovna souradnic ref. znacek
NonLinIntris(1,:) = xlsread(get(handles.NonLinCam1,'String'));              %nelinearni parametry leve a prace kamery
NonLinIntris(2,:) = xlsread(get(handles.NonLinCam2,'String'));

for i=1:2                                                                   %cyklus ziskani dat z prave a leve fotografie
    
    delete(h)                                                               %progressbar
    h = waitbar(i/2,'Hledani kalibracnich bodu');
    
%NACTENI OBRAZKU, UPRAVA, NALEZENI ELIPS, VYPOCET SOURADNIC JEJICH STREDU,
%CTENI KODU A JEJICH ROZPOZNANI
  
    Img = imread(name(i,:));                                                %nacteni obrazku
    
    GrayImg = im2double(rgb2gray(Img)).*255;                                %prevedeni do cernobile a na skalu od 0 do 255
    Canny = edge(GrayImg,'Canny',Sens);                                     %vytvoreni obrazu upraveneho Cannyho algoritmem
    
    [EllipseData] = EllipseDetect(Canny,Distortion,Bigmin,Bigmax,Small,Convar,Tolerance,CutOff);
                                                                            %vytrideni elips dle zadanych parametru 
    [EllipseDataAccurate] = StarOperator(GrayImg,EllipseData,Star);         %star operator - vypocet souradnic stredu kal. znacek v obraze
    
    [Codes] = CodeRead(GrayImg,EllipseData,Steps);                          %cteni kodu okolo znacky
    
    [Match] = CodeRecognition(Codes,souradnice,knihovna,EllipseDataAccurate,MaxHamming);
                                                                            %dekodovani znacek, prirazeni si odpovidajicich si souradnic v prostoru a v obraze
%DLT A ULOZENI VYSLEDKU                                                                 
                                                                            
    [DLTResCell{i},B,DETG(i)] = DLT(Match);                                 %DLT
    
    CDLT(i) = DLT_Recalc(DLTResCell{i},B,Match);                            %chyba rekalkulace
    
    MatchCell{i} = Match;                                                   %ulozeni matice s kody a souradnicemi
                                                              
    xCell{i} = Match(:,5);                                                  %ulozeni souradnic ref. znacek ve snimku
    yCell{i} = Match(:,6);
                                                              
    XCell{i} = Match(:,2);                                                  %ulozeni souradnic ref. znacek v prostoru
    YCell{i} = Match(:,3);
    ZCell{i} = Match(:,4);
                                                              
    xcorCell{i} = xCell{i} - DLTResCell{i}(8);                              %ulozeni korigovanych aouradnic ref. znacek ve snimku (priprava, odecitany jsou souradnice principalniho bodu)
    ycorCell{i} = yCell{i} - DLTResCell{i}(9);
     
    K1G(i) = NonLinIntris(i,1);                                             %definovani nelinearnich parametru kamery - koeficienty radialniho, tangencialniho a afinniho zkresleni
    K2G(i) = NonLinIntris(i,2);
    K3G(i) = NonLinIntris(i,3);
    P1G(i) = NonLinIntris(i,4);
    P2G(i) = NonLinIntris(i,5);
    C1G(i) = NonLinIntris(i,6);
    C2G(i) = NonLinIntris(i,7);
      
end

%ODSTRANENI ZKRESLENI SNIMKU
                                                                          
global ijk                                                                            
for ijk = 1:2
    
    delete(h)
    h = waitbar(ijk/2,'Odstraneni zkresleni snimku');                       %progressbar
    
    DLTresult = DLTResCell{ijk};                                            %parametry kamery - pocatecni hodnoty
    
    [CameraParams{ijk},ResDev(ijk),R{ijk}] = DistortionRemove(DLTresult);   %prostorova resekce se znamymi nelinearnimi parametry
  
end

set(handles.ReprojectionError1,'String',num2str(ResDev(1)));                %zapis odchylek rekalkulace do GUI
set(handles.ReprojectionError2,'String',num2str(ResDev(2)));

MatchCell{1}(:,5) = xcorCell{1};                                            %nahrazeni souradnic bodu merenych ve snimku souradnicemi korigovanymi
MatchCell{1}(:,6) = ycorCell{1};
MatchCell{2}(:,5) = xcorCell{2};
MatchCell{2}(:,6) = ycorCell{2};

%KALIBRACE STEREOPARU - SVAZANI ZOBRAZENI

delete(h)
h = waitbar(1/2,'Kalibrovani stereoparu');                                  %prograssbar
[sumpnp,scale] = StereoVisionCal(CameraParams, R);                          %kalibrace stereoparu
set(handles.RelativeOrientationQuality,'String',num2str(sumpnp));           %vypis kvality kalibrace stereoparu do GUI - rozdil rekalkulaci znacek z jednotlivych kaemr v prostoru

%VYPOCET TRANSFORMACE Z MODELOVEHO S. S. DO GLOBALNIHO S. S.

delete(h)
h = waitbar(1/2,'Vypocet globalnich souradnic');                            %progressbar
[Qual] = Model2Global(R,scale);                                             %vypocet transformace
set(handles.SpaceMarkersRecalc,'String',num2str(Qual));                     %vypis kvality kalibrace do GUI - rozdil rekalkulovanych souradnic od souradnic referencnich

delete(h)

function SaveParams_Callback(hObject, eventdata, handles)

%Ulozeni parametru stereoparu do .xlsx souboru - docasne reseni, v budnoucnu
%budnou vyuzivany primo v dalsich krocich aplikace

global Vbxm                                                                 %baze v modelovem s. s.
global Vbym
global Vbzm
global OmegaR                                                               %rotace mezi s. s. kamer
global FiR
global KapaR

global ScaleAbs                                                             %meritko mezi modelovym s. s. a s. s. absolutnim
global OmegaAbs                                                             %rotace mezi modleovym s. s. a s. s. absolutnim
global FiAbs
global KapaAbs
global VxAbs                                                                %posunuti mezi modelovym s. s. a s. s. absolutnim 
global VyAbs
global VzAbs

a = [Vbxm,Vbym,Vbzm,OmegaR,FiR,KapaR,ScaleAbs,OmegaAbs,FiAbs,KapaAbs,VxAbs,VyAbs,VzAbs];
xlswrite('ParametryStereoparu.xlsx',a);                                     %zapis vysledku

function MarkersDecoded_Callback(hObject, eventdata, handles)

%Funkce zobrazi oba kalibracni snimky a popise na nich nalezene kalibr.
%znacky

global Imgl
global Imgr

global CameraParams
global MatchCell

Px1 = CameraParams{1}(8);
Py1 = CameraParams{1}(9);
Px2 = CameraParams{2}(8);
Py2 = CameraParams{2}(9);

figure

imshow(Imgl);                                                             

hold on;

format short
for i = 1:size(MatchCell{1},1)
    text(MatchCell{1}(i,5)+Px1+25,MatchCell{1}(i,6)+Py1-30,num2str(MatchCell{1}(i,1)),'FontSize',16,'Color','g');
end
hold off

figure

imshow(Imgr);                                                                %Vykreslovani

hold on;

format short
for i = 1:size(MatchCell{2},1)
    text(MatchCell{2}(i,5)+Px2+25,MatchCell{2}(i,6)+Py2-30,num2str(MatchCell{2}(i,1)),'FontSize',16,'Color','g');
end
hold off

function SpaceDistr_Callback(hObject, eventdata, handles)

%Funkce zobrazi 3D graf, kde lze videt obe polohy kamer a vazebne body v
%globalnim s. s. - souradnice jak rekalkulovane, tak referencni

global X
global Y
global Z
global Xp
global Yp
global Zp

global Vx1
global Vy1
global Vz1
global Vx2
global Vy2
global Vz2

figure
hold on
axis equal
scatter3(Vx1,Vy1,Vz1,'r')
scatter3(Vx2,Vy2,Vz2,'r')

scatter3(X,Y,Z,'k')
scatter3(Xp,Yp,Zp,'g');

function Distortion_Callback(hObject, eventdata, handles)

function Distortion_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function DiameterMax_Callback(hObject, eventdata, handles)

function DiameterMax_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function DiameterMin_Callback(hObject, eventdata, handles)

function DiameterMin_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function Small_Callback(hObject, eventdata, handles)

function Small_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function Convar_Callback(hObject, eventdata, handles)

function Convar_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function Sensitivity_Callback(hObject, eventdata, handles)

function Sensitivity_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function Coordinates_Callback(hObject, eventdata, handles)

function Coordinates_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function Library_Callback(hObject, eventdata, handles)

function Library_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function SpaceMarkersRecalc_Callback(hObject, eventdata, handles)

function SpaceMarkersRecalc_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function RelativeOrientationQuality_Callback(hObject, eventdata, handles)

function RelativeOrientationQuality_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function ReprojectionError2_Callback(hObject, eventdata, handles)

function ReprojectionError2_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function ReprojectionError1_Callback(hObject, eventdata, handles)

function ReprojectionError1_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function NonLinCam1_Callback(hObject, eventdata, handles)

function NonLinCam1_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function NonLinCam2_Callback(hObject, eventdata, handles)

function NonLinCam2_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end