[37] | 1 | % AFFINEFUNDMATRIX - computes affine fundamental matrix from 4 or more points |
---|
| 2 | % |
---|
| 3 | % Function computes the affine fundamental matrix from 4 or more matching |
---|
| 4 | % points in a stereo pair of images. The Gold Standard algorithm given |
---|
| 5 | % by Hartley and Zisserman p351 (2nd Ed.) is used. |
---|
| 6 | % |
---|
| 7 | % Usage: [F, e1, e2] = affinefundmatrix(x1, x2) |
---|
| 8 | % [F, e1, e2] = affinefundmatrix(x) |
---|
| 9 | % |
---|
| 10 | % Arguments: |
---|
| 11 | % x1, x2 - Two sets of corresponding point. If each set is 3xN |
---|
| 12 | % it is assumed that they are homogeneous coordinates. |
---|
| 13 | % If they are 2xN it is assumed they are inhomogeneous. |
---|
| 14 | % |
---|
| 15 | % x - If a single argument is supplied it is assumed that it |
---|
| 16 | % is in the form x = [x1; x2] |
---|
| 17 | % Returns: |
---|
| 18 | % F - The 3x3 fundamental matrix such that x2'*F*x1 = 0. |
---|
| 19 | % e1 - The epipole in image 1 such that F*e1 = 0 |
---|
| 20 | % e2 - The epipole in image 2 such that F'*e2 = 0 |
---|
| 21 | % |
---|
| 22 | |
---|
| 23 | % Peter Kovesi |
---|
| 24 | % School of Computer Science & Software Engineering |
---|
| 25 | % The University of Western Australia |
---|
| 26 | % pk at csse uwa edu au |
---|
| 27 | % http://www.csse.uwa.edu.au/~pk |
---|
| 28 | % |
---|
| 29 | % Feb 2005 |
---|
| 30 | |
---|
| 31 | |
---|
| 32 | function [F,e1,e2] = affinefundmatrix(varargin) |
---|
| 33 | |
---|
| 34 | [x1, x2, npts] = checkargs(varargin(:)); |
---|
| 35 | |
---|
| 36 | X = [x2; x1]; % Form vectors of correspondences |
---|
| 37 | Xmean = mean(X,2); % Mean |
---|
| 38 | |
---|
| 39 | deltaX = zeros(size(X)); |
---|
| 40 | for k = 1:4 |
---|
| 41 | deltaX(k,:) = X(k,:) - Xmean(k); |
---|
| 42 | end |
---|
| 43 | |
---|
| 44 | [U,D,V] = svd(deltaX',0); |
---|
| 45 | |
---|
| 46 | % Form fundamental matrix from the column of V corresponding to |
---|
| 47 | % smallest singular value. |
---|
| 48 | v = V(:,4); |
---|
| 49 | F = [ 0 0 v(1) |
---|
| 50 | 0 0 v(2) |
---|
| 51 | v(3) v(4) -v'*Xmean]; |
---|
| 52 | |
---|
| 53 | % Solve for epipoles |
---|
| 54 | [U,D,V] = svd(F,0); |
---|
| 55 | e1 = V(:,3); |
---|
| 56 | e2 = U(:,3); |
---|
| 57 | |
---|
| 58 | %-------------------------------------------------------------------------- |
---|
| 59 | % Function to check argument values and set defaults |
---|
| 60 | |
---|
| 61 | function [x1, x2, npts] = checkargs(arg); |
---|
| 62 | |
---|
| 63 | if length(arg) == 2 |
---|
| 64 | x1 = arg{1}; |
---|
| 65 | x2 = arg{2}; |
---|
| 66 | if ~all(size(x1)==size(x2)) |
---|
| 67 | error('x1 and x2 must have the same size'); |
---|
| 68 | elseif size(x1,1) == 3 |
---|
| 69 | % Convert to inhomogeneous coords |
---|
| 70 | x1(1,:) = x1(1,:)./x1(3,:); |
---|
| 71 | x1(2,:) = x1(2,:)./x1(3,:); |
---|
| 72 | x2(1,:) = x2(1,:)./x2(3,:); |
---|
| 73 | x2(2,:) = x2(2,:)./x2(3,:); |
---|
| 74 | x1 = x1(1:2,:); x2 = x2(1:2,:); |
---|
| 75 | elseif size(x1,1) ~= 2 |
---|
| 76 | error('x1 and x2 must be 2xN or 3xN arrays'); |
---|
| 77 | end |
---|
| 78 | |
---|
| 79 | elseif length(arg) == 1 |
---|
| 80 | if size(arg{1},1) == 6 |
---|
| 81 | x1 = arg{1}(1:3,:); |
---|
| 82 | x2 = arg{1}(4:6,:); |
---|
| 83 | % Convert to inhomogeneous coords |
---|
| 84 | x1(1,:) = x1(1,:)./x1(3,:); |
---|
| 85 | x1(2,:) = x1(2,:)./x1(3,:); |
---|
| 86 | x2(1,:) = x2(1,:)./x2(3,:); |
---|
| 87 | x2(2,:) = x2(2,:)./x2(3,:); |
---|
| 88 | x1 = x1(1:2,:); x2 = x2(1:2,:); |
---|
| 89 | elseif size(arg{1},1) == 4 |
---|
| 90 | x1 = arg{1}(1:2,:); |
---|
| 91 | x2 = arg{1}(3:4,:); |
---|
| 92 | else |
---|
| 93 | error('Single argument x must be 6xN'); |
---|
| 94 | end |
---|
| 95 | else |
---|
| 96 | error('Wrong number of arguments supplied'); |
---|
| 97 | end |
---|
| 98 | |
---|
| 99 | npts = size(x1,2); |
---|
| 100 | if npts < 4 |
---|
| 101 | error('At least 4 points are needed to compute the affine fundamental matrix'); |
---|
| 102 | end |
---|
| 103 | |
---|