Home > General-Functions > SUGR > Projection_3D_2D > sugr_estimation_ml_Projection_3D_2D_from_point_pairs.m

sugr_estimation_ml_Projection_3D_2D_from_point_pairs

PURPOSE ^

% ML estimate of Projection from corresponding 3D-2D point pairs

SYNOPSIS ^

function [P,sigma_0,R] = sugr_estimation_ml_Projection_3D_2D_from_point_pairs(X,y,xa,T,maxiter)

DESCRIPTION ^

% ML estimate of Projection from corresponding 3D-2D point pairs
 scene points 

 [P,sigma_0,R] = sugr_estimate_ml_Projection3D_2D_from_point_pairs(X,y,xa,T,maxiter)

 * X    = struct of points X.h = Nx4 matrix, X.Crr = Nx3x3 field of CovM
 * y    = struct of points y.h = Nx3 matrix, y.Crr = Nx2x2 field of CovM
 * xa   = struct/3x4-matrix, approximate value
 * T    = threshold for iteration
 * maxiter = maximal iteration

 * P = struct estimated homography
 * sigma0 = estimated sigma0
 * R = redundancy

 see Algorithm 17, p. 499,
 however, internally performing conditioning/unconditioning

 Wolfgang Förstner 6/2017
 wfoerstn@uni-bonn.de

 See also sugr_estimation_algebraic_Projection_3D_2D_from_point_pairs
 sugr_ghm_cg_Projection_3D_2D_from_point_pairs

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %% ML estimate of Projection from corresponding 3D-2D point pairs
0002 % scene points
0003 %
0004 % [P,sigma_0,R] = sugr_estimate_ml_Projection3D_2D_from_point_pairs(X,y,xa,T,maxiter)
0005 %
0006 % * X    = struct of points X.h = Nx4 matrix, X.Crr = Nx3x3 field of CovM
0007 % * y    = struct of points y.h = Nx3 matrix, y.Crr = Nx2x2 field of CovM
0008 % * xa   = struct/3x4-matrix, approximate value
0009 % * T    = threshold for iteration
0010 % * maxiter = maximal iteration
0011 %
0012 % * P = struct estimated homography
0013 % * sigma0 = estimated sigma0
0014 % * R = redundancy
0015 %
0016 % see Algorithm 17, p. 499,
0017 % however, internally performing conditioning/unconditioning
0018 %
0019 % Wolfgang Förstner 6/2017
0020 % wfoerstn@uni-bonn.de
0021 %
0022 % See also sugr_estimation_algebraic_Projection_3D_2D_from_point_pairs
0023 % sugr_ghm_cg_Projection_3D_2D_from_point_pairs
0024 
0025 
0026 function [P,sigma_0,R] = sugr_estimation_ml_Projection_3D_2D_from_point_pairs(X,y,xa,T,maxiter)
0027 
0028 global print_option_estimation
0029 global plot_option
0030 
0031 %% Initialization
0032 U   = 11;           % number of unknown parameters
0033 % Nl  = 6;            % number of parameters per observational group (4+2)
0034 Nlr = 5;            % number of reduced parameters per observational group
0035 Gc  = 2;            % number of constraints per observational group
0036 Nc  = size(X.e,1);  % number of pairs
0037 
0038 %% condition points and approximate projection matrix
0039 
0040 % condition
0041 [Xc,MX] = sugr_condition_Points(X);
0042 [ye,My] = sugr_condition_Points(y);
0043 % [Xc.h,10^6*reshape(Xc.Crr,Nc,9)];
0044 % [ye.e,10^6*reshape(ye.Cee,Nc,4)];
0045 
0046 
0047 % conditioning_MX=MX
0048 % conditioning_My=My
0049 Pc              = condition_Homography(reshape(xa,3,4),My,MX);
0050 
0051 % % check projection
0052 %  xs = (Pc*Xc.h')';
0053 %  xse = xs(:,1:2)./(xs(:,3)*ones(1,2));
0054 %  yse = ye.e;
0055 %  [xse-yse]
0056 
0057 Pc = Pc/norm(Pc(:));
0058 xa = Pc(:);
0059 
0060 %% provide data for estimation
0061 lh = zeros(Nc,6);
0062 lCrr = zeros(Nc,5,5);
0063 for n = 1:Nc
0064     lh(n,:)     = [Xc.h(n,:),ye.e(n,:)];
0065     lCrr(n,:,:) = [squeeze(Xc.Crr(n,:,:)) zeros(3,2); ...
0066         zeros(2,3) squeeze(ye.Cee(n,:,:))];
0067 end
0068 
0069 G      = Nc * Gc;       % number of constraints
0070 R      = G - U;         % redundancy
0071 if R < 0
0072     disp('redundancy negative')
0073     return
0074 end;
0075 
0076 nu=0;                            % Initiate iterations
0077 estl = lh;                       % initialize estimated observations
0078 w_f  = ones(Nc,1);               % initial weights for robust estimate
0079 
0080 if isstruct(xa)                  % initiate estx, estimated unknowns
0081     estx = xa.P(:);
0082 else
0083     estx = xa(:);
0084 end
0085 
0086 
0087 s=0;                             % control variable for iterations
0088 residuals=zeros(Nc,Gc);          % intial residuals
0089 
0090 
0091 %% Start iteration -------------------------------------
0092 for nu=1:maxiter
0093     if print_option_estimation > 0
0094         sprintf('nu = %2',nu);
0095     end
0096     Cr       = zeros(Nc,Nlr,Nlr);    % reduced covariance matrices
0097     v_r      = zeros(Nc,Nlr);        % reduced residuals
0098     A        = zeros(Nc,Gc,U);       % Jacobians c -> x
0099     B        = zeros(Nc,Gc,Nlr);     % Jacobians c -> l
0100     W        = zeros(Nc,Gc,Gc);      % Weights of constraints
0101     cg       = zeros(Nc,Gc);         % constraint's residuals
0102     
0103     N_matrix = zeros(U);   % normal equation matrix
0104     h_vector = zeros(U,1); % right hand sides
0105     
0106     %% Build design matrices
0107     for n=1:Nc
0108         estl_n = estl(n,:)';                 % get estl
0109         l_n    = lh(n,:)';                   % get l
0110         Crr_n  = squeeze(lCrr(n,:,:));
0111         % determine cg and Jacobians (checked)
0112         [lr_n,Cr_n,cg_n,atr_n,btr_n] = ...
0113             sugr_ghm_cg_Projection_3D_2D_from_point_pairs(l_n,estl_n,estx,Crr_n);
0114         %cg_n'
0115         
0116         % Store these
0117         A(n,:,:)    = atr_n;                 % Gc x U
0118         B(n,:,:)    = btr_n;                 % Gc x Nlr
0119         Cr(n,:,:)   = Cr_n;                  % Nlr x Nlr
0120         v_r(n,:)    = -lr_n';                % 1 x Nlr
0121         cg(n,:)     = cg_n';                 % 1 x Gc
0122         
0123         % weight of contraint
0124         bCovb_n  = btr_n * Cr_n * btr_n';      % Gc x Gc
0125         W(n,:,:) = inv(bCovb_n) * w_f(n);      %#ok<MINV> % Gc x Gc
0126         aW       = atr_n' * squeeze(W(n,:,:)); % U x Gc
0127         
0128         N_matrix = N_matrix + aW * atr_n;    % U x U
0129         h_vector = h_vector + aW * cg_n;     % U x 1
0130         
0131     end
0132     
0133     det(N_matrix);
0134     log_ev=log(abs(eig(N_matrix)));
0135     %% Solve
0136     Cxrxr    = inv(N_matrix);
0137     estx_r   = Cxrxr * h_vector;                                           %#ok<MINV>
0138     
0139     if print_option_estimation > 0
0140         disp(['Result of estimation in iteration: ',num2str(nu)]);
0141         estimated_corr=estx_r'                                             %#ok<NOPRT,NASGU>
0142     end
0143     
0144 %     iter_converg=[nu,max(abs(estx_r)./sqrt(diag(Cxrxr)))];
0145     
0146     if print_option_estimation > 0
0147         iter_maxl_minl_cond_dx=...
0148             [nu,max(log_ev),min(log_ev),...
0149             max(log_ev)-min(log_ev),max(abs(estx_r)./sqrt(diag(Cxrxr)))]   %#ok<NOPRT,NASGU>
0150     end
0151 %     conv(nu)=max(abs(estx_r)./sqrt(diag(Cxrxr)));
0152     
0153     if max(abs(estx_r)./sqrt(diag(Cxrxr))) < T
0154         s=2;
0155     end
0156     
0157     %% Determine Updates
0158     Omega = 0;
0159     check = zeros(Gc,1);
0160     for n=1:Nc
0161         % covariance matrix of observations (normalized)
0162         Clrlr = squeeze(Cr(n,:,:));
0163         
0164         % corrections of reduced observations
0165         delta_l_r   = Clrlr * squeeze(B(n,:,:))' * squeeze(W(n,:,:)) ...
0166             * (cg(n,:)'-squeeze(A(n,:,:))*estx_r)- v_r(n,:)';
0167         ver_r       = v_r(n,:)' + delta_l_r;
0168         
0169         % sum of squared residuals
0170         if w_f(n) > 0
0171             vvp_r = ver_r' * inv(Clrlr) * ver_r;                           %#ok<MINV>
0172             Omega = Omega + vvp_r;
0173             residuals(n)=vvp_r;
0174             
0175             % eliminate observation by setting w_f=0 (robust version)
0176             %             if vvp_r > 10
0177             %                 w_f(n)=0;
0178             %             end
0179         end
0180         % updated estimates of both points
0181         estl(n,1:4) = sugr_ghm_update_vector(estl(n,1:4)',delta_l_r(1:3))';
0182         estl(n,5:6) = estl(n,5:6) + delta_l_r(4:5)';
0183         
0184     end
0185     
0186     sigma_0 = 1;
0187     if R > 0
0188         sigma_0 = sqrt(Omega/R);
0189     end
0190     if print_option_estimation > 0
0191         sigma_0                                                            %#ok<NOPRT>
0192     end
0193     
0194     % update estimate of x
0195 %     estx0 = estx;
0196     estx  = sugr_ghm_update_vector(estx,estx_r);
0197     %estx'
0198     % check
0199     for n=1:Nc
0200         Jl = [null(estl(n,1:4))  zeros(4,2)       ; ...
0201             zeros(2,3)         eye(2)];
0202         check = check ...
0203             + squeeze(A(n,:,:))*null(estx')'*estx(:) ...
0204             + squeeze(B(n,:,:))*Jl'*estl(n,:)';
0205     end
0206 %     check_cg = check';
0207     
0208     
0209     %% Stop iteration
0210     if s == 2
0211         break
0212     end
0213     
0214 end
0215 
0216 if plot_option == -1
0217     figure(2)
0218     hold on
0219     plot(1:nu,log(conv),'bo-')
0220 end
0221 %% Evaluation of result ------------------------------
0222 
0223 % log_condition_number_conditioned=...
0224 %     max(log_ev)-min(log_ev);
0225 
0226 % determine Cxx
0227 Jrh = null(estx');
0228 Cxx= Jrh * Cxrxr * Jrh';                                                   %#ok<MINV>
0229 
0230 % determine sigma_0
0231 
0232 if R > 0
0233     sigma_0 = sqrt(Omega/R);
0234 else
0235     sigma_0 = 1;
0236 end
0237 
0238 % set output, still conditioned
0239 Pce = reshape(estx,3,4);
0240 Pc  = sugr_Projection_3D_2D(Pce,Cxx);
0241 
0242 %% uncondition projection matrix
0243 P = sugr_uncondition_Projection(Pc,My,MX);
0244

Generated on Mon 08-Jan-2018 17:21:49 by m2html © 2005