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

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

 * X    = struct of points X.e = Nx3 matrix, X.Cee = Nx3x3 field of CovM
 * y    = struct of points y.e = Nx3 matrix, y.Cee = 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
 - allowing for uncertain scene coordinates

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

Generated on Sat 21-Jul-2018 20:56:10 by m2html © 2005