clc; clear; close all; %% Diplomová práce Zdeněk Schoř 
load('finalni_snimek_vektory_XYZuv.mat')
%% Vyber vhodnych kalibracnich bodu
% vybrane_body=[1 2 3 4 5 6 7 10 13 14 15 16 17 18 19 21 26 27 28 29  ...
%     30 31 33 34 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52];%40bodu
vybrane_body=[15 18 42 48];% 4 finalni body
X=X(vybrane_body);
Y=Y(vybrane_body);
Z=Z(vybrane_body);
u=u(vybrane_body);
v=v(vybrane_body);

%% Priprava optimalizacni funkce lsqnonlin
x0 = [10,10,10,10,10,10,10,10];%nahodny nastrel
% Kriterialni funkce
funkce=@(vst)(((vst(1)*X+vst(2)*Y+vst(3)*Z+vst(7)...
-u).^2)+((vst(4)*X+vst(5)*Y+vst(6)*Z +vst(8)-v).^2));

opts = optimoptions('lsqnonlin', 'MaxFunctionEvaluations', 1.00e+14,...
'MaxIterations',1.00e+05,'FunctionTolerance',1.00e-24,'StepTolerance',...
1.00e-30, 'FiniteDifferenceType','central','Algorithm','levenberg-marquardt');
%% Hlavni vypocet
output = lsqnonlin(funkce,x0,[],[],opts);
% usporadani kalibracnich matic
Q=[output(7);output(8)];
T=[output(1),output(2),output(3);
   output(4),output(5),output(6)]; 

%% Zkouska vysledku vsech bodu
aktualni_vysledek_uv=(T*[X,Y,Z]')'+Q';%nový vysledek
Porovnani=zeros(length(Z),5);
Porovnani(:,1)=aktualni_vysledek_uv(:,1); % vypocitane u
Porovnani(:,2)=u; % spravne u
Porovnani(:,3)=aktualni_vysledek_uv(:,2); % vypocitane v
Porovnani(:,4)=v; % spravne v
Porovnani(:,5)= 1:1:length(Z); %cislovani radku pro orientaci

%% Zobrazeni bodu v obrazu
imshow(m);hold on
plot(Porovnani(:,1),Porovnani(:,3),'rO','MarkerSize',7,'LineWidth',3) %Nove vypocitane uv
plot(Porovnani(:,2),Porovnani(:,4),'go','MarkerSize',7,'LineWidth',3) %Spravne uv

% P=pinv(T)*Q  % prepocet na matice Q na matici P, pro overeni
save('QaT4bodyzkouska.mat','Q','T')

