
%% Standard Lens Calibration C-implementaion
%
% Solves for the correct (un-distorted/original) image coordinates which 
% gave rise to the input distorted coordinates. The input image coordinates
% are assumed to originate from imaged straight lines using a camera
% following a distortion model of the form
%
%   x_u = H(x_d, y_d, k)
%   y_u = H(x_d, y_d, k)
%
% The levenberg-marqaurdt optimization routine is applied to the model
% parameters to minimize cost functions which penalize the
% non-'straightness' of the distorted image coordinate lines.

%% Input Arguments

% seqName - Image sequence name.
%           String.

% coefs - Optional initial estimated of lens distortion model parameters.
%         Qx1 vector. (default: NaN)

% dirPath - Optional parent directory path (relative userpath) to image sequence.
%           String (default: '.')

% maxIter - Optional maximum number of iterations.
%           Positive integer. (default:1500)

% outDir - Optional output directory for save-files
%          String

% outName - Optional output file name for save-files
%           String

% noiseVar - Optional additive uncorrelated 2D gaussian noise variance to
%            inject into extracted distorted points. ex. (Var-x, Var-y)
%            Scalar (>=0)


%% Output Arguments

% coefs - Lens distortion model parameters. 
%         Qx1 vector. 

% corrPts - Corrected (original) coordinate matrix containing ordered list 
%           of all line points grouped hiearchically by line and then group
%           Nx2 matrix.

% error - L2-nom of residual, error,  of calibration
%         Positive scalar.

%% Standard Lens Calibration C-implementation

function [coefs, corrPts, error] = standard_lens_calibration(seqName, varargin)

%general constants
TOL      = 1e-6;

%add lsd tools
matlabDir = userpath;
addpath(strcat(matlabDir(1:end-1),'/MASC/Camera Calibration Tools/Nonmetric Calibration/Calibration Harp/standard plumb-line method/test_environment'));

%parse input parameters
[seqName, dirPath, coefs, maxIter, outFile, noiseVar] = input_parser(seqName, varargin{:});

save(outFile, 'seqName');

%--------------------------------------------------
%		 <step 1>
%--------------------------------------------------
%description: extract from image sequence normalized line points

[disPts, lineSizeMat] = extract_line_points(seqName, 'dirPath', dirPath ... 
                                                   , 'minLinePts', 20    ...%0
                                                   , 'ptsPerLine', 75   ...%75
                                                   , 'decFactor', 3     ...%3
                                                   , 'angleTol', 30     ...%20
                                                   , 'densityTol', 0.75  ...%0.5
                                                   , 'isNorm', true     );
                                               
%optional injection of gaussian noise into extracted distorted points                                               
if sum(noiseVar) > 0               
    nPts = size(disPts, 1);
    disPts = disPts + mvnrnd(zeros(nPts, 2), diag(noiseVar)); 
end


save(outFile, 'disPts', 'lineSizeMat', '-append');                                               
                                               
%--------------------------------------------------
%		 <step 2>
%--------------------------------------------------
%description: initialize the model parameters (assume unity transform)
    
tic;
[coefs, corrPts, error] = standard_lens_calibration_c(disPts, lineSizeMat, coefs, TOL, maxIter);
runTime = toc; %#ok<NASGU>

save(outFile, 'coefs', 'corrPts', 'error', 'runTime', '-append');


end% end of standard lens correction


%% Input Parser
function [seqName, dirPath, coefs, maxIter, outFile, noiseVar] = input_parser(seqName, varargin)

% parse input arguments
p = inputParser;
p.addRequired('seqName', @(x) ischar(x));	
p.addParameter('dirPath', '', @(x) ischar(x)); 
p.addParameter('coefs', [], @(x) ...   
               validateattributes(x,{'numeric'},{'real','nonnan','vector','finite'}));  
p.addParameter('maxIter', 1500, @(x) ...       
               validateattributes(x,{'numeric'},{'real','nonnan','scalar', 'positive','finite'}));  
p.addParameter('outDir', '.', @(x) ischar(x));	
p.addParameter('outName','standard_lens_calibration_c.mat', @(x) ischar(x));	
p.addParameter('noiseVar', [0; 0], @(x) ...   
               validateattributes(x,{'numeric'},{'real','nonnan','vector','finite', 'numel', 2}));

p.parse(seqName, varargin{:});
inputs           = p.Results;
seqName          = inputs.seqName;
dirPath          = inputs.dirPath;
coefs            = inputs.coefs;
maxIter          = inputs.maxIter;
outDir           = inputs.outDir;
outName          = inputs.outName;
noiseVar         = inputs.noiseVar;

coefs    = coefs(:);
noiseVar = noiseVar(:);

outFile = fullfile(outDir, outName);

end %end of input parser

























