function [f, varargout] = estimateFundamentalMatrix(...
  matched_points1, matched_points2, varargin)
% Estimate fundamental matrix from corresponding points in stereo image.

% Copyright 2009 The MathWorks, Inc.
% $Revision: 1.1.6.4 $  $Date: 2011/02/21 07:18:28 $
%#codegen

coder.extrinsic('cvstGetCoordsChoice');
isXY = eml_const(cvstGetCoordsChoice('fcnEstimateFundamentalMatrix'));

% Code below is only used by Simulink, when the function is
% placed in the MATLAB Function block. Simulink first propagates
% the sizes followed by the types. The block of code below sets
% the sizes during the Simulink size propagation stage.
if eml_ambiguous_types
  f = zeros(3);
  if nargout > 1
    if isXY
      varargout{1} = false(size(matched_points1, 1), 1);
    else
      varargout{1} = false(1, size(matched_points1, 2));
    end
    if nargout == 3
      varargout{2} = int32(0);
    end
  end
  return;
end

% Declare that the inputs are most likely constant in order to generate 
% more optimized code.
eml_prefer_const(varargin);

% List of status code
statusCode = struct('NoError',           int32(0),...
                    'PtsWrongSize',      int32(1),...
                    'NumPtsMismatch',    int32(2),...
                    'NotEnoughPts',      int32(3),...
                    'NotEnoughInliers',  int32(4));

% Parse and check inputs
[method, outputClass, numTrials, distanceType, distanceThreshold, ...
  confidence, inlierPercentage, reportRuntimeError] ...
  = parseInputs(matched_points1, matched_points2, varargin{:});
  
% Compute the fundamental matrix
[f, inliers, status] = cvalgEstimateFundamentalMatrix(...
  matched_points1, matched_points2, ...
  method, outputClass, numTrials, distanceType, distanceThreshold, ...
  confidence, inlierPercentage, statusCode, isXY);
  
if nargout >= 2
  varargout{1} = inliers;
  if nargout == 3
    varargout{2} = status;
  end
end

if cast(reportRuntimeError, 'logical')
  % Report runtime error
  checkRuntimeStatus(statusCode, status, isXY);
end
end

%========================================================================== 
% Check runtime status and report error if there is one
%========================================================================== 
function checkRuntimeStatus(statusCode, status, isXY)
if status == statusCode.NoError
  % No runtime failure
elseif status == statusCode.PtsWrongSize
  if isXY
    assert(false, 'vision:estimateFundamentalMatrix:ptsWrongSize', ...
      'Expected MATCHED_POINTS1 and MATCHED_POINTS2 to be M-by-2 matrices.');
  else
    assert(false, 'vision:estimateFundamentalMatrix:ptsWrongSize', ...
      'Expected MATCHED_POINTS1 and MATCHED_POINTS2 to be 2-by-N matrices.');
  end
elseif status == statusCode.NumPtsMismatch
  assert(false, 'vision:estimateFundamentalMatrix:numPtsMismatch', ...
    ['Expected MATCHED_POINTS1 and MATCHED_POINTS2 to have the same '...
    'number of points.']);
elseif status == statusCode.NotEnoughPts
  assert(false, 'vision:estimateFundamentalMatrix:notEnoughPts', ...
    'MATCHED_POINTS1 and MATCHED_POINTS2 do not have enough points.');
else % status == statusCode.NotEnoughInliers
  assert(false, 'vision:estimateFundamentalMatrix:notEnoughInliers', ...
    'Could not find enough inliers in MATCHED_POINTS1 and MATCHED_POINTS2.');
end
end

%==========================================================================
% Parse and check inputs
%========================================================================== 
function [method, outputClass, numTrials, distanceType, ...
  distanceThreshold, confidence, inlierPercentage, reportRuntimeError] ...
  = parseInputs(matched_points1, matched_points2, varargin)

% Value of string enums
methodOpt1 = 'LMedS';
methodOpt2 = 'LTS';
methodOpt3 = 'RANSAC';
methodOpt4 = 'MSAC';
methodOpt5 = 'Norm8Point';

classOpt1 = 'double';
classOpt2 = 'single';

distanceOpt1 = 'Sampson';
distanceOpt2 = 'Algebraic';
  
%--------------------------------------------------------------------------
% Parse optional inputs
%--------------------------------------------------------------------------
parms = struct( ...
  'Method',             uint32(0), ...
  'OutputClass',        uint32(0), ...
  'NumTrials',          uint32(0), ...
  'DistanceType',       uint32(0), ...
  'DistanceThreshold',  uint32(0), ...
  'Confidence',         uint32(0), ...
  'InlierPercentage',   uint32(0), ...
  'ReportRuntimeError', uint32(0));

popt = struct( ...
  'CaseSensitivity', true, ...
  'StructExpand',    true, ...
  'PartialMatching', false);

