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 | |
---|