Description of sugr_estimation_ml_Point_2D_from

0001 %% ML estimate of intersection Point_2D from N Lines_2D
0002 %
0003 % [x,sigma_0,R] = sugr_ml_estimate_2D_Point_from_lines(l,xa,T,maxiter)
0004 %
0005 % * l    = struct of lines: lines, uncertain, spherically normalized homogeneous
0006 % * xa   = struct/2-vector, approximate value
0007 % * T    = threshold for iteration
0008 % * maxiter = maximal iteration
0009 %
0010 % * x = struct estimated 2D point
0011 % * sigma0 = estimated sigma0
0012 % * R = redundancy
0013 %
0014 % Wolfgang Förstner
0015 % wfoerstn@uni-bonn.de
0016 %
0017 % See also sugr_Point_2D, sugr_Line_2D, sugr_estimation_ml_Point_2D_from_Lines_Hessian
0018 % sugr_estimation_algebraic_Point_2D_from_Lines, sugr_estimation_geometric_Point_2D_from_Lines
0019 
0020 function [x,sigma_0,R] = ...
0021     sugr_estimation_ml_Point_2D_from_Lines(l,xa,T,maxiter)
0022 
0023 global print_option_estimation
0024 global min_redundancy
0025 
0026 %% Initialization
0027 
0028 lh    = l.h;
0029 lCrr  = l.Crr;
0030 [N,~] = size(lh);                 % number of elements
0031 R = N-2;            % redundancy
0032 if R < 0
0033     return
0034 end;
0035 
0036 estl = lh;                       % initialize estimated observations
0037 w_g  = ones(N,1);                % initial weights for robust estimate
0038 
0039 if isstruct(xa)                  % initiate estx, estimated unknowns
0040     estx = xa.h;
0041 else
0042     estx = xa;
0043 end
0044 
0045 s=0;                             % control variable for iterations
0046 residuals=zeros(N,1);            % intial residuals
0047 
0048 %% Start iteration -------------------------------------
0049 for nu=1:maxiter
0050     if print_option_estimation > 0
0051         sprintf('nu = %2',nu);
0052     end
0053     Cr       = zeros(N,2,2); % reduced covariance matrices
0054     v_r      = zeros(N,2); % reduced residuals
0055     A        = zeros(N,2); % Jacobians c -> x
0056     B        = zeros(N,2); % Jacobians c -> l
0057     W        = zeros(N,1); % Weights of constraints
0058     cg       = zeros(N,1); % constraint's residuals
0059     
0060     N_matrix = zeros(2);   % normal equation matrix
0061     h_vector = zeros(2,1); % right hand sides
0062     
0063     %% Build design matrices
0064     for n=1:N
0065         estl_n = estl(n,:)';                  % get estl
0066         l_n    =   lh(n,:)';                  % get l
0067         Crr_n  = squeeze(lCrr(n,:,:));
0068         %determine cg and Jacobians (checked)
0069         [lr_n,Cr_n,cg_n,atr_n,btr_n] = ...
0070             sugr_ghm_cg_Point_2D_from_Lines(l_n,estl_n,estx,Crr_n);
0071         % Store these
0072         A(n,:)    = atr_n(:);
0073         B(n,:)    = btr_n(:);
0074         Cr(n,:,:) = Cr_n;
0075         v_r(n,:)  = -lr_n';
0076         cg(n)     = cg_n;
0077         
0078         % weight W_g_gamma of contraint
0079         bCovb_n = btr_n * Cr_n * btr_n';
0080         W(n)    = 1 / bCovb_n * w_g(n);
0081         aW      = atr_n' * W(n);
0082         
0083         N_matrix = N_matrix + aW * atr_n;
0084         h_vector = h_vector + aW * cg_n;
0085         
0086     end
0087     
0088     det(N_matrix);
0089     %% Solve
0090     Cxrxr    = inv(N_matrix);
0091     estx_r   = Cxrxr * h_vector;                                           %#ok<*MINV>
0092     
0093     if print_option_estimation > 1
0094         disp(['Result of estimation in iteration: ',num2str(nu)]);
0095         %         estx_r;
0096     end
0097     
0098     max(abs(estx_r)./sqrt(diag(Cxrxr)));
0099     if max(abs(estx_r)./sqrt(diag(Cxrxr))) < T
0100         s=2;
0101     end
0102     
0103     %% Determine Updates
0104     Omega = 0;
0105     check=zeros(2,1);
0106     for n=1:N
0107         % covariance matrix of observations (normalized)
0108         Clrlr = squeeze(Cr(n,:,:));
0109         
0110         % corrections of reduced observations
0111         delta_l_r   = Clrlr * B(n,:)' * W(n) * (cg(n)-A(n,:)*estx_r)- v_r(n,:)';
0112         ver_r       = v_r(n,:)' + delta_l_r;
0113         
0114         % sum of squared residuals
0115         if w_g(n) > 0
0116             vvp_r = ver_r' * inv(Clrlr) * ver_r;
0117             Omega = Omega + vvp_r;
0118             residuals(n)=vvp_r;
0119             check=check+A(n,:)'*W(n)*B(n,:)*ver_r;
0120             
0121             % eliminate observation by setting w_g=0
0122             if vvp_r > 10
0123                 w_g(n)=0;
0124             end
0125         end
0126         % updated estimates of observations
0127         estl(n,:) = sugr_ghm_update_vector(estl(n,:)',delta_l_r)';
0128         
0129     end
0130     if print_option_estimation > 0
0131         sigma_0 = sqrt(Omega/R)                                             %#ok<NOPRT,NASGU>
0132     end
0133     %     checkt = check';
0134     
0135     % update estimate of x
0136     %     estx0=estx;
0137     %     estx = sugr_ghm_update_vector(estx,estx_r);
0138     %     check_Atpv = (estl*estx.*w_g)';
0139     
0140     %% Stop iteration
0141     if s == 2
0142         break
0143     end
0144     
0145 end
0146 %% Evaluation of result ------------------------------
0147 % determine Cxx
0148 Cxx = null(estx') * Cxrxr * null(estx')';
0149 
0150 % determin sigma_0
0151 % sigma_0=1;
0152 
0153 if R > 0
0154     sigma_0 = sqrt(Omega/R);
0155 else
0156     sigma_0 = 1;
0157 end
0158 % residuals';
0159 
0160 % choose factor
0161 f = 1;
0162 if R > min_redundancy
0163     f = sigma_0;
0164 end
0165 
0166 % estimated covariance matrix of estx
0167 estCxx = f^2 * Cxx;
0168 
0169 % set output
0170 x = sugr_Point_2D(estx,estCxx);
0171 
0172 
0173
sugr_estimation_ml_Point_2D_from_Lines

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