optarg             = eml_parse_parameter_inputs(parms, popt, varargin{:});

method             = eml_tolower(eml_get_parameter_value(optarg.Method, ...
                       methodOpt1, varargin{:}));
outputClass        = eml_tolower(eml_get_parameter_value( ...
                       optarg.OutputClass, classOpt1, varargin{:}));
distanceType       = eml_tolower(eml_get_parameter_value( ...
                       optarg.DistanceType, distanceOpt1, varargin{:}));
numTrials          = eml_get_parameter_value(optarg.NumTrials, ...
                       500, varargin{:});
distanceThreshold  = eml_get_parameter_value(optarg.DistanceThreshold, ...
                       0.01, varargin{:});
confidence         = eml_get_parameter_value(optarg.Confidence, 99,...
                       varargin{:});
inlierPercentage   = eml_get_parameter_value(optarg.InlierPercentage,...
                       50, varargin{:});
reportRuntimeError = eml_get_parameter_value(optarg.ReportRuntimeError, ...
                       true, varargin{:});

%--------------------------------------------------------------------------
% Check inputs
%--------------------------------------------------------------------------
eml_lib_assert(strcmp(method, eml_tolower(methodOpt1)) ...
  || strcmp(method, eml_tolower(methodOpt2)) ...
  || strcmp(method, eml_tolower(methodOpt3)) ...
  || strcmp(method, eml_tolower(methodOpt4)) ...
  || strcmp(method, eml_tolower(methodOpt5)), ...
  'vision:estimateFundamentalMatrix:invalidMethod', ...
  ['Expected Method to be ''', methodOpt1, ''', ''', ...
  methodOpt2, ''', ''', methodOpt3, ''', ''', ...
  methodOpt4, ''' or ''', methodOpt5, '''.']);

eml_lib_assert(strcmp(outputClass, eml_tolower(classOpt1)) ...
  || strcmp(outputClass, eml_tolower(classOpt2)), ...
  'vision:estimateFundamentalMatrix:invalidOutputClass', ...
  ['Expected OutputClass to be ''', ...
  classOpt1, ''' or ''', classOpt2, '''.']);

eml_lib_assert(strcmp(distanceType, eml_tolower(distanceOpt1)) ...
  || strcmp(distanceType, eml_tolower(distanceOpt2)), ...
  'vision:estimateFundamentalMatrix:invalidDistanceType', ...
  ['Expected DistanceType to be ''', ...
  distanceOpt1, ''' or ''', distanceOpt2, '''.']);

eml_lib_assert(isnumeric(matched_points1) && isreal(matched_points1) ...
  && ~issparse(matched_points1), ...
  'vision:estimateFundamentalMatrix:invalidMatchedPoints1', ...
  'Data type of MATCHED_POINTS1 must be numeric, real, and nonsparse.');

eml_lib_assert(isnumeric(matched_points2) && isreal(matched_points2) ...
  && ~issparse(matched_points2), ...
  'vision:estimateFundamentalMatrix:invalidMatchedPoints2', ...
  'Data type of MATCHED_POINTS2 must be numeric, real, and nonsparse.');

eml_lib_assert(eml_is_positive_integer_scalar(numTrials),...
  'vision:estimateFundamentalMatrix:invalidNumTrials', ...
  'Expected NumTrials to be a positive integer.');

eml_lib_assert(isscalar(distanceThreshold) ...
  && isnumeric(distanceThreshold) && isreal(distanceThreshold)...
  && distanceThreshold>0 && isfinite(distanceThreshold) ...
  && ~issparse(distanceThreshold), ...
  'vision:estimateFundamentalMatrix:invalidDistanceThreshold', ...
  'Expected DistanceThreshold to be a positive value.');

eml_lib_assert(isscalar(confidence) && isnumeric(confidence) ...
  &&isreal(confidence) && confidence>0 && confidence<100 ...
  && isfinite(confidence) && ~issparse(confidence), ...
  'vision:estimateFundamentalMatrix:invalidConfidence', ...
  'Expected Confidence to be a positive value and less than 100.');

eml_lib_assert(isscalar(inlierPercentage) && isnumeric(inlierPercentage)...
  && isreal(inlierPercentage) && inlierPercentage>0 ...
  && isfinite(inlierPercentage) && inlierPercentage<100 ...
  && ~issparse(inlierPercentage), ...
  'vision:estimateFundamentalMatrix:invalidInlierPercentage', ...
  'Expected InlierPercentage to be a positive value and less than 100.');

eml_lib_assert(isscalar(reportRuntimeError) ...
  && isreal(reportRuntimeError) && ~issparse(reportRuntimeError) ...
  && (isnumeric(reportRuntimeError) || islogical(reportRuntimeError)), ...
  'vision:estimateFundamentalMatrix:invalidReportRuntimeError', ...
  'Expected ReportRuntimeError to be true or false.');
end
