Description of demo_estimate_Projection_3D_2D_from_point_pairs

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

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
