function [sumpnp, scale] = StereoVisionCal(CameraParams, R)

%Funkce ma za ucel kalibrovat parametry stereoparu - jsou znamy vnitrni 
%parametry kamery, vychazi se z korigovanych souradnic kal. znacek ve snimcich,
%vystupem jsou modelove souradnice bodu viditelnych na obou snimcich a 
%parametry stereoparu v modelovem s. s. 

%PRIPRAVA HODNOT PRO VYPOCET

global MatchCell

Stereomatch = [];                                                           %vytovreni matice obsahujici korigovane souradnice kalibracnich znacek v obou snimcich a v prostoru - vazebnych bodu
for i = 1:size(MatchCell{1},1)
    for j = 1:size(MatchCell{2},1)
        if MatchCell{1}(i,1)==MatchCell{2}(j,1)
            Stereomatch(size(Stereomatch,1)+1,:) = horzcat(MatchCell{1}(i,1),MatchCell{1}(i,2:6),MatchCell{2}(j,5:6));
        end
    end
end

global x1                                                                   %prichystani zminenych promennych pro vypocet
x1 = Stereomatch(:,5);
global y1
y1 = Stereomatch(:,6);
global X
X = Stereomatch(:,2);
global Y
Y = Stereomatch(:,3);
global Z
Z = Stereomatch(:,4);
global x2
x2 = Stereomatch(:,7);
global y2
y2 = Stereomatch(:,8);
global Cxy1                                                                 %ohniskove vzdalenosti
Cxy1 = CameraParams{1}(10,1);   
global Cxy2
Cxy2 = CameraParams{2}(10,1);

EUL = Mat2Eul(transpose(-R{1})*(-R{2}));                                    %rotace mezi s. s. kamer - pocatecni hodnoty

global Vx1                                                                  %polohy stedu zobrazeni kamer v prostoru
global Vy1
global Vz1
global Vx2
global Vy2
global Vz2

Vx1 = CameraParams{1}(2,1);
Vy1 = CameraParams{1}(3,1);
Vz1 = CameraParams{1}(4,1);
Vx2 = CameraParams{2}(2,1);
Vy2 = CameraParams{2}(3,1);
Vz2 = CameraParams{2}(4,1);

V = transpose(-R{1})*([Vx2;Vy2;Vz2] - [Vx1;Vy1;Vz1]);                       %baze v souradnem systemu prvni kamery

scale = V(2);                                                               %meritko - tak aby V(2) bylo 1

VFIN = V./scale;                                                            %baze v meritku

b0 = [VFIN(1),VFIN(3),EUL];                                                 %vektor pocatecnich hodnot

%VYPOCET

options = optimoptions(@lsqnonlin,'MaxIter',15,'TolFun',10^(-100),'TolX',10^(-100),'Display','off');
                                                                            %nastaveni resice - metody nelin. nejm. ctvercu
[bf,rz] = lsqnonlin(@Stereo,b0, [],[],options);                             %aplikace metody nelin. nejm. ctvercu

%ZAPIS VYSLEDKU

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

Vbxm = bf(1);
Vbym = 1;
Vbzm = bf(2);
OmegaR = bf(3);
FiR = bf(4);
KapaR = bf(5);

global xm1                                                                  %rekalkulace jednotlivych bodu z kamery 1 a kamery 2 v modelovem souradnem systemu                                                             
global xm2
global ym1
global ym2
global zm1
global zm2
global pxp                                                                  %rozdily rekalkulovanych souradnic z jednotlivych kamer v jednotlivych osach
global pyp
global pzp

sumpnp = 1/size(x1,1)*sum(sqrt(pxp.^2+pyp.^2+pzp.^2));                      %prumerna hodnota rozdilu rekalkulovanych souradnic z jednotlivych kamer

global xm                                                                   %modelove souradnice vazebnych bodu
xm = (xm1(:)+xm2(:))./2;
global ym
ym = (ym1(:)+ym2(:))./2;
global zm
zm = (zm1(:)+zm2(:))./2;

end

