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