
%% Blind Lens Correction 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 the standard or FOV lens distortion model (see plumb_line.h)
%
% An alternate-minimization scheme is employed in order to solve for the
% correct (x,y) coordinates as well as the model parameters k.

%% Input Arguments

% pts - Normalized coordinate matrix containing ordered list of all line points
%       grouped hiearchically by line and then group (image)
%       Nx2 matrix.

% lineSizeMat - Size of sets matrix. Columns correspond to group (image) index,
%               and rows correspond to line index. To deduce number of points 
%               within line segment with group and line index i and j, 
%               respectively then N = setSize(i,j)
%               GxL matrix. 

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

% maxIter - Optional maximum number of iterations (alternate minimization scheme)
%           Positive integer

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

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


%% Output Arguments

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

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

% error - Error curve (residual l2-norm) of iterations
%         maxIter x 1 vector.


%% Blind Lens Correction C-implementation

function [corrPts, coefs, error] = blind_lens_calibration(pts, lineSizeMat, varargin)

%general constants
TOL      = 1e-6;

%add lsd tools
matlabDir = userpath;
addpath(strcat(matlabDir(1:end-1),'/MASC/Image Processing Tools/Edge Detection/'));
addpath(strcat(matlabDir(1:end-1),'/MASC/Camera Calibration Tools/Nonmetric Calibration/Calibration Harp/'));

%parse input parameters
[disPts, lineSizeMat, coefs, ~, maxIter, ~, outFile] = input_parser(pts, lineSizeMat, varargin{:});

save(outFile, 'disPts', 'lineSizeMat');


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

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


end% end of blind lens correction


%% Input Parser
function [pts, lineSizeMat, coefs, nGroups, maxIter, nMaxLines, outFile] = input_parser(pts, lineSizeMat, varargin)

% parse input arguments
p = inputParser;
p.addRequired('pts', @(x) ...
              validateattributes(x,{'numeric'},{'real','nonnan','2d','finite'}));
p.addRequired('lineSizeMat', @(x) ...
              validateattributes(x,{'numeric'},{'real','nonnan','2d','finite'}));  
p.addParameter('coefs', [], @(x) ...          
               validateattributes(x,{'numeric'},{'real','nonnan','vector','finite'})); 
p.addParameter('maxIter', 1500, @(x) ...          
               validateattributes(x,{'numeric'},{'real','nonnan','positive','finite','scalar'})); 
p.addParameter('outDir', '.', @(x) ischar(x));	
p.addParameter('outName', 'blind_lens_calibration_c.mat', @(x) ischar(x));

p.parse(pts, lineSizeMat, varargin{:});
inputs       = p.Results;
pts          = inputs.pts;
lineSizeMat  = inputs.lineSizeMat;
coefs        = inputs.coefs;
maxIter      = inputs.maxIter;
outDir       = inputs.outDir;
outName      = inputs.outName;

coefs = coefs(:);

[nGroups, nMaxLines] = size(lineSizeMat);

outFile = fullfile(outDir, outName);

end %end of input parser

























