% Algebraic estimation of E-Matrix from point pairs,
assumption: independent observations
assumption: points are conditioned.
following Sect. 13.3.2.3, p. 575
with CovM following Sect. 4.9.2.4
[E,est_sigma_0,error] = sugr_estimation_algebraic_E_Matrix_from_point_pairs(P)
P contains point pairs
P.h = [X,Y]
* X = N x 3 matrix of points
* Y = N x 3 matrix of points
P.Crr = N x 4 x 4 contains reduced covariance matrix
model
X(n,:) * E * Y(n,:)' = 0
E relatiove orientation
E.bR = estimated homography with uncertainty,
using algebraic minimization, thus neglecting the accuracy
E.Crr = CovM of reduced parameters, derived by variance propagation
est_sigma_0 estimated standard deviation of constraint
error = 0 ok
= 1 no basis found
Wolfgang Förstner 2/2010
wfoerstn@uni-bonn.de
See also sugr_E_Matrix, sugr_estimation_ml_E_Matrix_from_point_pairs0001 %% Algebraic estimation of E-Matrix from point pairs, 0002 % assumption: independent observations 0003 % assumption: points are conditioned. 0004 % 0005 % following Sect. 13.3.2.3, p. 575 0006 % with CovM following Sect. 4.9.2.4 0007 % 0008 % [E,est_sigma_0,error] = sugr_estimation_algebraic_E_Matrix_from_point_pairs(P) 0009 % 0010 % P contains point pairs 0011 % P.h = [X,Y] 0012 % * X = N x 3 matrix of points 0013 % * Y = N x 3 matrix of points 0014 % P.Crr = N x 4 x 4 contains reduced covariance matrix 0015 % 0016 % model 0017 % X(n,:) * E * Y(n,:)' = 0 0018 % 0019 % E relatiove orientation 0020 % E.bR = estimated homography with uncertainty, 0021 % using algebraic minimization, thus neglecting the accuracy 0022 % E.Crr = CovM of reduced parameters, derived by variance propagation 0023 % 0024 % est_sigma_0 estimated standard deviation of constraint 0025 % 0026 % error = 0 ok 0027 % = 1 no basis found 0028 % 0029 % Wolfgang Förstner 2/2010 0030 % wfoerstn@uni-bonn.de 0031 % 0032 % See also sugr_E_Matrix, sugr_estimation_ml_E_Matrix_from_point_pairs 0033 0034 0035 function [E,est_sigma_0,error] = sugr_estimation_algebraic_E_Matrix_from_point_pairs(P) 0036 0037 % pair coordinates 0038 Ph = P.h; 0039 % homogeneous coordinates 0040 xl = Ph(:,1:3); 0041 xr = Ph(:,4:6); 0042 % number of point pairs 0043 N = size(Ph,1); 0044 0045 %% 0046 % estimate E algebraically using > 7 points 0047 % 0048 % build coefficient matrix 0049 A = zeros(N,9); 0050 for n = 1:N 0051 % Kronecker 1x9 -matrix 0052 A(n,:) = [xr(n,1)*xl(n,:), xr(n,2)*xl(n,:) xr(n,3)*xl(n,:)]; 0053 end 0054 0055 % partition 0056 N_rows = size(A,1); 0057 if N_rows == 8 0058 e = null(A); 0059 A_plus = A'*inv(A*A'); %#ok<*MINV> 0060 est_sigma_0 = 1; 0061 else 0062 [U,D,V] = svd(A,'econ'); 0063 % find algebraically best solution 0064 e = V(:,9); 0065 % determine A+ = pseudo inverse of A with last singular value = 0 0066 Di = inv(D+eps*eye(9)); 0067 est_sigma_0 = sqrt(Di(9,9)/(N-5)); 0068 Di(9,9) = 0; 0069 A_plus = V * Di * U'; 0070 end 0071 E0 = reshape(e,3,3); 0072 0073 % Enforce same eigenvalues == 1 0074 [U0,~,V0]= svd(E0); 0075 U0 = U0*det(U0); 0076 V0 = V0*det(V0); 0077 Ee = U0 * diag([1,1,0]) * V0'; 0078 0079 % select from four cases 0080 [b,R] = sugr_select_bR_from_E_UV_uv(U0,V0,xl,xr); 0081 if norm(b) == 0 0082 E = zeros(3); 0083 est_sigma_0 = 0; 0084 error = 1; 0085 return 0086 end 0087 0088 % determine CovM of residuals of constraints from x=xl, y=xr 0089 % (following Sect. 4.9.2.4) 0090 % x' E y = g 0091 % d(x' E y) = d(g) 0092 % x' E dy + y' E' dx = dg 0093 %x' E Jyr dyr + y' E' Jxr dxr = dg 0094 % 0095 Cgg = sparse(zeros(N)); 0096 for n=1:N 0097 x1 = xl(n,:)'; 0098 x2 = xr(n,:)'; 0099 % VarProp for g 0100 J_x = + x2' * Ee' * null(x1'); 0101 J_y = + x1' * Ee * null(x2'); 0102 J = [J_x, J_y]; 0103 % effect of both 0104 Cgg(n,n) = Cgg(n,n) ... 0105 + J * squeeze(P.Crr(n,:,:)) * J'; %#ok<SPRIX> 0106 end 0107 0108 % determine Crr for 5 parameters 0109 Chh = A_plus * Cgg * A_plus'; 0110 0111 E = sugr_E_Matrix(b,R,Chh); 0112 error = 0;