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

sugr_estimation_ml_E_Matrix_from_point_pairs

PURPOSE ^

% ML estimation of E-Matrix from point pairs

SYNOPSIS ^

function [E, sigma_0, R] = sugr_estimation_ml_E_Matrix_from_point_pairs(l, xa, T, maxiter)

DESCRIPTION ^

% ML estimation of E-Matrix from point pairs

 [E,sigma_0,R] = sugr_estimation_ml_E_Matrix_from_point_pairs(l,xa,T,maxiter)

 * l    = struct of point paris
 * xa   = 3 x 4 matrix [b,R] 
 * T    = threshold for iteration
 * maxiter = maximal iteration

 * E = struct estimated essential matrix, with sigma_0=1
 * sigma0 = estimated sigma0
 * R = redundancy


 Wolfgang Förstner 11/2017
 wfoerstn@uni-bonn.de

 See also sugr_E_Matrix, sugr_estimation_algebraic_E_Matrix_from_point_pairs

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %% ML estimation of E-Matrix from point pairs
0002 %
0003 % [E,sigma_0,R] = sugr_estimation_ml_E_Matrix_from_point_pairs(l,xa,T,maxiter)
0004 %
0005 % * l    = struct of point paris
0006 % * xa   = 3 x 4 matrix [b,R]
0007 % * T    = threshold for iteration
0008 % * maxiter = maximal iteration
0009 %
0010 % * E = struct estimated essential matrix, with sigma_0=1
0011 % * sigma0 = estimated sigma0
0012 % * R = redundancy
0013 %
0014 %
0015 % Wolfgang Förstner 11/2017
0016 % wfoerstn@uni-bonn.de
0017 %
0018 % See also sugr_E_Matrix, sugr_estimation_algebraic_E_Matrix_from_point_pairs
0019 
0020 function [E, sigma_0, R] = sugr_estimation_ml_E_Matrix_from_point_pairs(l, xa, T, maxiter)
0021 
0022 global print_option_estimation
0023 global min_redundancy
0024 
0025 %% Initialization
0026 U = 5; % number of unknown parameters
0027 Nlr = 4; % number of reduced parameters per observational group
0028 Gc = 1; % number of constraints per observational group
0029 
0030 lh = l.h; % Nc x Nl matrix
0031 lCrr = l.Crr; % Nc x Nlr x Nlr matric with Crr
0032 Nc = size(lh, 1); % number of pairs
0033 
0034 G = Nc * Gc; % number of constraints
0035 R = G - U; % redundancy
0036 if R < 0
0037     return
0038 end;
0039 
0040 estl = lh; % initialize estimated observations
0041 w_f = ones(Nc, 1); % initial weights for robust estimate
0042 
0043 if isstruct(xa) % initiate estx, estimated unknowns
0044     estx = xa.bR; % [ba,Ra]
0045 else
0046     estx = xa;
0047 end
0048 
0049 s = 0; % control variable for iterations
0050 residuals = zeros(Nc, 1); % intial residuals
0051 
0052 %% Start iteration -------------------------------------
0053 for nu = 1:maxiter
0054     if print_option_estimation > 0
0055         sprintf('nu = %2', nu);
0056     end
0057     Cr = zeros(Nc, Nlr, Nlr); % reduced covariance matrices
0058     v_r = zeros(Nc, Nlr); % reduced residuals
0059     A = zeros(Nc, Gc, U); % Jacobians c -> x
0060     B = zeros(Nc, Gc, Nlr); % Jacobians c -> l
0061     W = zeros(Nc, Gc, Gc); % Weights of constraints
0062     cg = zeros(Nc, Gc); % constraint's residuals
0063  
0064     N_matrix = zeros(U); % normal equation matrix
0065     h_vector = zeros(U, 1); % right hand sides
0066  
0067     %% Build design matrices
0068     for n = 1:Nc
0069         estl_n = estl(n, :)';                 % get estl
0070         l_n = lh(n, :)';                   % get l
0071         Crr_n = squeeze(lCrr(n, :, :));
0072         % determine cg and Jacobians (checked)
0073         [lr_n, Cr_n, cg_n, atr_n, btr_n] = sugr_ghm_cg_E_matrix_from_point_pairs(l_n, estl_n, estx, Crr_n);
0074         % Store these
0075         A(n, :, :) = atr_n; % Gc x U
0076         B(n, :, :) = btr_n; % Gc x Nlr
0077         Cr(n, :, :) = Cr_n; % Nlr x Nlr
0078         v_r(n, :) = - lr_n';                % 1 x Nlr
0079         cg(n, :) = cg_n';                 % 1 x Gc
0080      
0081         % weight of contraint
0082         bCovb_n = btr_n * Cr_n * btr_n';      % Gc x Gc
0083         W(n, :, :) = inv(bCovb_n) * w_f(n); % Gc x Gc
0084         aW = atr_n' * squeeze(W(n,:,:)); % U x Gc
0085      
0086         N_matrix = N_matrix + aW * atr_n; % U x U
0087         h_vector = h_vector + aW * cg_n; % U x 1
0088      
0089     end
0090  
0091     if print_option_estimation > 0
0092         N_matrix = N_matrix                                                %#ok<NOPRT,ASGSL>
0093         l_lambda = log(eigs(N_matrix))                                     %#ok<NOPRT,NASGU>
0094         h_vector                                                           %#ok<NOPRT>
0095     end
0096  
0097     det(N_matrix);
0098     %% Solve
0099     Cxrxr = inv(N_matrix);
0100     estx_r = Cxrxr * h_vector;                                             %#ok<*MINV>
0101  
0102     if print_option_estimation > 0
0103         disp(['Result of estimation in iteration: ', num2str(nu)]);
0104         estimated_x = estx_r'                                               %#ok<NOPRT,NASGU>
0105     end
0106  
0107     max(abs(estx_r) ./ sqrt(diag(Cxrxr)));
0108     if max(abs(estx_r) ./ sqrt(diag(Cxrxr))) < T
0109         s = 2;
0110     end
0111  
0112     %% Determine Updates
0113     Omega = 0;
0114     % Omega_c = 0;    % check c'Wc = Omega stimmt
0115     check = zeros(Gc, 1);
0116     for n = 1:Nc
0117         % covariance matrix of observations (normalized)
0118         Clrlr = squeeze(Cr(n, :, :));
0119      
0120         % corrections of reduced observations
0121         delta_l_r = Clrlr * squeeze(B(n, :, :)) * squeeze(W(n, :, :)) * ...
0122             (cg(n, :)'-squeeze(A(n,:,:))' * estx_r) - v_r(n, :)';
0123         ver_r = v_r(n, :)' + delta_l_r;
0124      
0125         %sum of squared residuals
0126         if w_f(n) > 0
0127             vvp_r = ver_r' * inv(Clrlr) * ver_r;                            
0128             Omega = Omega + vvp_r;
0129             residuals(n) = vvp_r;
0130          
0131             % Omega_c = Omega_c + cg(n,:)' * squeeze(W(n,:,:)) * cg(n,:);
0132          
0133             %             % eliminate obsservation by setting w_f=0
0134             %             if vvp_r > 10
0135             %                 w_f(n)=0;
0136             %             end
0137          
0138         end
0139         % weigths = w_f'
0140         % [n, norm(ver_r ./ sqrt(diag(Clrlr)))];
0141         
0142         % updated estimates of both points
0143         estl(n, 1:3) = sugr_ghm_update_vector(estl(n, 1:3)',delta_l_r(1:2))';
0144         estl(n, 4:6) = sugr_ghm_update_vector(estl(n, 4:6)',delta_l_r(3:4))';
0145      
0146     end
0147     sigma_0 = 1;
0148     if R > min_redundancy
0149         sigma_0 = sqrt(Omega / R);
0150     end
0151     if print_option_estimation > 0
0152         R                                                                  %#ok<NOPRT>
0153         sigma_0                                                            %#ok<NOPRT>
0154         %sigma_c = sqrt(Omega_c/R)          % check stimmt
0155     end
0156  
0157     % update estimate of x
0158     estx = sugr_ghm_update_E_Matrix(estx, estx_r);
0159  
0160     % perform check
0161     for n = 1:Nc
0162         E = calc_S(estx(:, 1)) * estx(:, 2:4)';
0163         check(n) = estl(n, 1:3) * E * estl(n, 4:6)';
0164     end
0165  
0166     %% Stop iteration
0167     if s == 2
0168         break
0169     end
0170  
0171 end
0172 %% Evaluation of result ------------------------------
0173 
0174 % determin sigma_0
0175 
0176 if R > min_redundancy
0177     sigma_0 = sqrt(Omega / R);
0178 else
0179     sigma_0 = 1;
0180 end
0181 
0182 % % choose factor
0183 % f = 1;
0184 % if R > min_redundancy
0185 %     f = sigma_0;
0186 % end
0187 
0188 % set output
0189 E = sugr_E_Matrix(estx(:, 1), estx(:, 2:4), Cxrxr);
0190 
0191 
0192 
0193 
0194

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