1 | function [F, Sa, Sf] = linEstF(left, right, NUM_RESCALE) |
---|
2 | % [F, Sa, Sf] = linEstF(left, right) |
---|
3 | % Estimate F matrix from 3 x n matrices of corresponding points left |
---|
4 | % and right. |
---|
5 | % NUM_RESCALE (default TRUE) uses Hartley's rescaling. Always use |
---|
6 | % rescaling, unless you wish to show how badly the un-normalized |
---|
7 | % algorithm works. |
---|
8 | % Returns F, the singular values Sa of the 9x9 linear system, and |
---|
9 | % Sf, the singular values of the approximate F matrix (before grabbing |
---|
10 | % a rank 2 approximation. |
---|
11 | |
---|
12 | |
---|
13 | if nargin < 3 |
---|
14 | NUM_RESCALE = 1; |
---|
15 | end |
---|
16 | |
---|
17 | nPts = size(left,2); |
---|
18 | if nPts < 8 | nPts ~= size(right,2) |
---|
19 | fprintf(2, 'lineEstF: Innappropriate number of left and right points.'); |
---|
20 | F = []; |
---|
21 | return; |
---|
22 | end |
---|
23 | |
---|
24 | if size(left,1) == 2 |
---|
25 | left = [left; ones(1, nPts)]; |
---|
26 | else % Normalize to pixel coords |
---|
27 | left = left./repmat(left(3,:), 3,1); |
---|
28 | end |
---|
29 | if size(right,1) == 2 |
---|
30 | right = [right; ones(1, nPts)]; |
---|
31 | else % Normalize to pixel coords |
---|
32 | right = right./repmat(right(3,:), 3,1); |
---|
33 | end |
---|
34 | |
---|
35 | imPts = cat(3, left, right); |
---|
36 | |
---|
37 | %% Rescale image data for numerical stability. |
---|
38 | if NUM_RESCALE |
---|
39 | Knum = repmat(eye(3), [1,1,2]); |
---|
40 | %%% Rescale for numerical stability |
---|
41 | mn = sum(imPts(1:2,:,:),2)/nPts; |
---|
42 | mns = reshape(mn, [2 1 2]); |
---|
43 | var = sum(sum((imPts(1:2,:,:)-repmat(mns, [1 nPts 1])).^2,2)/nPts, 1); |
---|
44 | %% Scale image points so that sum of variances of x and y = 2. |
---|
45 | scl = sqrt(2./var(:)); |
---|
46 | %% Sanity: varScl = var .* reshape(scl.^2, [1 1 2]); % Should be 2 |
---|
47 | |
---|
48 | %% Scale so x and y variance is roughly 1, translate so image mean (x,y) is zero. |
---|
49 | Knum(1:2,3,:) = -mn; |
---|
50 | Knum(1:2,:,:) = Knum(1:2,:,:).*repmat(reshape(scl, [1 1 2]), [2, 3,1]); |
---|
51 | for kIm = 1:2 |
---|
52 | imPts(:,:,kIm) = reshape(Knum(:,:,kIm),3,3) * imPts(:,:,kIm); |
---|
53 | end |
---|
54 | %% Sanity check |
---|
55 | % sum(imPts(1:2,:,:),2)/nPts % Should be [0 0]' |
---|
56 | % sum(sum(imPts(1:2,:,:).^2,2)/nPts,1) % Should be 2. |
---|
57 | end |
---|
58 | |
---|
59 | %% Make constraint matrix A. |
---|
60 | %% The matrix F satisfies: A f = 0, where f = (F_1,1; F_1,2; ... F_3,3). |
---|
61 | left = reshape(imPts(:,:,1), [3 nPts]); |
---|
62 | right = reshape(imPts(:,:,2), [3 nPts]); |
---|
63 | A = [(repmat(left(1,:)',1,3).* right') (repmat(left(2,:)',1,3).* right') ... |
---|
64 | (right')]; |
---|
65 | |
---|
66 | %% Factor A |
---|
67 | [Ua Sa Va] = svd(A); Sa = diag(Sa); |
---|
68 | |
---|
69 | %% Set F to be the right null vector of A, reshaped to a 3x3 matrix. |
---|
70 | F = reshape(Va(:,end), 3,3)'; |
---|
71 | |
---|
72 | %% Modify F to make it rank 2. |
---|
73 | [Uf Sf Vf] = svd(F); Sf = diag(Sf); |
---|
74 | Sf0 = Sf; |
---|
75 | Sf0(end) = 0.0; |
---|
76 | F = Uf * diag(Sf0) * Vf'; |
---|
77 | |
---|
78 | %% Undo the renormalization |
---|
79 | if NUM_RESCALE |
---|
80 | F = reshape(Knum(:,:,1),3,3)' * F * reshape(Knum(:,:,2),3,3); |
---|
81 | end |
---|
82 | |
---|