function [f, inliers, status] = cvalgEstimateFundamentalMatrix(...
  pts1, pts2, method, outputClass, numTrials, distanceType, ...
  distanceThreshold, confidence, inlierPercentage, statusCode, isXY)
% Algorithm for computing the fundamental matrix.
%#codegen

%--------------------------------------------------------------------------
% Set default values for outputs and cast the class of the inputs
%--------------------------------------------------------------------------
integerClass = 'int32';
f = zeros([3, 3], outputClass);
status = statusCode.NotEnoughPts;
conf = cast(confidence, outputClass) * cast(0.01, outputClass);
threshold = cast(distanceThreshold, outputClass);
nTrials = cast(numTrials, integerClass);

%--------------------------------------------------------------------------
% Check the size of pts1 and pts2
%--------------------------------------------------------------------------
if isXY
  nPts = cast(size(pts1, 1), integerClass);
  inliers = false(nPts, 1);
  if size(pts1, 2) ~= 2 || size(pts2, 2) ~= 2
    status = statusCode.PtsWrongSize;
    return;
  elseif size(pts1, 1) ~= size(pts2, 1)
    status = statusCode.NumPtsMismatch;
    return;
  end
else
  nPts = cast(size(pts1, 2), integerClass);
  inliers = false(1, nPts);
  if size(pts1, 1) ~= 2 || size(pts2, 1) ~= 2
    status = statusCode.PtsWrongSize;
    return;
  elseif size(pts1, 2) ~= size(pts2, 2)
    status = statusCode.NumPtsMismatch;
    return;
  end
end

%--------------------------------------------------------------------------
% Convert pts1 and pts2 to homogeneous coordinates by padding with ones.
%--------------------------------------------------------------------------
pts1h = coder.nullcopy(zeros(3, nPts, outputClass));
pts2h = coder.nullcopy(zeros(3, nPts, outputClass));
pts1h(3, :)   = 1;
pts2h(3, :)   = 1;

if isXY
  pts1h(1:2, :) = pts1';
  pts2h(1:2, :) = pts2';
else
  pts1h(1:2, :) = pts1;
  pts2h(1:2, :) = pts2;
end

%--------------------------------------------------------------------------
% Compute the fundamental matrix and inliers based on selected method.
%--------------------------------------------------------------------------
if strcmp(method, 'norm8point')
  if nPts >= 8
    f = norm8Point(pts1h, pts2h, outputClass, integerClass);
    inliers(:) = true(1, nPts);
    status = statusCode.NoError;
  end
else
  % Find the inliers in pts1 and pts2 (pts1h and pts2h).
  switch method
    case 'lmeds'
      [inliers(:), status] = lmeds(statusCode, distanceType,...
        outputClass, integerClass, pts1h, pts2h, nPts, nTrials);
    case 'lts'
      percent = cast(inlierPercentage,outputClass)*cast(0.01,outputClass);
      [inliers(:), status] = lts(statusCode, distanceType,...
        outputClass, integerClass, pts1h, pts2h, nPts, nTrials, percent);
    case 'ransac'
      [inliers(:), status] = ransac(statusCode, distanceType,...
        outputClass, integerClass, pts1h, pts2h, nPts, nTrials,...
        threshold, conf);
    otherwise % MSAC
      [inliers(:), status] = msac(statusCode, distanceType,...
        outputClass, integerClass, pts1h, pts2h, nPts, nTrials,...
        threshold, conf);
  end

  % Compute the fundamental matrix from the inliers
  if status == statusCode.NoError
    f = norm8Point(pts1h(:, inliers), pts2h(:, inliers), outputClass,...
      integerClass);
  end
end

%========================================================================== 
% Find out inliers in the input data by using LMedS algorithm
%========================================================================== 
function [inliers, status] = lmeds(statusCode, disType, outputClass,...
  integerClass, pts1h, pts2h, nPts, nTrials)

inliers = false(1, nPts);
bestDis = realmax(outputClass);
bestF = coder.nullcopy(zeros([3, 3], outputClass));

