sugr_estimate_F_and_epipoles_algebraically

% Algebraic estimate F-matrix and epipoles: from n > 7 points

 [F,Cff,el,Cll,er,Crr]=sugr_estimate_F_and_epipoles_algebraically(X,Cxx)

 X   = n x 4 Matrix of image points, not necessarily conditioned
 Cxx = n x 16 matrix of vectorized covariances of Euclidean point
       coordinates, correspoinding points may be correlated
       Cxx(i,:)= vec ( C(x_i'x_i')  C(x_i'x_i'') )
                      C(x_i''x_i')  C(x_i''x_i'') )

 F   = 3x3 matrix
 Cff = 9x9 Cov(vec(F))
 el  = left epipole
 Cll = 3x3 CovM of el
 er  = right epipole
 Crr = 3x3 CovM of er

 Wolfgang Förstner 2/2012
 wfoerstn@uni-bonn.de

0001 %% Algebraic estimate F-matrix and epipoles: from n > 7 points
0002 %
0003 % [F,Cff,el,Cll,er,Crr]=sugr_estimate_F_and_epipoles_algebraically(X,Cxx)
0004 %
0005 % X   = n x 4 Matrix of image points, not necessarily conditioned
0006 % Cxx = n x 16 matrix of vectorized covariances of Euclidean point
0007 %       coordinates, correspoinding points may be correlated
0008 %       Cxx(i,:)= vec ( C(x_i'x_i')  C(x_i'x_i'') )
0009 %                      C(x_i''x_i')  C(x_i''x_i'') )
0010 %
0011 % F   = 3x3 matrix
0012 % Cff = 9x9 Cov(vec(F))
0013 % el  = left epipole
0014 % Cll = 3x3 CovM of el
0015 % er  = right epipole
0016 % Crr = 3x3 CovM of er
0017 %
0018 % Wolfgang Förstner 2/2012
0019 % wfoerstn@uni-bonn.de
0020 
0021 function [F, Cff, el, Cll, er, Crr] = sugr_estimate_F_and_epipoles_algebraically(X, Cxx)
0022 
0023 % Number of observations N
0024 [N, ~] = size(X);
0025 
0026 % condition
0027 % left
0028 mxl = mean(X(:, 1)); sxl = std(X(:, 1));
0029 myl = mean(X(:, 2)); syl = std(X(:, 2));
0030 Y(:, 1) = (X(:, 1) - mxl) / sxl;
0031 Y(:, 2) = (X(:, 2) - myl) / syl;
0032 Tl = [1/sxl, 0, -mxl/sxl; 0, 1/syl, -myl/syl; 0 0 1];
0033 % right
0034 mxr = mean(X(:, 3)); sxr = std(X(:, 3));
0035 myr = mean(X(:, 4)); syr = std(X(:, 4));
0036 Y(:, 3) = (X(:, 3) - mxr) / sxr;
0037 Y(:, 4) = (X(:, 4) - myr) / syr;
0038 Tr = [1/sxr, 0, -mxr/sxr; 0, 1/syr, -myr/syr; 0, 0, 1];
0039 
0040 
0041 % Coefficients for f=vecF
0042 A = [Y(:, 1) .* Y(:, 3), ...
0043      Y(:, 2) .* Y(:, 3), ...
0044      1 * Y(:, 3), ...
0045      Y(:, 1) .* Y(:, 4), ...
0046      Y(:, 2) .* Y(:, 4), ...
0047      Y(:, 4), ...
0048      Y(:, 1), ...
0049      Y(:, 2), ...
0050      ones(N, 1)...
0051      ];
0052 % best f, approximate
0053 [U, D, V] = svd(A);
0054 
0055 % Pseudoinverse for Covariance matrix
0056 Dinv = D';
0057 for i = 1:8
0058     Dinv(i, i) = 1 / D(i, i);
0059 end
0060 Ainv = V * Dinv * U';
0061 
0062 % approximate F
0063 Fa = reshape(V(:, 9), 3, 3);
0064 
0065 % partition
0066 [U, D, V] = svd(Fa);
0067 
0068 % enforce singularity
0069 D(3, 3) = 0;
0070 
0071 % final estimate conditioned
0072 F = U * D * V';
0073 normF = norm(F, 'fro');
0074 F = F/normF; % normalize such that Frobenius norm is 1
0075 f = F(:);
0076 
0077 % check
0078 % diag([Y(:, 1), Y(:, 2), ones(N, 1)] * F * [Y(:, 3), Y(:, 4), ones(N, 1)]')
0079 
0080 % Covariance matrix: Ainv * Cn * Ainv' restricted to det F=0
0081 Cff = zeros(9);
0082 var_n = zeros(N,1);
0083 for n = 1:N
0084     % n-th constraint vector
0085     xx = [[Y(n, 3:4), 1] * F',[Y(n,1:2),1]*F];
0086     % n-th covariance matrix Euclidean
0087     C = reshape(Cxx(n, :), 4, 4);
0088     % n-th covarinace matrix homogeneous
0089     Cn = [C(1:2, 1:2) zeros(2, 1) C(1:2, 3:4) zeros(2, 1); ...
0090           zeros(1, 6); ...
0091           C(3:4, 1:2) zeros(2, 1) C(3:4, 3:4) zeros(2, 1); ...
0092           zeros(1, 6)...
0093           ];
0094     % Jacobian for x->y
0095     J = [Tl, zeros(3); zeros(3), Tr];
0096     % covariance matrix for conditioned homogeneous coordinates
0097     Cn = J * Cn * J';
0098     % variance of constraint
0099     var_n(n) = (xx * Cn * xx');
0100     % covarance matrix of vec f approximate
0101     Cff = Cff + Ainv(:, n) * Ainv(:, n)' * var_n(n);
0102 end
0103 % rankCffa = rank(Cff)
0104 
0105 % projector for enforcing det and norm constraint
0106 FA = adjunctMatrix(F)';
0107 H = [FA(:), f];
0108 P = eye(9) - H * inv(H'*H)*H';                                             %#ok<MINV>
0109 % covariance matrix for conditioned F
0110 Cff = P * Cff * P;
0111 
0112 % rankCff = rank(Cff)
0113 
0114 % uncoditioned F
0115 F = Tl' * F * Tr;
0116 
0117 % covariance matrix of unconditioned F
0118 J = kron(Tr',Tl');
0119 Cff = J * Cff * J';
0120 
0121 % rank(Cff);
0122 
0123 % determine epipoles and their covariances
0124 % left
0125 el = cross(F(:, 1), F(:, 2));
0126 fac = norm(el);
0127 el = el / fac;
0128 Jl = [-calc_S(F(:, 2)), calc_S(F(:, 1))];
0129 Cll = Jl * Cff(1:6, 1:6) * Jl'/fac^2;
0130 %right
0131 er = cross(F(1, :), F(2, :))';
0132 fac = norm(er);
0133 er = er / fac;
0134 Jr = [-calc_S(F(2, :)), calc_S(F(1, :))];
0135 Crr = Jr * Cff([1, 4, 7, 2, 5, 8], [1, 4, 7, 2, 5, 8]) * Jr'/fac^2;
0136 
0137 % check
0138 %[X(:,1),X(:,2),ones(N,1)]*F*[X(:,3),X(:,4),ones(N,1)]'
0139