Description of sugr_estimation_ml_Plane_from

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

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
