1 | %u2FI Estimate fundamental matrix using ortogonal LS regression |
---|
2 | % |
---|
3 | % F = u2F(u) estimates F from u using NORMU |
---|
4 | % F = u2F(u,'nonorm') disables normalization |
---|
5 | % |
---|
6 | % see also NORMU, U2FA |
---|
7 | % |
---|
8 | % Returns 0 if too few points are available |
---|
9 | |
---|
10 | function F = u2FI (u, str, A1, A2) |
---|
11 | |
---|
12 | sampcols = find(sum(~isnan(u(1:3:end,:))) == 2); |
---|
13 | if length(sampcols) < 8 |
---|
14 | F = 0; return |
---|
15 | end |
---|
16 | |
---|
17 | if (nargin > 2) & ~strcmp(str, 'nonorm') & ~strcmp(str, 'usenorm') |
---|
18 | donorm = 1; |
---|
19 | else |
---|
20 | donorm = 0; |
---|
21 | end |
---|
22 | |
---|
23 | ptNum = size(sampcols,2); |
---|
24 | |
---|
25 | if donorm |
---|
26 | A1 = normu(u(1:3,sampcols)); |
---|
27 | A2 = normu(u(4:6,sampcols)); |
---|
28 | if isempty(A1) | isempty(A2), F = 0; return; end |
---|
29 | |
---|
30 | u1 = A1*u(1:3,sampcols); %in u1, u2 there are only columns of sampcols |
---|
31 | u2 = A2*u(4:6,sampcols); |
---|
32 | else |
---|
33 | u1 = u(1:3,sampcols); %" " |
---|
34 | u2 = u(4:6,sampcols); |
---|
35 | end |
---|
36 | |
---|
37 | for i = 1:ptNum |
---|
38 | Z(i,:) = reshape(u1(:,i)*u2(:,i)',1,9); |
---|
39 | end |
---|
40 | |
---|
41 | M = Z'*Z; |
---|
42 | V = seig(M); |
---|
43 | F = reshape(V(:,1),3,3); |
---|
44 | |
---|
45 | %odrizneme nejmensi vlastni slozku, aby F melo hodnost 2 |
---|
46 | [uu,us,uv] = svd(F); |
---|
47 | %[y,i] = min (abs(diag(us))); |
---|
48 | i = 3; |
---|
49 | %if us(i,i) > 1e-12, disp('rank(F)>2'); end |
---|
50 | us(i,i) = 0; |
---|
51 | F = uu*us*uv'; |
---|
52 | |
---|
53 | if donorm | strcmp(str, 'usenorm') |
---|
54 | F = A1'*F*A2; |
---|
55 | end |
---|
56 | |
---|
57 | F1=F; |
---|
58 | |
---|
59 | F = F /norm(F,2); |
---|
60 | |
---|
61 | if rank(F) > 2 |
---|
62 | %disp('!!! Error: u2FI: rank(F) > 2'); |
---|
63 | % snizime hodnost, us(3,3) je stejne 1e-16 |
---|
64 | [uu,us,uv] = svd(F); |
---|
65 | us(3,3) = 0; |
---|
66 | F = uu*us*uv'; |
---|
67 | end |
---|
68 | |
---|
69 | %seig sorted eigenvalues |
---|
70 | function [V,d] = seig(M) |
---|
71 | [V,D] = eig(M); |
---|
72 | [d,s] = sort(diag(D)); |
---|
73 | V = V(:,s); |
---|