1 | % FUNDMATRIX - computes fundamental matrix from 8 or more points |
---|
2 | % |
---|
3 | % Function computes the fundamental matrix from 8 or more matching points in |
---|
4 | % a stereo pair of images. The normalised 8 point algorithm given by |
---|
5 | % Hartley and Zisserman p265 is used. To achieve accurate results it is |
---|
6 | % recommended that 12 or more points are used |
---|
7 | % |
---|
8 | % Usage: [F, e1, e2] = fundmatrix(x1, x2) |
---|
9 | % [F, e1, e2] = fundmatrix(x) |
---|
10 | % |
---|
11 | % Arguments: |
---|
12 | % x1, x2 - Two sets of corresponding 3xN set of homogeneous |
---|
13 | % points. |
---|
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 | % Copyright (c) 2002-2005 Peter Kovesi |
---|
24 | % School of Computer Science & Software Engineering |
---|
25 | % The University of Western Australia |
---|
26 | % http://www.csse.uwa.edu.au/ |
---|
27 | % |
---|
28 | % Permission is hereby granted, free of charge, to any person obtaining a copy |
---|
29 | % of this software and associated documentation files (the "Software"), to deal |
---|
30 | % in the Software without restriction, subject to the following conditions: |
---|
31 | % |
---|
32 | % The above copyright notice and this permission notice shall be included in |
---|
33 | % all copies or substantial portions of the Software. |
---|
34 | % |
---|
35 | % The Software is provided "as is", without warranty of any kind. |
---|
36 | |
---|
37 | % Feb 2002 - Original version. |
---|
38 | % May 2003 - Tidied up and numerically improved. |
---|
39 | % Feb 2004 - Single argument allowed to enable use with RANSAC. |
---|
40 | % Mar 2005 - Epipole calculation added, 'economy' SVD used. |
---|
41 | % Aug 2005 - Octave compatibility |
---|
42 | |
---|
43 | function [F,e1,e2] = fundmatrix(varargin) |
---|
44 | |
---|
45 | [x1, x2, npts] = checkargs(varargin(:)); |
---|
46 | v = version; Octave = v(1)<'5'; % Crude Octave test |
---|
47 | |
---|
48 | % Normalise each set of points so that the origin |
---|
49 | % is at centroid and mean distance from origin is sqrt(2). |
---|
50 | % normalise2dpts also ensures the scale parameter is 1. |
---|
51 | [x1, T1] = normalise2dpts(x1); |
---|
52 | [x2, T2] = normalise2dpts(x2); |
---|
53 | |
---|
54 | % Build the constraint matrix |
---|
55 | A = [x2(1,:)'.*x1(1,:)' x2(1,:)'.*x1(2,:)' x2(1,:)' ... |
---|
56 | x2(2,:)'.*x1(1,:)' x2(2,:)'.*x1(2,:)' x2(2,:)' ... |
---|
57 | x1(1,:)' x1(2,:)' ones(npts,1) ]; |
---|
58 | |
---|
59 | if Octave |
---|
60 | [U,D,V] = svd(A); % Don't seem to be able to use the economy |
---|
61 | % decomposition under Octave here |
---|
62 | else |
---|
63 | [U,D,V] = svd(A,0); % Under MATLAB use the economy decomposition |
---|
64 | end |
---|
65 | |
---|
66 | % Extract fundamental matrix from the column of V corresponding to |
---|
67 | % smallest singular value. |
---|
68 | F = reshape(V(:,9),3,3)'; |
---|
69 | |
---|
70 | % Enforce constraint that fundamental matrix has rank 2 by performing |
---|
71 | % a svd and then reconstructing with the two largest singular values. |
---|
72 | [U,D,V] = svd(F,0); |
---|
73 | F = U*diag([D(1,1) D(2,2) 0])*V'; |
---|
74 | |
---|
75 | % Denormalise |
---|
76 | F = T2'*F*T1; |
---|
77 | |
---|
78 | if nargout == 3 % Solve for epipoles |
---|
79 | [U,D,V] = svd(F,0); |
---|
80 | e1 = hnormalise(V(:,3)); |
---|
81 | e2 = hnormalise(U(:,3)); |
---|
82 | end |
---|
83 | |
---|
84 | %-------------------------------------------------------------------------- |
---|
85 | % Function to check argument values and set defaults |
---|
86 | |
---|
87 | function [x1, x2, npts] = checkargs(arg); |
---|
88 | |
---|
89 | if length(arg) == 2 |
---|
90 | x1 = arg{1}; |
---|
91 | x2 = arg{2}; |
---|
92 | if ~all(size(x1)==size(x2)) |
---|
93 | error('x1 and x2 must have the same size'); |
---|
94 | elseif size(x1,1) ~= 3 |
---|
95 | error('x1 and x2 must be 3xN'); |
---|
96 | end |
---|
97 | |
---|
98 | elseif length(arg) == 1 |
---|
99 | if size(arg{1},1) ~= 6 |
---|
100 | error('Single argument x must be 6xN'); |
---|
101 | else |
---|
102 | x1 = arg{1}(1:3,:); |
---|
103 | x2 = arg{1}(4:6,:); |
---|
104 | end |
---|
105 | else |
---|
106 | error('Wrong number of arguments supplied'); |
---|
107 | end |
---|
108 | |
---|
109 | npts = size(x1,2) |
---|
110 | if npts < 8 |
---|
111 | warning('At least 8 points are needed to compute the fundamental matrix'); |
---|
112 | end |
---|
113 | |
---|