Description of sugr_estimation_ml_E_Matrix_from_point

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

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
