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

sugr_estimation_ml_Projection_3D_2D_from_point_pairs_hnh

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_hnh(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_hnh(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

 Wolfgang Förstner 4/2018
 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_hnh(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 internally performing conditioning/unconditioning
0017 %
0018 % Wolfgang Förstner 4/2018
0019 % wfoerstn@uni-bonn.de
0020 %
0021 % See also sugr_estimation_algebraic_Projection_3D_2D_from_point_pairs
0022 % sugr_ghm_cg_Projection_3D_2D_from_point_pairs
0023 
0024 
0025 function [P,sigma_0,R] = sugr_estimation_ml_Projection_3D_2D_from_point_pairs_hnh(X,y,xa,T,maxiter)
0026 
0027 global print_option_estimation
0028 global plot_option
0029 
0030 %% Initialization
0031 U   = 11;           % number of unknown parameters
0032 Ng  = 2;            % number of parameters per observational group
0033 Nc  = size(X.e,1);  % number of pairs = number of constraints
0034 
0035 %% condition points and approximate projection matrix
0036 
0037 % condition
0038 [Xe,MX] = sugr_condition_Points(X);
0039 [ye,My] = sugr_condition_Points(y);
0040 
0041 
0042 % conditioning_MX=MX
0043 % conditioning_My=My
0044 Pc              = condition_Homography(reshape(xa,3,4),My,MX);
0045 
0046 % % check projection
0047 %  xs = (Pc*Xc.h')';
0048 %  xse = xs(:,1:2)./(xs(:,3)*ones(1,2));
0049 %  yse = ye.e;
0050 %  [xse-yse]
0051 
0052 Pc = Pc/norm(Pc(:));
0053 xa = Pc(:);
0054 
0055 %% provide data for estimation
0056 le = zeros(Nc,2);
0057 lCee = zeros(Nc,2,2);
0058 for n = 1:Nc
0059     le(n,:)     = ye.e(n,:);
0060     lCee(n,:,:) = squeeze(ye.Cee(n,:,:));
0061 end
0062 
0063 N      = Nc * Ng;       % number of observations
0064 R      = N - U;         % redundancy
0065 if R < 0
0066     disp('redundancy negative')
0067     return
0068 end;
0069 
0070 nu=0;                            % Initiate iterations
0071 estl = le;                       % initialize estimated observations
0072 
0073 if isstruct(xa)                  % initiate estx, estimated unknowns
0074     estx = xa.P(:);
0075 else
0076     estx = xa(:);
0077 end
0078 
0079 
0080 s=0;                             % control variable for iterations
0081 residuals=zeros(Nc,Ng);          % intial residuals
0082 
0083 
0084 %% Start iteration -------------------------------------
0085 for nu=1:maxiter
0086     if print_option_estimation > 0
0087         sprintf('nu = %2',nu);
0088     end
0089     C        = zeros(Nc,Ng,Ng);      % covariance matrices
0090     v        = zeros(Nc,Ng);         % residuals
0091     A        = zeros(Nc,Ng,U);       % Jacobians c -> x
0092     W        = zeros(Nc,Ng,Ng);      % Weights of constraints
0093     dl       = zeros(Nc,Ng);         % linearized observations
0094     
0095     N_matrix = zeros(U);   % normal equation matrix
0096     h_vector = zeros(U,1); % right hand sides
0097     
0098     %% Build design matrices
0099     % 12x11 Jacobian for xa
0100     Jr     = null(xa');
0101     
0102     for n=1:Nc
0103         l_n    = le(n,:)';                   % get l
0104         C_n    = squeeze(lCee(n,:,:));       % get CovM(l)
0105         % approximate fitted point homogeneous/non-homogeneous
0106         est_lh_n   = reshape(estx,3,4)*[Xe.e(n,:)';1];
0107         est_l_n    = est_lh_n(1:2)/est_lh_n(3);
0108         % determine dl
0109         dl_n   = l_n - est_l_n;       
0110         % 2x3 Jacobian for c(x)
0111         Jc_n     = [est_lh_n(3)*eye(2), -est_lh_n(1:2)]/est_lh_n(3)^2;
0112         % 2x11 design matrix for n-th observation
0113         atr_n  = Jc_n*kron([Xe.e(n,:),1],eye(3))*Jr;
0114 
0115         % Store these
0116         A(n,:,:)    = atr_n;                 % Ng x U
0117         C(n,:,:)    = C_n;                   % Ng x Ng
0118         dl(n,:)     = dl_n';                 %  1 x Ng
0119         
0120         % weight of contraint
0121         W(n,:,:) = inv(C_n);                   % Nc x Nc
0122         aW       = atr_n' * squeeze(W(n,:,:)); %  U x Ng
0123         
0124         % normal equation system
0125         N_matrix = N_matrix + aW * atr_n;    % U x U
0126         h_vector = h_vector + aW * dl_n;     % U x 1
0127         
0128     end
0129     
0130             det(N_matrix);
0131             log_ev=log(abs(eig(N_matrix)));
0132     %% Solve for reduced parameters of the projection matrix
0133     Cxrxr    = inv(N_matrix);
0134     estx_r   = Cxrxr * h_vector;                                           %#ok<MINV>
0135     
0136             if print_option_estimation > 0
0137                 disp(['Result of estimation in iteration: ',num2str(nu)]);
0138                 estimated_corr=estx_r'  
0139                 iter_maxl_minl_cond_dx=...
0140                     [nu,max(log_ev),min(log_ev),...
0141                     max(log_ev)-min(log_ev),max(abs(estx_r)./sqrt(diag(Cxrxr)))]   %#ok<NOPRT,NASGU>
0142             end
0143     % check for convergence
0144     if max(abs(estx_r)./sqrt(diag(Cxrxr))) < T
0145         s=2;
0146     end
0147     
0148     %% Determine Updates
0149     Omega = 0;
0150     for n=1:Nc
0151         % covariance matrix of observations (normalized)
0152         C_n = squeeze(C(n,:,:));
0153         % fitted observation
0154         est_lh_n   = reshape(estx,3,4)*[Xe.e(n,:)';1];
0155         est_le_n   = est_lh_n(1:2)/est_lh_n(3);
0156         % residuals
0157         ver        = est_le_n - le(n,:)';
0158         % sum of squared residuals
0159         vvp = ver' * inv(C_n) * ver;                           %#ok<MINV>
0160         Omega = Omega + vvp;
0161         residuals(n)=vvp;
0162     end
0163     
0164     sigma_0 = 1;
0165     if R > 0
0166         sigma_0 = sqrt(Omega/R);
0167     end
0168     if print_option_estimation > 0
0169         sigma_0                                                            %#ok<NOPRT>
0170     end
0171     
0172     % update estimate of x
0173 %     estx0 = estx;
0174     estx  = sugr_ghm_update_vector(estx,estx_r);
0175     %estx'
0176     % check A' W v = 0
0177     check = zeros(U,1);
0178     for n=1:Nc
0179         check = check ...
0180             + squeeze(A(n,:,:))'*inv( squeeze(C(n,:,:)))*dl(n,:)';
0181     end
0182     check_cg = check';
0183     
0184     
0185     %% Stop iteration
0186     if s == 2
0187         break
0188     end
0189     
0190 end
0191 
0192 if plot_option == -1
0193     figure(2)
0194     hold on
0195     plot(1:nu,log(conv),'bo-')
0196 end
0197 %% Evaluation of result ------------------------------
0198 
0199 % log_condition_number_conditioned=...
0200 %     max(log_ev)-min(log_ev);
0201 
0202 % determine Cxx
0203 Jrh = null(estx');
0204 Cxx= Jrh * Cxrxr * Jrh';                                                   %#ok<MINV>
0205 
0206 % determine sigma_0
0207 
0208 if R > 0
0209     sigma_0 = sqrt(Omega/R);
0210 else
0211     sigma_0 = 1;
0212 end
0213 
0214 % set output, still conditioned
0215 Pce = reshape(estx,3,4);
0216 Pc  = sugr_Projection_3D_2D(Pce,Cxx);
0217 
0218 %% uncondition projection matrix
0219 P = sugr_uncondition_Projection(Pc,My,MX);
0220

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