function [f, varargout] = estimateFundamentalMatrix(...
  matched_points1, matched_points2, varargin)
%estimateFundamentalMatrix Estimate fundamental matrix.
%   estimateFundamentalMatrix estimates the fundamental matrix from
%   corresponding points in stereo images. It can be configured to use all
%   corresponding points to compute the fundamental matrix, or to exclude
%   outliers by using a robust estimation technique such as RANSAC.
%
%   F = estimateFundamentalMatrix(MATCHED_POINTS1,MATCHED_POINTS2) returns
%   the 3-by-3 fundamental matrix, F, using the Least Median of Squares
%   (LMedS) method. Format of corresponding points MATCHED_POINTS1 and
%   MATCHED_POINTS2 is [x1,y1; x2,y2; ...; xM,yM], where x and y are the
%   column and row coordinates, respectively, and M is the number of
%   points. MATCHED_POINTS1 and MATCHED_POINTS2 must contain points which
%   are putatively matched by using a function such as matchFeatures.
%  
%   [F,INLIERS_INDEX] =
%   estimateFundamentalMatrix(MATCHED_POINTS1,MATCHED_POINTS2) returns
%   logical indices, INLIERS_INDEX, for the inliers used to compute the
%   fundamental matrix. The INLIERS_INDEX output is a M-by-1 vector. Each
%   element of INLIERS_INDEX is true if the corresponding point was used to
%   compute the fundamental matrix, and false otherwise.
%  
%   [F,INLIERS_INDEX,STATUS] =
%   estimateFundamentalMatrix(MATCHED_POINTS1,MATCHED_POINTS2) returns a
%   status code with the following possible values:
% 
%     0: No error. 
%     1: Either MATCHED_POINTS1 or MATCHED_POINTS2 or both are not M-by-2
%        matrices. 
%     2: MATCHED_POINTS1 and MATCHED_POINTS2 do not have the same number of
%        points. 
%     3: MATCHED_POINTS1 and MATCHED_POINTS2 do not contain enough points.
%        Norm8Point, RANSAC, and MSAC require at least 8 points, LMedS 16
%        points, and LTS ceil(800/InlierPercentage).
%     4: Not enough inliers found.
%  
%   [F,INLIERS_INDEX,STATUS] = estimateFundamentalMatrix(
%      MATCHED_POINTS1,MATCHED_POINTS2,PARAM1,VAL1,PARAM2,VAL2,...)
%   sets parameters for finding outliers and computing the fundamental
%   matrix. Name and value of the parameters are:
%  
%   'Method'             One of the strings: 'Norm8Point', 'LMedS',
%                        'RANSAC', 'MSAC', or 'LTS'.
%                        Specify the method used to compute the fundamental
%                        matrix. To produce reliable results using
%                        'Norm8Point' algorithm, MATCHED_POINTS1 and
%                        MATCHED_POINTS2 must match precisely. The other
%                        methods can tolerate outliers and therefore only
%                        require putatively matched input points. You can
%                        obtain putatively matched points by using
%                        matchFeatures function.
% 
%                          'Norm8Point': Normalized Eight-Point algorithm
%                          'LMedS':      Least Median of Squares
%                          'RANSAC':     RANdom SAmple Consensus
%                          'MSAC':       M-estimator SAmple Consensus
%                          'LTS':        Least Trimmed Squares
%  
%                        Default: 'LMedS'
%  
%   'OutputClass'        One of the strings: 'double' or 'single'.
%                        Specifies the class for the fundamental matrix and
%                        for the internal computation.
% 
%                        Default: 'double'
%  
%   'NumTrials'          Positive integer scalar.
%                        Specifies the number of random trials for finding
%                        the outliers. This parameter applies when you set
%                        the Method parameter to 'LMedS', 'RANSAC', 'MSAC',
%                        or 'LTS'. When you set the Method parameter to
%                        either 'LMedS' or 'LTS', the function sets this
%                        parameter to the actual number of trials. When you
%                        set the Method parameter to either 'RANSAC' or
%                        'MSAC', the function sets this parameter to the
%                        maximum number of trials and the actual number of
%                        trials depends on MATCHED_POINTS1,
%                        MATCHED_POINTS2, and the value of the Confidence
%                        parameter.
% 
%                        Default: 500
%  
%   'DistanceType'       One of the strings: 'Algebraic' or 'Sampson'.
%                        Specifies the distance type to determine whether a
%                        pair of points is an inlier or outlier. This
%                        parameter applies when you set the Method
%                        parameter to 'LMedS', 'RANSAC', 'MSAC', or 'LTS'.
% 
%                        Default: Sampson
% 
%   'DistanceThreshold'  Positive scalar value.
%                        Specifies the distance threshold for finding
%                        outliers. This parameter applies when you set the
%                        Method parameter to 'RANSAC' or 'MSAC'.
% 
%                        Default: 0.01
% 
%   'Confidence'         Scalar value greater than 0 and less than 100.
%                        Specifies the desired confidence (in percentage)
%                        for finding the maximum number of inliers. This
%                        parameter applies when you set the Method
%                        parameter to 'RANSAC' or 'MSAC'.
%  
%                        Default: 99
%  
%   'InlierPercentage'   Scalar value greater than 0 and less than 100.
%                        Specifies the minimum percentage of inliers in
%                        MATCHED_POINTS1 and MATCHED_POINTS2. This
%                        parameter applies when you set the Method
%                        parameter to 'LTS'.
% 
%                        Default: 50
%    
%   'ReportRuntimeError' true or false
%                        Set this parameter to true to report runtime
%                        errors when the function cannot compute the
%                        fundamental matrix from MATCHED_POINTS1 and
%                        MATCHED_POINTS2. When you set this parameter to
%                        false, you can check the STATUS output to verify
%                        validity of the fundamental matrix.
%
%                        Default: true
%
%   Class Support
%   -------------
%   MATCHED_POINTS1 and MATCHED_POINTS2 must be numeric, real,
%   two-dimensional, and nonsparse.
%
%   Example 1
%   ---------
%     % Use the Random Sample Consensus method to compute the fundamental
%     % matrix. This method requires that the input points are already 
%     % putatively matched, for example, by using matchFeatures function.
%     % Outliers which may still be contained within putatively matched
%     % points are further eliminated by using the RANSAC algorithm.     
%     load stereoPointPairs
%     fRANSAC=estimateFundamentalMatrix(matched_points1,matched_points2,...
%      'Method', 'RANSAC', 'NumTrials', 2000, 'DistanceThreshold', 1e-4)
%  
%   Example 2
%   ---------
%     % Use the Least Median of Squares method to find inliers and to
%     % compute the fundamental matrix. We begin by loading putatively 
%     % matched points matched_points1, and matched_points2.
%     load stereoPointPairs
%     [fLMedS, inliers] = estimateFundamentalMatrix(...
%       matched_points1, matched_points2, 'NumTrials', 2000);
% 
%     % Load the stereo images.
%     I1 = imread('viprectification_deskLeft.png');
%     I2 = imread('viprectification_deskRight.png');
% 
%     % Show the putatively matched points.
%     cvexShowImagePair(I1, I2, 'Matched Points in Left Image',...
%       'Matched Points in Right Image', 'MultipleColors',...
%       matched_points1, matched_points2);
% 
%     % Show the inlier points.
%     cvexShowImagePair(I1, I2, 'Inlier Points in Left Image',...
%       'Inlier Points in Right Image', 'MultipleColors',...
%       matched_points1(inliers,:), matched_points2(inliers,:));
%     
%   Example 3
%   ---------
%     % Use the Normalized Eight-Point algorithm to compute the fundamental
%     % matrix. This algorithm is only suitable for input points which do
%     % not contain any outliers.    
%     load stereoPointPairs
%     inlierPts1 = matched_points1(knownInliers, :);
%     inlierPts2 = matched_points2(knownInliers, :);
%     fNorm8Point = estimateFundamentalMatrix(inlierPts1, inlierPts2, ...
%       'Method', 'Norm8Point')
%
%   Example 4
%   ---------
%     % Automatically register and rectify stereo images. This example
%     % detects corners in stereo images, matches the corners, computes
%     % the fundamental matrix, and then rectifies the images. 
%     % <a href="matlab:web(fullfile(matlabroot,'toolbox','vision','visiondemos','html','videorectification.html'))">View example</a>
%
% See also vision.CornerDetector, epipolarLine, extractFeatures, 
%     matchFeatures, vision.GeometricTransformEstimator,
%     estimateUncalibratedRectification
%
% References:
%   [1] R. Hartley, A. Zisserman, "Multiple View Geometry in Computer
%       Vision," Cambridge University Press, 2003.
%   [2] P. Rousseeuw, A. Leroy, "Robust Regression and Outlier Detection,"
%       John Wiley & Sons, 1987.
%   [3] P. H. S. Torr and A. Zisserman, "MLESAC: A New Robust Estimator
%       with Application to Estimating Image Geometry," Computer Vision
%       and Image Understanding, 2000.

