% RANSACFITAFFINEFUND - fits affine fundamental matrix using RANSAC%% Usage:   [F, inliers] = ransacfitaffinefund(x1, x2, t)%% Arguments:%          x1  - 2xN or 3xN set of homogeneous points.  If the data is%                2xN it is assumed the homogeneous scale factor is 1.%          x2  - 2xN or 3xN set of homogeneous points such that x1<->x2.%          t   - The distance threshold between data point and the model%                used to decide whether a point is an inlier or not. %                Note that point coordinates are normalised to that their%                mean distance from the origin is sqrt(2).  The value of%                t should be set relative to this, say in the range %                0.001 - 0.01  %% Note that it is assumed that the matching of x1 and x2 are putative and it% is expected that a percentage of matches will be wrong.%% Returns:%          F       - The 3x3 fundamental matrix such that x2'Fx1 = 0.%          inliers - An array of indices of the elements of x1, x2 that were%                    the inliers for the best model.%% See Also: RANSAC, FUNDMATRIX AFFINEFUNDMATRIX% Copyright (c) 2004-2005 Peter Kovesi% School of Computer Science & Software Engineering% The University of Western Australia% http://www.csse.uwa.edu.au/% % Permission is hereby granted, free of charge, to any person obtaining a copy% of this software and associated documentation files (the "Software"), to deal% in the Software without restriction, subject to the following conditions:% % The above copyright notice and this permission notice shall be included in % all copies or substantial portions of the Software.%% The Software is provided "as is", without warranty of any kind.% February 2004  Original version% August   2005  Distance error function changed to match changes in RANSACfunction [F, inliers] = ransacfitaffinefund(x1, x2, t, feedback)    if ~all(size(x1)==size(x2))        error('Data sets x1 and x2 must have the same dimension');    end        if nargin == 3	feedback = 0;    end        [rows,npts] = size(x1);    if rows~=2 & rows~=3        error('x1 and x2 must have 2 or 3 rows');    end        if rows == 2    % Pad data with homogeneous scale factor of 1        x1 = [x1; ones(1,npts)];        x2 = [x2; ones(1,npts)];            end        % Normalise each set of points so that the origin is at centroid and    % mean distance from origin is sqrt(2).  normalise2dpts also ensures the    % scale parameter is 1.  Note that 'fundmatrix' will also call    % 'normalise2dpts' but the code in 'ransac' that calls the distance    % function will not - so it is best that we normalise beforehand.    [x1, T1] = normalise2dpts(x1);    [x2, T2] = normalise2dpts(x2);    s = 4;  % Number of points needed to fit an affine fundamental matrix.         fittingfn = @affinefundmatrix;    distfn    = @funddist;    degenfn   = @isdegenerate;    % x1 and x2 are 'stacked' to create a 6xN array for ransac    [F, inliers] = ransac([x1; x2], fittingfn, distfn, degenfn, s, t, feedback);    % Now do a final least squares fit on the data points considered to    % be inliers.    F = affinefundmatrix(x1(:,inliers), x2(:,inliers));        % Denormalise    F = T2'*F*T1;    %--------------------------------------------------------------------------% Function to evaluate the first order approximation of the geometric error% (Sampson distance) of the fit of a fundamental matrix with respect to a% set of matched points as needed by RANSAC.  See: Hartley and Zisserman,% 'Multiple View Geometry in Computer Vision', page 270.function [inliers, F] = funddist(F, x, t);        x1 = x(1:3,:);    % Extract x1 and x2 from x    x2 = x(4:6,:);        x2tFx1 = zeros(1,length(x1));    for n = 1:length(x1)	x2tFx1(n) = x2(:,n)'*F*x1(:,n);    end        Fx1 = F*x1;    Ftx2 = F'*x2;             % Evaluate distances    d =  x2tFx1.^2 ./ ...	 (Fx1(1,:).^2 + Fx1(2,:).^2 + Ftx2(1,:).^2 + Ftx2(2,:).^2);        inliers = find(abs(d) < t);     % Indices of inlying points    %----------------------------------------------------------------------% (Degenerate!) function to determine if a set of matched points will result% in a degeneracy in the calculation of a fundamental matrix as needed by% RANSAC.  This function assumes this cannot happen...     function r = isdegenerate(x)    r = 0;