function varargout = NonLinearCamCal(varargin)
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @NonLinearCamCal_OpeningFcn, ...
                   'gui_OutputFcn',  @NonLinearCamCal_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 NonLinearCamCal_OpeningFcn(hObject, eventdata, handles, varargin)
handles.output = hObject;
guidata(hObject, handles);

function varargout = NonLinearCamCal_OutputFcn(hObject, eventdata, handles) 
varargout{1} = handles.output;

function SelectPhotos_Callback(hObject, eventdata, handles)

%Funkce spusti prohlizec a vyzve uzivatele k vybrani kalibracnich
%fotografii, minimalni pocet jsou 2

clear global name                                                           %priprava globalni promenne se jmeny snimku
global name

[FileName,PathName] = uigetfile({'*.png';'*.jpg';'*.jpeg';'*.raw'},'MultiSelect','on');
                                                                            %spusteni prohlizece - vyzve uzivatele k vybrani kalibracnich fotografii, mozne formaty png jpg jpeg raw
for i = 1:size(FileName,2)                                                  %ulozeni jmen do promenne
    name(i,:) = char(FileName{i});
end

addpath(PathName);                                                          %pridani cesty ke snimkum

Fotka = (char(name(1,:)));                                                  %zobrazeni prvni fotografie v GUI
Img = imread(Fotka); 
axes(handles.axes1);
imshow(Img);

set(handles.Images,'String',num2str(size(name,1)));                         %zobrazeni poctu kal. fotografii v GUI

function DispCanny_Callback(hObject, eventdata, handles)

%Funkce zobrazi dany snimek upraveny Cannyho hranovym operatorem s danym
%parametrem filtru - test, zda je parametr volen spravne

global name                                                                 
Fotka = (char(name(1,:)));
Img = imread(Fotka); 
axes(handles.axes1);
Sens = str2double(get(handles.Sensitivity,'String'));
GrayImg = im2double(rgb2gray(Img)).*255; 
Img = edge(GrayImg,'Canny',Sens);
imshow(Img);
 
function SaveParams_Callback(hObject, eventdata, handles)

%Funkce ulozi vypocitane parametry kamery do souboru .xlsx

global CamParams
xlswrite('ParametryKamery.xlsx',CamParams);

function Compute_Callback(hObject, eventdata, handles)

%Provedeni vypoctu nelinearnich parametru kamery - v budoucnu bude
%ukladani neznamych provedeno pomoci strukturovanych promennych nebo cell
%arrays

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

%NASTAVENI KRITERII PRO SELEKCI ELIPS V OBRAZE, DEKODOVANI JEJICH KODU A
%STAR OPERATOR, NACTENI EXCELOVSKYCH DOKUMENTU

Small = str2double(get(handles.Small,'String'));                            %dolni hranice delky vedlejsich poloos hledanych elips
Bigmin = str2double(get(handles.Bigmin,'String'));                          %dolni hranice delky hlavnich poloos
Bigmax = str2double(get(handles.Bigmax,'String'));                          %horni hranice hlavnich poloos hledanych elips
Distortion = str2double(get(handles.Distortion,'String'));                  %maximalni zkresleni - zavisi na uhlu, pod kterym je elipsa na fotografii zachycena
Convar = str2double(get(handles.Convar,'String'));                          %vyplnena oblast deleno konvexni oblast - odstrani nekonvexni utvary
Sens = str2double(get(handles.Sensitivity,'String'));                       %citlivost Cannyho algoritmu
Tolerance = 1;                                                              %maximalni hodnota zbytku po prolozeni hranice elipsy obecnou rovnici elipsy
CutOff = 250;                                                               %"oriznuti" fotografie [pix] - elipsy, jejichz souradnice jsou blize nektere hrane snimku nez je tato mez, nejsou uvazovany, kvuli tomu, aby nedoslo ke skenovani mimo matici snimku 

Star = 8;                                                                   %pocet radialnich paprsku star operatoru deleno dvema (je jich 2x tolik) (= pocet primek)
Steps = linspace(7,38,20);                                                  %radialni "rozliseni", kroky od stredu, v kterych bude skenovan carovy kod - empiricky zjisteno
MaxHamming = 0.1;                                                           %maximalni Hammingova vzdalenost rozpoznaneho bitoveho kodu od kodu v knihovne

