clc;
clear all;

numOfSamples = 1000;
numOfpoints = 50;

Kl = [6.71682514e+03, 0, 1.53020255e+03;  #Camera matrix of left camera
0., 6.71682514e+03, 1.07119051e+03;
0., 0., 1.];

Kr = [6.71692514e+03, 0, 1.53077489e+03;  #Camera matrix of right camera
0., 6.71692514e+03, 1.07164911e+03;
0., 0., 1.];

R = [[ 9.99114418e-01, -2.33502867e-02,  3.50020588e-02;  #Rotation matrix
 2.33568407e-02, 9.99727167e-01,  2.21693414e-04;
 -3.49976857e-02,  5.96040424e-04,  9.99387216e-01]...
,[0; 0; 0]; [0, 0, 0, 1]];

T = [eye(4, 3), [-58.82558678;  #Translation matrix
-0.45547543;
-4.32686439;
1]];

# End of user input

El = eye(3,4); 
Er = (R.'/T)(1:3, :);
Pl = Kl * El;
Pr = Kr * Er;

uvLeft = zeros(numOfpoints, 2);
uvRight = zeros(numOfpoints, 2);

# Random 3D point generator and projection of points to 2D 
for p = 1:numOfpoints

  x = randi([-200,200]);  #Bounding box for point positions
  y = randi([-200,200]);
  z = randi([100,600]);

  point3D = [x; y; z; 1];

  uvLeftS = Kl * El * point3D;
  uvRightS = Kr * Er * point3D;

  uvLeft(p, :) = [uvLeftS(1)/uvLeftS(3), uvLeftS(2)/uvLeftS(3)];
  uvRight(p, :) = [uvRightS(1)/uvRightS(3), uvRightS(2)/uvRightS(3)];

endfor

resultMat = zeros(4, numOfSamples*numOfpoints);  #Allocation for result matrix

paramsStr={"fl", "cxl", "cyl", "fr", "cxr", "cyr", "Tx", "Ty", "Tz"};
params = [Kl(1,1), Kl(1,3), Kl(2,3), Kr(1,1), Kr(1,3), Kr(2,3), T(1, 4), T(2, 4), T(3, 4)];

c = cell (5,9);
c (1,:)= paramsStr;


for k = 1:9

  for i = 1:numOfpoints
    
    ptsLeft = uvLeft(i,:)';
    ptsRight = uvRight(i,:)';
    
    for j = 1:numOfSamples

      params(k) = params(k) + randn(1); 
          
      Klt = [params(1), 0, params(2);  #Camera matrix of left camera
      0., params(1), params(3);
      0., 0., 1.];

      Krt = [params(4), 0, params(5);  #Camera matrix of right camera
      0., params(4), params(6);
      0., 0., 1.];

      Tt = [eye(4, 3), [params(7);  #Translation matrix
      params(8);
      params(9);
      1]];
      
      Er = (R.'/Tt)(1:3, :);
      Pl = Klt * El;
      Pr = Krt * Er;
      
      [pts3D] = Reconstruct(ptsLeft, ptsRight, Pr, Pl);
      resultMatSample(:, j) = pts3D;
           
      params=[Kl(1,1), Kl(1,3), Kl(2,3), Kr(1,1), Kr(1,3), Kr(2,3), T(1, 4), T(2, 4), T(2, 4), T(3, 4)];            
      
    endfor
    resultMatSample = resultMatSample-mean(resultMatSample, 2);
    resultMat(:, (i-1)*numOfSamples+1:i*numOfSamples)=resultMatSample;
  endfor
   [U, S, V] = svd(resultMat.', "econ"); 
    normedS = sqrt(diag(S).^2/(size(resultMat,2)-1));
    citlivost = S(1, 1);
    c {2,k}= S;
    c {3,k}= V;
    c {4,k}= citlivost;
    c {5,k}= 1/citlivost;
endfor 

y = cell2mat(c([4, 9, 14, 19, 24, 29, 34, 39, 44]));
bar(y);
title ("Citlivost na kalibracni parmetry");