[37] | 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); |
---|