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 scene points ')
0032 disp(' (generalizing PCV Alg. 17) ')
0033 disp('------------------------------------------------------------')
0034
0035
0036
0037
0038 N_samples = 50;
0039
0040 disp(strcat('Number of samples : ' , num2str(N_samples)));
0041
0042 N = 50;
0043
0044
0045 disp(strcat('Number of points : ' , num2str(N)));
0046
0047 c = 500;
0048 disp(strcat('Principal distance : ' , num2str(c)));
0049
0050
0051 sigma_X = 0.0001;
0052 sigma_y = c/10000;
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
0057 init_rand = 12;
0058
0059
0060
0061 S = 0.999;
0062 Slo = (1-S)/2;
0063 Sup = 1- Slo;
0064
0065
0066 T = 10^(-10);
0067 maxiter = 20;
0068 factor_p = 100;
0069
0070
0071 init_rand_seed(init_rand);
0072
0073
0074 b_random = 1;
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
0088 R_tilde = calc_Rot_rod([1,2,3]);
0089 K_tilde = diag([c,c,1]);
0090
0091
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
0106 plot_optinon = 2;
0107 print_option = 2;
0108
0109 if N_samples > 1
0110 plot_option = 0;
0111 print_option = 0;
0112 end
0113
0114
0115
0116
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
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
0134 monitor_samples(i,N_samples);
0135
0136 if i==1
0137 plot_option = 2;
0138 end
0139
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
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
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
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
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
0189
0190 if N_samples > 12
0191
0192 disp('Check estimation ')
0193 disp('------------------------')
0194
0195 R = 2*N-11;
0196
0197
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
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
0207 figure('Color','w','Position',[ss(1)/2,ss(2)/2-100,ss(1)/2,ss(2)/2]);
0208
0209
0210
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
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
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);
0232
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);
0246
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);
0256
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