% Copyright 2009 The MathWorks, Inc.
% $Revision: 1.1.6.7 $  $Date: 2011/05/09 03:42:51 $

% 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
r = parseInputs(matched_points1, matched_points2, varargin);
method = lower(r.Method);
outputClass = lower(r.OutputClass);
distanceType = lower(r.DistanceType);

isXY = cvstGetCoordsChoice('fcnEstimateFundamentalMatrix');

% Compute the fundamental matrix
[f, inliers, status] = cvalgEstimateFundamentalMatrix(...
  matched_points1, matched_points2, method, outputClass, r.NumTrials, ...
  distanceType, r.DistanceThreshold, r.Confidence, r.InlierPercentage, ...
  statusCode, isXY);

if nargout >= 2
  varargout{1} = inliers;
  if nargout == 3
    varargout{2} = status;
  end
end

if logical(r.ReportRuntimeError)
  % Report runtime error
  checkRuntimeStatus(statusCode, status, isXY);
end

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

%==========================================================================
% Parse and check inputs
%========================================================================== 
function r = parseInputs(matched_points1, matched_points2, optionalParameters)
% Value of string enums. The first string is the default.
methodOptions = {'LMedS', 'LTS', 'RANSAC', 'MSAC', 'Norm8Point'};
classOptions = {'double', 'single'};
distanceOptions = {'Sampson', 'Algebraic'};

