%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright 2016 MBARI.                                                     %
% MBARI Proprietary Information. All rights reserved.                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 20180702  mar  
% Matlab script to camdata matrices and vectors to csv files
%
if 0
    filename=input('Input the directory containing the camera calibration data  ','s')
    if exist(['./',filename,'/CalData.mat'])>0  % I save StereParams in CalData.mat.  Else, I assume StereoParams is in the workspace
        load(['./',filename,'/CalData.mat'])
    end
end
%load Cal40.mat
    
    KLeft=stereoParams.CameraParameters1.IntrinsicMatrix; %Intrinsic paramters
    KRight=stereoParams.CameraParameters2.IntrinsicMatrix;
    k1=inv(KLeft');  %k1=[1/fRight(1) 0 -centerRight(1)/fRight(1); 0 1/fRight(2)  -centerRight(2)/fRight(2); 0 0 1];
    k2=inv(KRight');  %k2=[1/fLeft(1) 0 -centerLeft(1)/fLeft(1); 0 1/fLeft(2)  -centerLeft(2)/fLeft(2); 0 0 1];
    R_c1_c2=stereoParams.RotationOfCamera2;
    T=stereoParams.TranslationOfCamera2';    %in mm '
    B=-R_c1_c2*T;  % location of camera2 wrt camera 1 expressed in camera 1 coordinates.%  
    Fmatrix=stereoParams.FundamentalMatrix;
    distortLeft=stereoParams.CameraParameters1.RadialDistortion;  % These are required to undistort the images
    distortRight=stereoParams.CameraParameters2.RadialDistortion;  % They aren't used in TestWF


%save the variables to CSV files with 15 places of precision
dlmwrite('B.csv', B, 'precision', 15)
dlmwrite('Fmatrix.csv', Fmatrix, 'precision', 15)
%dlmwrite('KLeft.csv', KLeft, 'precision', 15)
%dlmwrite('KRight.csv', KRight, 'precision', 15)
dlmwrite('R_c1_c2.csv', R_c1_c2, 'precision', 15)
dlmwrite('R12.csv', R_c1_c2, 'precision', 15)
%dlmwrite('T.csv', T, 'precision', 15)
dlmwrite('k1.csv', k1, 'precision', 15)
dlmwrite('k2.csv', k2, 'precision', 15)
dlmwrite('distortLeft.csv', distortLeft, 'precision', 15)
dlmwrite('distortRight.csv',distortRight, 'precision', 15)