LIB = transpose(xlsread(get(handles.LibrarySelect,'String')));              %nacteni knihovny kodu kalibracnich znacek
Coordinates = xlsread(get(handles.TritopCoordinates,'String'));             %nacteni souboru s prostorovymi souradnicemi kalibracnich znacek

global name;                                                                %jmena fotografii

for ijk=1:size(name,1)
    delete(h)
    h = waitbar((ijk)/size(name,1),'Hledani kalibracnich bodu');            %prograssbar
    
%NACTENI OBRAZKU, VYTRIDENI ELIPTICKYCH REGIONU, ZISKANI JEJICH ZAKLADNICH
%CHARAKTERISTIK

    Img=imread(char(name(ijk,:)));                                          %nacteni obrazku
    GrayImg=im2double(rgb2gray(Img)).*255;                                  %prevedeni do cernobile a na cisla od 1 do 255
    Cany=edge(GrayImg,'Canny',Sens);                                        %vytvoreni obrazu upraveneho Cannyho algoritmem - nastavitelna citlivost
    
    [EllipseData] = EllipseDetect(Cany,Distortion,Bigmin,Bigmax,Small,Convar,Tolerance,CutOff);
                                                                            %Vytrideni elips dle zadanych parametru
%STAR OPERATOR - VYPOCET PRESNYCH SOURADNIC STREDU ELIPS
    
    [EllipseDataAccurate] = StarOperator(GrayImg,EllipseData,Star);
    
%CTENI KODU OKOLO ZNACKY
    
    [Scans] = CodeRead(GrayImg,EllipseData,Steps);
    
%ROZPOZNANI KODU A PRIRAZENI PROSTOROVYCH SOURADNIC
    
    [match] = CodeRecognition(Scans,Coordinates,LIB,EllipseDataAccurate,MaxHamming);
    
%PRIMA LINEARNI TRANSFORMACE PRO ZISKANI POCATECNICH HODNOT NEKTERYCH
%PARAMETRU KAMERY
    
    [A,B,DET]=DLT(match);                                                   %prima linearni transformace
    
    CDLT(ijk) = DLT_Recalc(A,B*DET,match);                                  %vycisleni chyby rekalkulace prime linearni transformace
    
%ULOZENI POCATECNICH HODNOT PARAMETRU - V BUDOUCNU BUDOU POUZITY
%STRUKTUROVANE PROMENNE NEBO CELL ARRAYE
    
    eval (['global DET' num2str(ijk)])                                      %vyrovnavaci znamenko matice rotace
    eval (['DET' num2str(ijk) '=DET;'])
    
    eval(['global match' num2str(ijk)]);                                    %matice obsahujici kody znacek, jejich souradnice ve snimku a v prostoru
    eval(['match' num2str(ijk) '= match;']);
    
    eval(['global X' num2str(ijk)]);                                        %souradnice bodu v prostoru
    eval(['X' num2str(ijk) '= match(:,2);']);
    eval(['global Y' num2str(ijk)]);
    eval(['Y' num2str(ijk) '= match(:,3);']);
    eval(['global Z' num2str(ijk)]);
    eval(['Z' num2str(ijk) '= match(:,4);']);
    
    eval(['global x' num2str(ijk)]);                                        %souradnice kalibracnich znacek ve snimku, nekorigovane
    eval(['x' num2str(ijk) '= match(:,5);']);
    eval(['global y' num2str(ijk)]);
    eval(['y' num2str(ijk) '= match(:,6);']);
    
    eval(['Vx' num2str(ijk) '= A(2);']);                                    %poloha stredu promitani kamery v globalnim souradnem systemu
    eval(['Vy' num2str(ijk) '= A(3);']);
    eval(['Vz' num2str(ijk) '= A(4);']);
    
    eval(['Omega' num2str(ijk) '= A(5);']);                                 %uhly rotace podle os x, y, z mezi souradnymi systemy
    eval(['Fi' num2str(ijk) '= A(6);']);
    eval(['Kappa' num2str(ijk) '= A(7);']);
    
    eval(['Px' num2str(ijk) '= A(8);']);                                    %souradnice realneho stredu snimku
    eval(['Py' num2str(ijk) '= A(9);']);
    
    eval(['Cxy' num2str(ijk) '= (A(10)+A(11))/2;']);                        %ohniskova vzdalenost
    
    eval(['global xcor' num2str(ijk)]);                                     %priprava korigovanych hodnot souradnic kalibracnich bodu ve snimku                                       
    eval(['global ycor' num2str(ijk)]);
    eval(['xcor' num2str(ijk) '= x' num2str(ijk) '-Px' num2str(ijk) ';']);
    eval(['ycor' num2str(ijk) '= y' num2str(ijk) '-Py' num2str(ijk) ';']);
      