if nPts >= 16
  for idx = 1: nTrials
    [d, f] = estTFormDistance(...
      disType, pts1h, pts2h, nPts, outputClass, integerClass);
    
    curDis = median(d);
    if bestDis > curDis
      bestDis = curDis;
      bestF = f;
    end
  end

  if bestDis < realmax(outputClass)
    d = computeDistance(disType, pts1h, pts2h, bestF);
    inliers = (d <= bestDis);
    status = statusCode.NoError;
  else
    status = statusCode.NotEnoughInliers;
  end
else
  status = statusCode.NotEnoughPts;
end

%========================================================================== 
% Find out inliers in the input data by using LTS algorithm
%========================================================================== 
function [inliers, status] = lts(statusCode, disType, outputClass,...
  integerClass, pts1h, pts2h, nPts, nTrials, inlierPercentage)

inliers = false(1, nPts);
bestDis = realmax(outputClass);
bestF = coder.nullcopy(zeros([3, 3], outputClass));

idxDis = cast(floor(inlierPercentage * cast(nPts, outputClass)),...
  integerClass);

if idxDis >= 8
  for idx = 1: nTrials
    [d, f] = estTFormDistance(...
      disType, pts1h, pts2h, nPts, outputClass, integerClass);
    
    d = sort(d, 'ascend');
    curDis = d(idxDis);
    if bestDis > curDis
      bestDis = curDis;
      bestF = f;
    end
  end

  if bestDis < realmax(outputClass)
    d = computeDistance(disType, pts1h, pts2h, bestF);
    inliers = (d <= bestDis);
    status = statusCode.NoError;
  else
    status = statusCode.NotEnoughInliers;
  end
else
  status = statusCode.NotEnoughPts;
end

%========================================================================== 
% Find out inliers in the input data by using RANSAC algorithm
%========================================================================== 
function [inliers, status] = ransac(statusCode, disType, outputClass,...
  integerClass, pts1h, pts2h, nPts, nTrials, threshold, confidence)

inliers = false(1, nPts);
if nPts >= 8
  maxNTrials = nTrials;
  curNTrials = zeros(1, integerClass);
  bestNInliers = zeros(1, integerClass);
  logOneMinusConf = log(ones(1, outputClass) - confidence);
  oneOverNPts = ones(1, outputClass) / cast(nPts, outputClass);
  
  while curNTrials < maxNTrials
    d = estTFormDistance(disType, pts1h, pts2h, nPts, outputClass,...
      integerClass);
    
    [curInliers, curNInliers] = findInliers(d, nPts, threshold);
    
    if bestNInliers < curNInliers
      bestNInliers = curNInliers;
      inliers = curInliers;
      
      % Update the number of trials
      maxNTrials = updateNumTrials(oneOverNPts, logOneMinusConf, ...
        outputClass, integerClass, curNInliers, maxNTrials);
    end
    curNTrials = curNTrials + 1;
  end
  
  if bestNInliers >= 8
    status = statusCode.NoError;
  else
    status = statusCode.NotEnoughInliers;
  end
else
  status = statusCode.NotEnoughPts;
end

%========================================================================== 
% Find out inliers in the input data by using MSAC algorithm
%========================================================================== 
function [inliers, status] = msac(statusCode, disType, outputClass,...
  integerClass, pts1h, pts2h, nPts, nTrials, threshold, confidence)

