Description of demo_ALG_17_estimate_Projection_3D_2D_from_point

0001 %% Demo estimate 3D projection from 2D point pairs, ALG 17
0002 %
0003 % Model y = c(PX), perspective camera
0004 %
0005 % PCV, Algorithm 17, including conditioning
0006 %
0007 % uncertain non-homogeneous coordinates for image points y,
0008 % certain homogeneous coordinates for scene points X
0009 %
0010 % simulation: 3D points X around origin
0011 %
0012 % Wolfgang Förstner 4/2018
0013 % wfoerstn@uni-bonn.de
0014 %
0015 
0016 clc
0017 close all
0018 clearvars
0019 
0020 addpath(genpath('../../General-Functions'))
0021 
0022 global plot_option
0023 global print_option
0024 global print_option_estimation
0025 
0026 sugr_INIT
0027 ss = plot_init;
0028 
0029 disp('==========================================================')
0030 disp(' Demo estimation of projection matrix, perspective model  ')
0031 disp('      with uncertain image and certain scene points       ')
0032 disp('                    (PCV Alg. 17)                         ')
0033 disp('----------------------------------------------------------')
0034 
0035 %% control parameters
0036 % for testing the implementation
0037 N_samples     = 344;
0038 %N_samples     = 200;
0039 %N_samples     = 50;
0040 %N_samples     = 1;
0041 disp(strcat('Number of samples                   : ' , num2str(N_samples)));
0042 % number of points
0043 %N              = 50;
0044 N              = 15;
0045 %N             = 7;
0046 disp(strcat('Number of points                    : ' , num2str(N)));
0047 
0048 c = 500;
0049 disp(strcat('Principal distance                  : ' , num2str(c)));
0050 
0051 sigma_y       = c/10000;                  % std. dev. in pixels
0052 disp(strcat('Std. dev. of image points [pixel]   : ' , num2str(sigma_y)));
0053 
0054 % random number intialization
0055  init_rand      = 0;                  % random seed
0056 %init_rand      = 12;                  % random seed
0057 
0058 % Significance level for tests
0059 S   = 0.999;
0060 Slo = (1-S)/2;
0061 Sup = 1- Slo;
0062 
0063 % Threshold for iteration
0064 T        = 10^(-10);
0065 maxiter  = 3;
0066 factor_p = 100;                  % for plot
0067 
0068 %% random number intialization
0069 init_rand_seed(init_rand);
0070 
0071 %% Generate points in box
0072 b_random      = 1;                        % random points in [-1,+1]^2
0073 
0074 if b_random == 1
0075     dX            = 1;
0076     dY            = 1;
0077     dZ            = 0.5;
0078     Z0            = 3.0;
0079     Z_tilde   = [dX/2;dY/2;Z0];
0080 else
0081     dX=0;dY=0;dZ=0;
0082     Z_tilde   = [0;0;3];
0083 end
0084 
0085 % set R and K
0086 R_tilde       = calc_Rot_rod([1,2,3]);
0087 K_tilde       = diag([c,c,1]);
0088 
0089 % true projection
0090 P_tilde         = sugr_Projection_3D_2D(K_tilde,R_tilde,Z_tilde);
0091 
0092 disp('true_Projection');
0093 disp(num2str(P_tilde.P));
0094 disp(' ')
0095 
0096 true_p          = P_tilde.P(:);
0097 
0098 print_option_estimation = 1;
0099 if N_samples > 1
0100     print_option_estimation = 0;
0101 end
0102 
0103 % generally do not plot samples
0104 plot_optinon = 2;
0105 print_option = 2;
0106 % except for one sample
0107 if N_samples > 1
0108     plot_option = 0;
0109     print_option = 0;
0110 end
0111 
0112 %% calculate samples
0113 
0114 % generate true points, common to all samples
0115 [Xt,yt] = sugr_generate_true_2D_point_pairs_Projection_3D_2D...
0116     (P_tilde.P,N,dX,dY,dZ,b_random);
0117 
0118 X_coord  = Xt.e;
0119 xs_coord = yt.e;
0120 
0121 % set arrays for evaluation
0122 est_P_samples_a_r  = zeros(N_samples,11);
0123 est_P_samples_ml_r = zeros(N_samples,11);
0124 est_s0q_samples_ml = zeros(N_samples,1);
0125 est_bias_a_r       = zeros(N_samples,1);
0126 est_bias_ml_r      = zeros(N_samples,1);
0127 mean_rel_s         = zeros(N_samples,1);
0128 
0129 start = cputime;
0130 for i = 1:N_samples
0131     % monitor samples
0132     monitor_samples(i,N_samples);
0133     
0134     if i==1
0135         plot_option = 2;                                                   
0136     end
0137     % perturb point pairs (sigma_X is set to 0)
0138     [X,y] = sugr_perturb_3D_2D_point_pairs(Xt,yt,0,sigma_y,factor_p);
0139     set(gcf,'Position',[20,ss(2)/2-100,ss(1)/2,ss(2)/2]);
0140     
0141     X_coord  = X.e;
0142     xs_coord = y.e;
0143     plot_option = 0;
0144     
0145     
0146     %% Estimate point algebraically
0147     est_P_a = sugr_estimation_algebraic_Projection_3D_2D_from_point_pairs(X,y);
0148     P_est_algebraically = est_P_a.P;
0149     P_true = P_tilde.P;
0150     
0151     %% Estimate point maximum likelihood
0152     [est_P_ml,sigma_0,~] = ...
0153         sugr_estimation_ml_Projection_3D_2D_from_point_pairs_hnh...
0154         (X,y,P_tilde.P,T,maxiter);
0155     
0156     % store sigma_0^2
0157     est_s0q_samples_ml(i) = sigma_0^2;
0158     est_P_ml.P;
0159     P_tilde.P;
0160     
0161     eigv                = abs(eig(est_P_ml.Crr));
0162     condition_number    = max(eigv)/min(eigv);
0163     mean_rel_std        = sqrt(trace(inv(est_P_ml.Crr)*est_P_a.Crr)/11);    
0164     mean_rel_s(i)       = mean_rel_std;
0165     
0166     [Ke,Re,Ze] = calc_KRZ_from_P(est_P_ml.P,1);
0167     [K,R,Z]    = calc_KRZ_from_P(P_tilde.P,1);
0168     
0169     if print_option > 0
0170         disp(['K_Diff = ']) 
0171         disp([inv(K)*Ke-eye(3)])                              
0172         disp(['R_Diff = '])                              
0173         disp([inv(R)*Re-eye(3)])    
0174         disp(['Z_Diff = '])     
0175         disp([Z-Ze]) 
0176     end
0177     
0178     % collect estimated parameters
0179     est_P_samples_a_r(i,:)  = null(true_p')'*est_P_a.P(:);
0180     est_P_samples_ml_r(i,:) = null(true_p')'*est_P_ml.P(:);
0181     est_bias_a_r(i)  = ...
0182         est_P_samples_a_r(i,:)*inv(est_P_a.Crr)*est_P_samples_a_r(i,:)';
0183     est_bias_ml_r(i) = ...
0184         est_P_samples_ml_r(i,:)*inv(est_P_ml.Crr)*est_P_samples_ml_r(i,:)';
0185     
0186 end
0187 disp(' ')
0188 disp(['CPU time for ', num2str(N_samples),' samples              : ',num2str(cputime-start)]);
0189 
0190 %% Check of estimation
0191 
0192 if N_samples > 12
0193     % --------------------------------------------------------------------
0194     disp('Check estimation        ')
0195     disp('------------------------')
0196     
0197     R = 2*N-11;
0198     
0199     % Algebraic
0200     check_estimation_result(R,zeros(11,1),est_P_a.Crr,...
0201         ones(N_samples,1),est_P_samples_a_r,S,' ALG')
0202     
0203     % Maximum likelihood
0204     check_estimation_result(R,zeros(11,1),est_P_ml.Crr,...
0205         est_s0q_samples_ml,est_P_samples_ml_r,S,' ML');
0206     set(gcf,'Position',[ss(1)/3,20,ss(1)/3,ss(2)/3]);
0207     
0208     % show additional information
0209     figure('Color','w','Position',[ss(1)/2,ss(2)/2-100,ss(1)/2,ss(2)/2]);
0210     
0211     % relative accuracy: lambda(empALG/theorML)
0212     % algebraic estimation
0213     mean_p_a_r = mean(est_P_samples_a_r);
0214     covm_p_a_r = cov(est_P_samples_a_r);
0215     CovM_a_theor_r = est_P_a.Crr;
0216     
0217     % ml-estimation
0218     mean_p_ml_r = mean(est_P_samples_ml_r);
0219     covm_p_ml_r = cov(est_P_samples_ml_r);
0220     CovM_ml_theor_r = est_P_ml.Crr;
0221     %
0222     
0223     disp(['Mean rel. std.dev [sqrt(trace(C_a/C_ml)/11)] : ',...
0224         num2str(mean(mean_rel_s))]);
0225     
0226     % plots
0227     N_bin = ceil(sqrt(N_samples));
0228     
0229     subplot(2,2,1)
0230     hold on
0231     hist(est_s0q_samples_ml,N_bin)
0232     [a,r] = hist(est_s0q_samples_ml,N_bin);
0233     range = abs(r(N_bin)-r(1))*N_bin/(N_bin-1);     % range of histogram
0234     % expected number of samples
0235     plot(r,N_bin*range*fpdf(r,2*N-11,100000),'-r','LineWidth',4);
0236     title('estimated variance factor')
0237     
0238     subplot(2,2,2)
0239     hist(mean_rel_s,ceil(sqrt(N_samples)))
0240     title('mean relative std ALG/ML of P')
0241     
0242     subplot(2,2,3)
0243     hold on
0244     N_bin = ceil(sqrt(N_samples));
0245     [aa,r] = hist(est_bias_a_r,N_bin);
0246     hist(est_bias_a_r,N_bin)
0247     range = abs(r(N_bin)-r(1))*N_bin/(N_bin-1);     % range of histogram
0248     % expected number of samples
0249     plot(r,N_bin*range*chi2pdf(r,11),'-r','LineWidth',4);
0250     title('Mahalanobis d. of $P_{\mbox{ALG}}$')
0251     
0252     subplot(2,2,4)
0253     hold on
0254     N_bin = ceil(sqrt(N_samples));
0255     [aml,r] = hist(est_bias_ml_r,N_bin);
0256     hist(est_bias_ml_r,N_bin)
0257     range = abs(r(N_bin)-r(1))*N_bin/(N_bin-1);     % range of histogram
0258     % expected number of samples
0259     plot(r,N_bin*range*chi2pdf(r,11),'-r','LineWidth',4);
0260     title('Mahalanobis d. of $P_{\mbox{ML}}$')  
0261 
0262     
0263 end
0264
demo_ALG_17_estimate_Projection_3D_2D_from_point_pairs

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
