
%% Plumb Line Cost Function
%
% Computes regression line residual error norm for plumb-line methods
% (blind/standard).
%

%% Input Parameters

% 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 - Lens distortion model parameters.
%         Qx1 vector. (default: NaN)

% weights - Optional Line points weights.
%           Nx1 vector

% maxIter - Optional maximum iteration count (for fixed point iteration of
%           blind method)
%           positive scalar

% methodType - Optional method type.
%              String {'blind', default:'standard'}    

% modelType - Optional lens distortion model type.
%             String {'std', 'rad', 'fov'}


%% Output Parameters

% err - L2-norm of regression line residuals (perpendicular offsets).
%         Scalar (>0)

% res - Regression line residuals (perpendicular offsets for each line
%       point).
%       Nx1 vector


%% Plumb Line Cost Function

function [err, res] = plumb_line_cost_function(pts, lineSizeMat, coefs, varargin)

%parse input parameters
[pts, lineSizeMat, coefs, weights, maxIter, methodType, modelType] = input_parser(pts, lineSizeMat, coefs, varargin{:});

%compute residual error
switch(modelType)
    case 'std'
        [res, alphas, betas, gammas, corrPts] = std_plumb_line_cost_function_c(pts, lineSizeMat, weights, coefs, maxIter, methodType); %#ok<ASGLU>
    case 'rad'
        [res, alphas, betas, gammas, corrPts] = rad_plumb_line_cost_function_c(pts, lineSizeMat, weights, coefs, maxIter, methodType); %#ok<ASGLU>
    case 'fov'
        [res, alphas, betas, gammas, corrPts] = fov_plumb_line_cost_function_c(pts, lineSizeMat, weights, coefs, maxIter, methodType); %#ok<ASGLU>
    otherwise
        error('plumbLineCostFunction:InvalidModel', 'Unknown model type specified');
end
        
%compute l2 norm of residuals
err = norm(res, 2);

end% end of cost function


%% Input Parser
function [pts, lineSizeMat, coefs, weights, maxIter, methodType, modelType] = input_parser(pts, lineSizeMat, coefs, 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.addRequired('coefs', @(x) ...          
               validateattributes(x,{'numeric'},{'real','nonnan','vector','finite'})); 
p.addParameter('weights', [], @(x) ...          
               validateattributes(x,{'numeric'},{'real','nonnan','vector','finite'}));
p.addParameter('maxIter', 2000, @(x) ...          
               validateattributes(x,{'numeric'},{'real','nonnan','positive','integer','finite'}));           
p.addParameter('methodType', 'standard', @(x) ...          
               ischar(validatestring(x,{'alvarez','blind','standard','std'})));  
p.addParameter('modelType', 'std', @(x) ...          
               ischar(validatestring(x,{'std','rad','fov'})));
           
p.parse(pts, lineSizeMat, coefs, varargin{:});
inputs       = p.Results;
pts          = inputs.pts;
lineSizeMat  = inputs.lineSizeMat;
coefs        = inputs.coefs;
weights      = inputs.weights;
maxIter      = inputs.maxIter;
methodType   = inputs.methodType;
modelType    = inputs.modelType;

coefs = coefs(:);

switch lower(methodType)   
    case {'alvarez'}
        methodType = 0;
    case {'blind'}
        methodType = 1;
    case{'standard','std'}
        methodType = 0;
    otherwise
        error('PlumbLineCostFunction:MethodType', 'Invalid method type');
end

if isempty(weights)
    weights = ones(size(pts,1), 1);
else
    weights = weights(:);
end

end %end of input parser