inliers = false(1, nPts);
if nPts >= 8
  maxNTrials = nTrials;
  curNTrials = zeros(1, integerClass);
  bestDis = realmax(outputClass);
  logOneMinusConf = log(ones(1, outputClass) - confidence);
  oneOverNPts = ones(1, outputClass) / cast(nPts, outputClass);
  
  while curNTrials < maxNTrials
    d = estTFormDistance(disType, pts1h, pts2h, nPts, outputClass,...
      integerClass);
    
    [curInliers, curNInliers] = findInliers(d, nPts, threshold);
    curDis = sum(d(curInliers)) + ...
      cast((nPts - curNInliers), outputClass) * threshold;
    
    if bestDis > curDis
      bestDis = curDis;
      inliers = curInliers;
      
      % Update the number of trials
      maxNTrials = updateNumTrials(oneOverNPts, logOneMinusConf, ...
        outputClass, integerClass, curNInliers, maxNTrials);
    end
    curNTrials = curNTrials + 1;
  end
  
  if sum(cast(inliers, integerClass), 'native') >= cast(8, integerClass)
    status = statusCode.NoError;
  else
    status = statusCode.NotEnoughInliers;
  end
else
  status = statusCode.NotEnoughPts;
end

%========================================================================== 
% Update the number of trials based on the desired confidence and the
% inlier ratio.
%========================================================================== 
function maxNTrials = updateNumTrials(oneOverNPts, logOneMinusConf, ...
  outputClass, integerClass, curNInliers, maxNTrials)

ratioOfInliers = cast(curNInliers, outputClass) * oneOverNPts;
if ratioOfInliers > cast(1, outputClass) - eps(outputClass)
  newNum = zeros(1, integerClass);
else
  ratio8 = ratioOfInliers^8;
  if ratio8 > eps(ones(1, outputClass))
    logOneMinusRatio8 = log(ones(1, outputClass) - ratio8);
    newNum = cast(ceil(logOneMinusConf / logOneMinusRatio8), integerClass);
  else
    newNum = intmax(integerClass);
  end
end

if maxNTrials > newNum
  maxNTrials = newNum;
end

%========================================================================== 
% Randomly select 8 points and compute the fundamental matrix.
%========================================================================== 
function [d, f] = estTFormDistance(disType, pts1h, pts2h, nPts,...
  outputClass, integerClass)

indices = generateRandomIndices(nPts, outputClass, integerClass);
f = norm8Point(pts1h(:, indices), pts2h(:, indices), outputClass,...
  integerClass);
d = computeDistance(disType, pts1h, pts2h, f);

%========================================================================== 
% Generate 8 unique random indices. The algorithm is summarized as follows:
% 1. Initialize 'indices' output to an empty vector and initialize
%    'nSets' to 0.
% 2. Generate a set of 8 random integers ranging from 1 to 'nPts'.
% 3. Find the unique random values and add them to 'indices'.
% 4. If 8 unique values have been added to 'indices', return; otherwise, 
%    do the following:
% 4.1 If 'nSets' is less than 5, generate another set of 8 random integers
%     ranging from 1 to nPts, increment 'nSets', and go to Step 3.
% 4.2 If 'nSets' equals to 5, generate 8 deterministic integers ranging
%     from 1 to nPts and go to Step 3.
%========================================================================== 
function indices = generateRandomIndices(nPts, outputClass, integerClass)

indices = zeros(8, 1, integerClass);
nUnique = zeros(1, integerClass);
nSets = zeros(1, integerClass);
idxIn = ones(1, integerClass);

% Generate a set of 8 random values.
randVal = cast(ceil(rand([1,8], outputClass) * cast(nPts, outputClass)),...
  integerClass);

% Loop until 8 unique numbers have been found.
while nUnique < cast(8, integerClass)
  % Check if the random value is different from all previously selected 
  % ones.
  isUnique = true;
  for idxOut = 1: nUnique
    if randVal(idxIn) == indices(idxOut)
      isUnique = false;
      break;
    end
  end
  
  % If the random value is unique, use it.
  if isUnique
    nUnique = nUnique + ones(1, integerClass);
    indices(nUnique) = randVal(idxIn);
  end
  
  % Move to the next random value; if there are no more random values,
  % generate a new set of random values or use deterministic values.
  if idxIn < cast(8, integerClass)
    idxIn = idxIn + 1;
  else
    if nUnique < cast(8, integerClass)
      % We will try 5 sets of random values. If we cannot find enough
      % random values from 5 sets, we will use any index that has not
      % been used.
      maxNumSetOfRandomDrawing = cast(5, integerClass);
      if nSets < maxNumSetOfRandomDrawing
        randVal = cast(ceil(rand([1, 8], outputClass) *...
          cast(nPts, outputClass)), integerClass);
        nSets = nSets + 1;
      else
        firstVal = randVal(8);
        randVal = firstVal + cast(1:8, integerClass);
        for idx = cast(1, integerClass): cast(8, integerClass)
          if randVal(idx) > nPts
            randVal(idx) = randVal(idx) - nPts;
          end
        end
      end
      idxIn(:) = 1;
    end
  end
