Description of sugr_estimation_ml_Point_2D_from_Lines

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

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
