0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
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
0036
0037 N_samples = 344;
0038
0039
0040
0041 disp(strcat('Number of samples : ' , num2str(N_samples)));
0042
0043
0044 N = 15;
0045
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;
0052 disp(strcat('Std. dev. of image points [pixel] : ' , num2str(sigma_y)));
0053
0054
0055 init_rand = 0;
0056
0057
0058
0059 S = 0.999;
0060 Slo = (1-S)/2;
0061 Sup = 1- Slo;
0062
0063
0064 T = 10^(-10);
0065 maxiter = 3;
0066 factor_p = 100;
0067
0068
0069 init_rand_seed(init_rand);
0070
0071
0072 b_random = 1;
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
0086 R_tilde = calc_Rot_rod([1,2,3]);
0087 K_tilde = diag([c,c,1]);
0088
0089
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
0104 plot_optinon = 2;
0105 print_option = 2;
0106
0107 if N_samples > 1
0108 plot_option = 0;
0109 print_option = 0;
0110 end
0111
0112
0113
0114
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
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
0132 monitor_samples(i,N_samples);
0133
0134 if i==1
0135 plot_option = 2;
0136 end
0137
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
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
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
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
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
0191
0192 if N_samples > 12
0193
0194 disp('Check estimation ')
0195 disp('------------------------')
0196
0197 R = 2*N-11;
0198
0199
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
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
0209 figure('Color','w','Position',[ss(1)/2,ss(2)/2-100,ss(1)/2,ss(2)/2]);
0210
0211
0212
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
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
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);
0234
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);
0248
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);
0258
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