end

%========================================================================== 
% Function norm8Point computes the fundamental matrix using THE NORMALIZED
% 8-POINT ALGORITHM as described in page 281 of the following reference:
%   R. Hartley, A. Zisserman, "Multiple View Geometry in Computer Vision," 
%   Cambridge University Press, 2003.
%========================================================================== 
function f = norm8Point(pts1h, pts2h, outputClass, integerClass)
% Normalize the points
num = cast(size(pts1h, 2), integerClass);
[pts1h, t1] = normalizePoints(pts1h, outputClass, integerClass);
[pts2h, t2] = normalizePoints(pts2h, outputClass, integerClass);

% Compute the constraint matrix
m = coder.nullcopy(zeros(num, 9, outputClass));
for idx = 1: num
  m(idx,:) = [...
    pts1h(1,idx)*pts2h(1,idx), pts1h(2,idx)*pts2h(1,idx), pts2h(1,idx), ...
    pts1h(1,idx)*pts2h(2,idx), pts1h(2,idx)*pts2h(2,idx), pts2h(2,idx), ...
                 pts1h(1,idx),              pts1h(2,idx), 1];
end

% Find out the eigen-vector corresponding to the smallest eigen-value.
[~, ~, vm] = svd(m, 0);
f = reshape(vm(:, end), 3, 3)';

% Enforce rank-2 constraint
[u, s, v] = svd(f);
s(end) = 0;
f = u * s * v';

% Transform the fundamental matrix back to its original scale.
f = t2' * f * t1;

% Normalize the fundamental matrix.
f = f / norm(f);
if f(end) < 0
  f = -f;
end

%========================================================================== 
% Find out the point pairs whose distances are less than the threshold.
%========================================================================== 
function [inliers, nInliers] = findInliers(distance, nPts, threshold)

inliers = coder.nullcopy(false(1, nPts));
nInliers = zeros(1, class(nPts));

for idx = 1: nPts
  if (distance(idx) <= threshold)
    inliers(idx) = true;
    nInliers = nInliers + 1;
  else
    inliers(idx) = false;
  end
end

%========================================================================== 
% Compute the distance of points according to a fundamental matrix.
%========================================================================== 
function d = computeDistance(disType, pts1h, pts2h, f)
pfp = (pts2h' * f)';
pfp = pfp .* pts1h;
d = sum(pfp, 1) .^ 2;

if strcmp(disType, 'sampson')
  epl1 = f * pts1h;
  epl2 = f' * pts2h;
  d = d ./ (epl1(1,:).^2 + epl1(2,:).^2 + epl2(1,:).^2 + epl2(2,:).^2);
end

%========================================================================== 
% Normalizes the points so the mass center is at [0,0] and root mean square
% is sqrt(2).
%========================================================================== 
function [newP, T] = normalizePoints(p, outputClass, integerClass)

num = cast(size(p, 2), integerClass);
% centroid and standard variance of points
cent = mean(p(1:2, :), 2);
squareDist = zeros(1, outputClass);
for idx = 1: num
  squareDist = squareDist + (p(1,idx)-cent(1)).^2 + (p(2,idx)-cent(2)).^2;
end

if squareDist > 0
  scale = sqrt(cast(2*num, outputClass) / squareDist);
else
  scale = ones(1, outputClass);
end

T = [scale,     0, -scale*cent(1);
         0, scale, -scale*cent(2);
         0,     0,  1];

newP = T * p;