end

%PROSTOROVA RESEKCE

options = optimoptions(@lsqnonlin,'MaxIter',10,'TolFun',10^(-20),'TolX',10^(-20),'Display','off');%'FiniteDifferenceType','central',
                                                                            %nastaveni resice - centralni num. derivace, vypnute zobrazeni, max. 10 iteraci
K1 = 0;                                                                     %pocatecni hodnoty nelinearnich parametru kamery
K2 = 0;
K3 = 0;
P1 = 0;
P2 = 0;
C1 = 0;
C2 = 0;

for j = 1:10                                                                %reseni, maximalne 10 cyklu, na konci terminacni podminka, pokud je zmena vysledku jiz mala
    
    delete(h)
    h = waitbar((j)/5,'Optimalizace parametru');                            %progressbar
    
    b0(1) = K1;                                                             %prichystani vektoru pocatecnich hodnot pro oprimalizaci - neznamymi jsou nelinearni parametry kamery,
    b0(2) = K2;                                                             %linearni vnitrni parametry kamery a vnejsi parametry kamery, nelinearni parametry kamery K, P, C jsou shodne pro vsechny
    b0(3) = K3;                                                             %pouzite snimky, ostatni parametry zustavaji individualni pro kazdy snimek
    b0(4) = P1;
    b0(5) = P2;
    b0(6) = C1;
    b0(7) = C2;
    
    for i = 1:size(name,1)
        
        eval(['b0(' num2str((i-1)*2+8) ')= Px' num2str(i) ';']);
        eval(['b0(' num2str((i-1)*2+9) ')= Py' num2str(i) ';']);
        
        eval(['b0(' num2str((i-1)+8+2*size(name,1)) ')= Cxy' num2str(i) ';']);
        
        eval(['b0(' num2str((i-1)*3+8+3*size(name,1)) ')= Vx' num2str(i) ';']);
        eval(['b0(' num2str((i-1)*3+9+3*size(name,1)) ')= Vy' num2str(i) ';']);
        eval(['b0(' num2str((i-1)*3+10+3*size(name,1)) ')= Vz' num2str(i) ';']);
        
        eval(['b0(' num2str((i-1)*3+8+6*size(name,1)) ')= Omega' num2str(i) ';']);
        eval(['b0(' num2str((i-1)*3+9+6*size(name,1)) ')= Fi' num2str(i) ';']);
        eval(['b0(' num2str((i-1)*3+10+6*size(name,1)) ')= Kappa' num2str(i) ';']);
        
    end
    
    [bf,rz(j)] = lsqnonlin(@ResectionCorrection,b0,[],[],options);          %reseni problemu metodou nelinearnich nejmensich ctvercu ve smyslu distribuce chyb mezi nezname parametry
    
    K1 = bf(1);                                                             %zapis vysledku - zpresnene parametry snimku
    K2 = bf(2);
    K3 = bf(3);  
    P1 = bf(4);
    P2 = bf(5);
    C1 = bf(6);
    C2 = bf(7);
    
    for i = 1:size(name,1);
        
        eval(['dPx' num2str(i) '= bf(' num2str((i-1)*2+8) ')-Px' num2str(i) ';'])
        eval(['dPy' num2str(i) '= bf(' num2str((i-1)*2+9) ')-Py' num2str(i) ';'])
        
        eval(['Px' num2str(i) '= bf(' num2str((i-1)*2+8) ');'])
        eval(['Py' num2str(i) '= bf(' num2str((i-1)*2+9) ');'])
        
        eval(['Cxy' num2str(i) '= bf(' num2str((i-1)+8+2*size(name,1)) ');']);
        
        eval(['Vx' num2str(i) '= bf(' num2str((i-1)*3+8+3*size(name,1)) ');']);
        eval(['Vy' num2str(i) '= bf(' num2str((i-1)*3+9+3*size(name,1)) ');']);
        eval(['Vz' num2str(i) '= bf(' num2str((i-1)*3+10+3*size(name,1)) ');']);
        
        eval(['Omega' num2str(i) '= bf(' num2str((i-1)*3+8+6*size(name,1)) ');']);
        eval(['Fi' num2str(i) '= bf(' num2str((i-1)*3+9+6*size(name,1)) ');']);
        eval(['Kappa' num2str(i) '= bf(' num2str((i-1)*3+10+6*size(name,1)) ');']);
        
    end
    
    for i = 1:size(name,1)                                                  %provedeni korekce, rekalkulace
        
        eval(['r' num2str(i) '= sqrt((xcor' num2str(i) '-dPx' num2str(i) ').^2 + (ycor' num2str(i) '-dPy' num2str(i) ').^2);']);
                                                                            %korigovana vzdalenost daneho bodu od stredu snimku
        eval(['dx' num2str(i) '= (xcor' num2str(i) '-dPx' num2str(i) ').*K1.*r' num2str(i) '.^2+(xcor' num2str(i) '-dPx' num2str(i) ').*K2/10^6.*r' num2str(i) '.^4+(xcor' num2str(i) '-dPx' num2str(i) ').*K3/10^12.*r' num2str(i) '.^6+P1.*(r' num2str(i) '.^2+2*(xcor' num2str(i) '-dPx' num2str(i) ').^2)+2.*P2.*(xcor' num2str(i) '-dPx' num2str(i) ').*(ycor' num2str(i) '-dPy' num2str(i) ')+C1.*(xcor' num2str(i) '-dPx' num2str(i) ')+C2.*(ycor' num2str(i) '-dPy' num2str(i) ');']);
        eval(['dy' num2str(i) '= (ycor' num2str(i) '-dPy' num2str(i) ').*K1.*r' num2str(i) '.^2+(ycor' num2str(i) '-dPy' num2str(i) ').*K2/10^6.*r' num2str(i) '.^4+(ycor' num2str(i) '-dPy' num2str(i) ').*K3/10^12.*r' num2str(i) '.^6+2.*P1.*(xcor' num2str(i) '-dPx' num2str(i) ').*(ycor' num2str(i) '-dPy' num2str(i) ')+P2.*(r' num2str(i) '.^2+2*(ycor' num2str(i) '-dPy' num2str(i) ').^2);']);
                                                                            %korekce bodu ve smeru os x a y
        eval(['xcor' num2str(i) '= x' num2str(i) '-dx' num2str(i) '-Px' num2str(i) ';']);
        eval(['ycor' num2str(i) '= y' num2str(i) '-dy' num2str(i) '-Py' num2str(i) ';']);
                                                                            %korigovane souradnice
        eval(['control' num2str(i) '= zeros(size(x' num2str(i) ',1),6);']); %prychystani matice pro provedeni rekalkulace
        eval(['control' num2str(i) '(:,2) = X' num2str(i) ';']);
        eval(['control' num2str(i) '(:,3) = Y' num2str(i) ';']);
        eval(['control' num2str(i) '(:,4) = Z' num2str(i) ';']);
        eval(['control' num2str(i) '(:,5) = xcor' num2str(i) ';']);
        eval(['control' num2str(i) '(:,6) = ycor' num2str(i) ';']);
        
        eval(['A' num2str(i) '= [0;Vx' num2str(i) ';Vy' num2str(i) ';Vz' num2str(i) ';Omega' num2str(i) ';Fi' num2str(i) ';Kappa' num2str(i) ';0;0;Cxy' num2str(i) ';Cxy' num2str(i) '];']);
                                                                            %vektor vysledku linearnich parametru kamery
        eval(['R' num2str(i) '= DET' num2str(i) '*RotMat(Omega' num2str(i) ',Fi' num2str(i) ',Kappa' num2str(i) ');']);
                                                                            %matice rotace
        eval(['[CFIN' num2str(i) ']'  '= DLT_Recalc(A' num2str(i) ',R' num2str(i) ',control' num2str(i) ');']);
                                                                            %provedeni rekalkulace
    end
    
    CFIN = [];                                                              %zobrazeni chyb DLT a resekce ve sloupcovych grafech v GUI                                           
    for i = 1:size(name)
        eval(['CFIN = vertcat(CFIN, CFIN' num2str(i) ');'])
    end
    y = horzcat(CDLT(:),CFIN);
    axes(handles.axes2);
    bar(y)
    ylabel('Chyba rekalkulace [pix]')
    legend({'Lineární transformace','Prostorová resekce'},'Location','northwest')

    
    if (j>2 && rz(j-1)-rz(j)<0.01)                                          %terminacni podminka - pokud je zmena sumy nejmensich ctvercu bezvyznamna, terminuje se cely cyklus vypoctu 
        break
    end 