% Instantiate an input parser
parser = inputParser;
parser.FunctionName = mfilename;
parser.CaseSensitive = true;

% Check MATCHED_POINTS1 and MATCHED_POINTS2
checkPts('matched_points1', matched_points1);
checkPts('matched_points2', matched_points2);

% Specify the optional parameters
parser.addParamValue('Method', methodOptions{1}, ...
  @(x)(checkStrings('Method', methodOptions, x)));
parser.addParamValue('OutputClass', classOptions{1}, ...
  @(x)(checkStrings('OutputClass', classOptions, x)));
parser.addParamValue('DistanceType', distanceOptions{1}, ...
  @(x)(checkStrings('DistanceType', distanceOptions, x)));
parser.addParamValue('NumTrials', 500, ...
  @(x)(checkNumTrials(x, 'NumTrials')));
parser.addParamValue('DistanceThreshold', 0.01, ...
  @(x)(checkThreshold(x, 'DistanceThreshold')));
parser.addParamValue('Confidence', 99, ...
  @(x)(checkPercentage(x, 'Confidence')));
parser.addParamValue('InlierPercentage', 50, ...
  @(x)(checkPercentage(x, 'InlierPercentage')));
parser.addParamValue('ReportRuntimeError', true, ...
  @(x)(checkReportRuntimeError(x, 'ReportRuntimeError')));

% Parse and check optional parameters
parser.parse(optionalParameters{:});
r = parser.Results;

%========================================================================== 
function checkPts(name, value)
validateattributes(value, {'numeric'}, ...
  {'2d', 'nonsparse', 'real'},...
  mfilename, name);

%========================================================================== 
function r = checkNumTrials(value, name)
validateattributes(value, {'numeric'}, ...
  {'scalar', 'nonsparse', 'real', 'integer', 'positive', 'finite'},...
  mfilename, name);
r = 1;

%========================================================================== 
function r = checkThreshold(value, name)
validateattributes(value, {'numeric'}, ...
  {'scalar', 'nonsparse', 'real', 'positive', 'finite'},...
  mfilename, name);
r = 1;

%========================================================================== 
function r = checkPercentage(value, name)
validateattributes(value, {'numeric'}, ...
  {'scalar', 'nonsparse', 'real', 'positive', 'finite', '<', 100},...
  mfilename, name);
r = 1;

%========================================================================== 
function r = checkReportRuntimeError(value, name)
validateattributes(value, {'logical', 'numeric'}, ...
  {'scalar', 'nonsparse', 'real'}, mfilename, name);
r = 1;

%========================================================================== 
function r = checkStrings(name, list, value)
potentialMatch = validatestring(value, list, mfilename, name);

if ~strcmpi(value, potentialMatch)
  message = sprintf('Expected %s to be', name);
  numStr = length(list);
  for idx = 1: numStr-2
    message = strcat(message, sprintf(' ''%s'',', list{idx}));
  end
  message = strcat(message, ...
    sprintf(' ''%s'' or ''%s''.', list{numStr-1}, list{numStr}));
  error('vision:estimateFundamentalMatrix:invalidString', message);
end
r = 1;
