Description of sugr_estimation_ml_symmetric_Planes_from

0001 %% ML estiamte of plane from points
0002 %
0003 % [x,sigma_0,R] = sugr_estimation_ml_symmetric_Planes_from_points(XL,XR,sigma,xa,T,maxiter)
0004 %
0005 % * XL, XR  = points,
0006 % * xa      = vector, approximate value
0007 %             xa(1:4),xa(5:8) for left and right plane
0008 % * sigma   = std.dev. of points
0009 % * T       = threshold for iteration
0010 % * maxiter = maximal iteration
0011 %
0012 % * x         x.h estimated planes
0013 %             x.Cxx joint covariance matrix
0014 % * sigma0 = estimated sigma0
0015 % * R = redundancy
0016 %
0017 % Wolfgang Förstner 7/2012
0018 % wfoerstn@uni-bonn.de
0019 
0020 function [x, estl, sigma_0, R] = ...
0021     sugr_estimation_ml_symmetric_Planes_from_points(XL, XR, xa, T, maxiter)
0022 
0023 global print_option_estimation
0024 global min_redundancy
0025 
0026 %% Initialization
0027 
0028 NL = size(XL.h, 1);
0029 NR = size(XR.h, 1);
0030 N = NL + NR;
0031 R = N - 4; % redundancy
0032 if R < 0
0033     return
0034 end;
0035 
0036 lh = [XL.h; XR.h]; % initialize estimated observations
0037 estl = lh;
0038 
0039 lCrr = zeros(N,3,3);
0040 for n = 1:N
0041     if n <= NL
0042         lCrr(n, :, :) = squeeze(XL.Crr(n, :, :));
0043     else
0044         lCrr(n, :, :) = squeeze(XR.Crr(n - NL, :, :));
0045     end
0046 end
0047 w_g = ones(N, 1); % initial weights for robust estimate
0048 
0049 estx = xa;
0050 
0051 s = 0; % control variable for iterations
0052 residuals = zeros(N, 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(N, 3, 3); % reduced covariance matrices
0060     v_r = zeros(N, 3); % reduced residuals
0061     A = zeros(N, 3); % Jacobians c -> x
0062     B = zeros(N, 3); % Jacobians c -> l
0063     W = zeros(N, 1); % Weights of constraints
0064     cg = zeros(N, 1); % constraint's residuals
0065     
0066     N_matrix = zeros(8); % normal equation matrix
0067     h_vector = zeros(8, 1); % right hand sides
0068     
0069     %% Build design matrices
0070     for n = 1:N
0071         
0072         estl_n = estl(n, :)';                  % get estl
0073         l_n = lh(n, :)';                  % get l
0074         Crr_n = squeeze(lCrr(n, :, :));
0075         %determine cg and Jacobians (checked)
0076         
0077         if n <= NL
0078             [lr_n, Cr_n, cg_n, atr_n, btr_n] = ...
0079                 sugr_ghm_cg_Plane_from_points(l_n, estl_n, estx(1:4), Crr_n);
0080         else
0081             [lr_n, Cr_n, cg_n, atr_n, btr_n] = ...
0082                 sugr_ghm_cg_Plane_from_points(l_n, estl_n, estx(5:8), Crr_n);
0083         end
0084         % Store these
0085         A(n, :) = atr_n(:);
0086         B(n, :) = btr_n(:);
0087         Cr(n, :, :) = Cr_n;
0088         v_r(n, :) = - lr_n';
0089         cg(n) = cg_n;
0090         
0091         % weight W_g_gamma of contraint
0092         bCovb_n = btr_n * Cr_n * btr_n';
0093         W(n) = 1 / bCovb_n * w_g(n);
0094         aW = atr_n' * W(n);
0095         if n <= NL
0096             N_matrix(1:3, 1:3) = N_matrix(1:3, 1:3) + aW * atr_n;
0097             h_vector(1:3) = h_vector(1:3) + aW * cg_n;
0098         else
0099             N_matrix(4:6, 4:6) = N_matrix(4:6, 4:6) + aW * atr_n;
0100             h_vector(4:6) = h_vector(4:6) + aW * cg_n;
0101         end
0102     end
0103     % include constraints
0104     factor = trace(N_matrix(1:6, 1:6));
0105     AL0 = estx(1:4);
0106     AR0 = estx(5:8);
0107     H = factor * ...
0108         [2 * [- AL0(1) * AR0(3) ^ 2, ...
0109         - AL0(2) * AR0(3) ^ 2, ...
0110         + AL0(3) * (AR0(1) ^ 2 + AR0(2) ^ 2), ...
0111         0] * null(AL0'),...
0112         2 * [+ AR0(1) * AL0(3) ^ 2, ...
0113         + AR0(2) * AL0(3) ^ 2, ...
0114         - AR0(3) * (AL0(1) ^ 2 + AL0(2) ^ 2), ...
0115         0] * null(AR0');...
0116         [[+ AR0(2), - AR0(1), 0, 0] * null(AL0'),...
0117         [- AL0(2), + AL0(1), 0, 0] * null(AR0')]];
0118 %     Hs = H / factor;
0119     N_matrix(7:8, 1:6) = H;
0120     N_matrix(1:6, 7:8) = H';
0121     h_vector(7:8) = factor * ...
0122         [- (AL0(3) ^ 2 * (AR0(1) ^ 2 + AR0(2) ^ 2) - AR0(3) ^ 2 * (AL0(1) ^ 2 + AL0(2) ^ 2)); ...
0123         - (AL0(1) * AR0(2) - AR0(1) * AL0(2))];
0124 %     N_matrix / factor
0125 %     h_vector'/factor
0126 %     hv78 = h_vector(7:8) / factor
0127 %     N_matrix;
0128 %     h_vector;
0129     
0130     det(N_matrix);
0131     
0132     %% Solve
0133     Cxrxr = inv(N_matrix);
0134     estx_r = Cxrxr * h_vector;                                             %#ok<MINV>
0135     
0136     if print_option_estimation > 1
0137         disp(['Result of estimation in iteration: ', num2str(nu)]);
0138         h_vector'                                                          %#ok<NOPRT>
0139         estx_r'                                                            %#ok<NOPRT>
0140     end
0141     
0142     max(abs(estx_r(1:6)) ./ sqrt(diag(Cxrxr(1:6, 1:6))));
0143     if max(abs(estx_r(1:6)) ./ sqrt(diag(Cxrxr(1:6, 1:6)))) < T
0144         s = 2;
0145     end
0146     
0147     %% Determine Updates
0148     Omega = 0;
0149     check = zeros(3, 1);
0150     for n = 1:N
0151         % covariance matrix of observations (normalized)
0152         Clrlr = squeeze(Cr(n, :, :));
0153         
0154         % corrections of reduced observations
0155         if n <= NL
0156             delta_l_r = Clrlr * B(n, :)' * W(n) * ...
0157                 (cg(n) - A(n, :) * estx_r(1:3)) - v_r(n, :)';
0158         else
0159             delta_l_r = Clrlr * B(n, :)' * W(n) * ...
0160                 (cg(n) - A(n, :) * estx_r(4:6)) - v_r(n, :)';
0161         end
0162         ver_r = v_r(n, :)' + delta_l_r;
0163         
0164         % sum of squared residuals
0165         if w_g(n) > 0
0166             vvp_r = ver_r' * inv(Clrlr) * ver_r;                           %#ok<MINV>
0167             Omega = Omega + vvp_r;
0168             residuals(n) = vvp_r;
0169             check = check + A(n, :)'*W(n)*B(n,:)*ver_r;
0170             
0171             % eliminate observation by setting w_g=0
0172             if vvp_r > 10
0173                 w_g(n) = 0;
0174             end
0175         end
0176         % updated estimates of observations
0177 %         estl0 = estl;
0178         estl(n, :) = sugr_ghm_update_vector(estl(n, :)',delta_l_r)';
0179         
0180     end
0181     if print_option_estimation > 0
0182         sigma_0 = sqrt(Omega / R)                                          %#ok<NOPRT,NASGU>
0183     end
0184 %     checkt = check';
0185     
0186     % update estimate of x
0187 %     estx0 = estx;
0188     estx(1:4) = sugr_ghm_update_vector(estx(1:4), estx_r(1:3));
0189     estx(5:8) = sugr_ghm_update_vector(estx(5:8), estx_r(4:6));
0190     %check_Atpv = (estl*estx.*w_g)';
0191     
0192     %% Stop iteration
0193     if s == 2
0194         break
0195     end
0196     
0197 end
0198 %% Evaluation of result ------------------------------
0199 % determine Cxx
0200 Jrh = [null(estx(1:4)')' zeros(3, 4); zeros(3, 4) null(estx(5:8)')'];
0201 Cxx = Jrh' * Cxrxr(1:6,1:6) * Jrh;
0202 
0203 % determine sigma_0
0204 
0205 if R > 0
0206     sigma_0 = sqrt(Omega / R);
0207 else
0208     sigma_0 = 1;
0209 end
0210 % residuals';
0211 
0212 % choose factor
0213 f = 1;
0214 if R > min_redundancy
0215     f = sigma_0;
0216 end
0217 
0218 % estimated covariance matrix of estx
0219 estCxx = f ^ 2 * Cxx;
0220 
0221 % estx;
0222 % estCxx;
0223 
0224 % set output
0225 x.h = estx;
0226 x.Cxx = estCxx;
0227 
0228
sugr_estimation_ml_symmetric_Planes_from_points

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