end

%ULOZENI NELINEARNICH PARAMETRU KAMERY DO GLOB. PROMENNE

global CamParams                                                            
CamParams = [K1,K2,K3,P1,P2,C1,C2];

%VYTVORENI DISTRIBUCE BODU RELATIVNE VUCI KAMERE - PRO UCEL KONTROLY

for i = 1:size(name,1)                                  
    eval(['clear global BODYR' num2str(i)])
    eval(['Xr' num2str(i) '= X' num2str(i) '-Vx' num2str(i) ';']);
    eval(['Yr' num2str(i) '= Y' num2str(i) '-Vy' num2str(i) ';']);
    eval(['Zr' num2str(i) '= Z' num2str(i) '-Vz' num2str(i) ';']);
    
    eval(['a(i) = size(Xr' num2str(i) ',1);']);
    eval(['global BODYR' num2str(i)])
    for j = 1:a(i)
        eval(['BODYR' num2str(i) '(j,:) =  (-1)*transpose([Xr' num2str(i) '(j),Yr' num2str(i) '(j),Zr' num2str(i) '(j)]*R' num2str(i) ');'])
    end
end

delete(h)

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 Bigmax_Callback(hObject, eventdata, handles)

function Bigmax_CreateFcn(hObject, eventdata, handles)

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

