Home > General-Functions > SUGR > E-Matrix > sugr_estimation_algebraic_E_Matrix_from_point_pairs.m

sugr_estimation_algebraic_E_Matrix_from_point_pairs

PURPOSE ^

% Algebraic estimation of E-Matrix from point pairs, assuming independent observations

SYNOPSIS ^

function [E,est_sigma_0,error] = sugr_estimation_algebraic_E_Matrix_from_point_pairs(P)

DESCRIPTION ^

% 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

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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;

Generated on Mon 08-Jan-2018 17:21:49 by m2html © 2005