function Bigmin_Callback(hObject, eventdata, handles)

function Bigmin_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 TritopCoordinates_Callback(hObject, eventdata, handles)

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

function LibrarySelect_Callback(hObject, eventdata, handles)

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

function ImageNumber_Callback(hObject, eventdata, handles)

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

function DisplayImage_Callback(hObject, eventdata, handles)

%Funkce zobrazi zvoleny obrazek v GUI

global name
a = str2double(get(handles.ImageNumber,'String'));
Img=imread(char(name(a,:))); 
axes(handles.axes1);
imshow(Img);

function DisplayDecode_Callback(hObject, eventdata, handles)

%Funkce zobrazi zvolenou fotografii s cisly dekodovanych kal. znacek

global name
eval (['global match' get(handles.ImageNumber,'String')])
eval (['a = imread(char(name(' get(handles.ImageNumber,'String') ',:)));'])  
figure
imshow(a);
hold on;
eval (['match1 = match' get(handles.ImageNumber,'String') ';'])
for i = 1:size(match1,1)
    text(match1(i,5)+25,match1(i,6)-30,num2str(match1(i,1)),'FontSize',16,'Color','r');
end
hold off

function Images_Callback(hObject, eventdata, handles)

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

function SpaceDistribution_Callback(hObject, eventdata, handles)

%Funkce zobrazi 3D graf s rozmistenim kalibracnich bodu na jednotlivych
%fotkach relativne vuci kamere

global name
figure
hold on
axis equal
scatter3(0,0,0)
for i = 1:size(name,1)
    eval(['global BODYR' num2str(i)])
    eval(['scatter3(BODYR' num2str(i) '(:,1),BODYR' num2str(i) '(:,2),BODYR' num2str(i) '(:,3))']);
end
hold off
