0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026 function [P,sigma_0,R] = sugr_estimation_ml_Projection_3D_2D_from_point_pairs(X,y,xa,T,maxiter)
0027
0028 global print_option_estimation
0029 global plot_option
0030
0031
0032 U = 11;
0033
0034 Nlr = 5;
0035 Gc = 2;
0036 Nc = size(X.e,1);
0037
0038
0039
0040
0041 [Xc,MX] = sugr_condition_Points(X);
0042 [ye,My] = sugr_condition_Points(y);
0043
0044
0045
0046
0047
0048
0049 Pc = condition_Homography(reshape(xa,3,4),My,MX);
0050
0051
0052
0053
0054
0055
0056
0057 Pc = Pc/norm(Pc(:));
0058 xa = Pc(:);
0059
0060
0061 lh = zeros(Nc,6);
0062 lCrr = zeros(Nc,5,5);
0063 for n = 1:Nc
0064 lh(n,:) = [Xc.h(n,:),ye.e(n,:)];
0065 lCrr(n,:,:) = [squeeze(Xc.Crr(n,:,:)) zeros(3,2); ...
0066 zeros(2,3) squeeze(ye.Cee(n,:,:))];
0067 end
0068
0069 G = Nc * Gc;
0070 R = G - U;
0071 if R < 0
0072 disp('redundancy negative')
0073 return
0074 end;
0075
0076 nu=0;
0077 estl = lh;
0078 w_f = ones(Nc,1);
0079
0080 if isstruct(xa)
0081 estx = xa.P(:);
0082 else
0083 estx = xa(:);
0084 end
0085
0086
0087 s=0;
0088 residuals=zeros(Nc,Gc);
0089
0090
0091
0092 for nu=1:maxiter
0093 if print_option_estimation > 0
0094 sprintf('nu = %2',nu);
0095 end
0096 Cr = zeros(Nc,Nlr,Nlr);
0097 v_r = zeros(Nc,Nlr);
0098 A = zeros(Nc,Gc,U);
0099 B = zeros(Nc,Gc,Nlr);
0100 W = zeros(Nc,Gc,Gc);
0101 cg = zeros(Nc,Gc);
0102
0103 N_matrix = zeros(U);
0104 h_vector = zeros(U,1);
0105
0106
0107 for n=1:Nc
0108 estl_n = estl(n,:)';
0109 l_n = lh(n,:)';
0110 Crr_n = squeeze(lCrr(n,:,:));
0111
0112 [lr_n,Cr_n,cg_n,atr_n,btr_n] = ...
0113 sugr_ghm_cg_Projection_3D_2D_from_point_pairs(l_n,estl_n,estx,Crr_n);
0114
0115
0116
0117 A(n,:,:) = atr_n;
0118 B(n,:,:) = btr_n;
0119 Cr(n,:,:) = Cr_n;
0120 v_r(n,:) = -lr_n';
0121 cg(n,:) = cg_n';
0122
0123
0124 bCovb_n = btr_n * Cr_n * btr_n';
0125 W(n,:,:) = inv(bCovb_n) * w_f(n);
0126 aW = atr_n' * squeeze(W(n,:,:));
0127
0128 N_matrix = N_matrix + aW * atr_n;
0129 h_vector = h_vector + aW * cg_n;
0130
0131 end
0132
0133 det(N_matrix);
0134 log_ev=log(abs(eig(N_matrix)));
0135
0136 Cxrxr = inv(N_matrix);
0137 estx_r = Cxrxr * h_vector;
0138
0139 if print_option_estimation > 0
0140 disp(['Result of estimation in iteration: ',num2str(nu)]);
0141 estimated_corr=estx_r'
0142 end
0143
0144
0145
0146 if print_option_estimation > 0
0147 iter_maxl_minl_cond_dx=...
0148 [nu,max(log_ev),min(log_ev),...
0149 max(log_ev)-min(log_ev),max(abs(estx_r)./sqrt(diag(Cxrxr)))]
0150 end
0151
0152
0153 if max(abs(estx_r)./sqrt(diag(Cxrxr))) < T
0154 s=2;
0155 end
0156
0157
0158 Omega = 0;
0159 check = zeros(Gc,1);
0160 for n=1:Nc
0161
0162 Clrlr = squeeze(Cr(n,:,:));
0163
0164
0165 delta_l_r = Clrlr * squeeze(B(n,:,:))' * squeeze(W(n,:,:)) ...
0166 * (cg(n,:)'-squeeze(A(n,:,:))*estx_r)- v_r(n,:)';
0167 ver_r = v_r(n,:)' + delta_l_r;
0168
0169
0170 if w_f(n) > 0
0171 vvp_r = ver_r' * inv(Clrlr) * ver_r;
0172 Omega = Omega + vvp_r;
0173 residuals(n)=vvp_r;
0174
0175
0176
0177
0178
0179 end
0180
0181 estl(n,1:4) = sugr_ghm_update_vector(estl(n,1:4)',delta_l_r(1:3))';
0182 estl(n,5:6) = estl(n,5:6) + delta_l_r(4:5)';
0183
0184 end
0185
0186 sigma_0 = 1;
0187 if R > 0
0188 sigma_0 = sqrt(Omega/R);
0189 end
0190 if print_option_estimation > 0
0191 sigma_0
0192 end
0193
0194
0195
0196 estx = sugr_ghm_update_vector(estx,estx_r);
0197
0198
0199 for n=1:Nc
0200 Jl = [null(estl(n,1:4)) zeros(4,2) ; ...
0201 zeros(2,3) eye(2)];
0202 check = check ...
0203 + squeeze(A(n,:,:))*null(estx')'*estx(:) ...
0204 + squeeze(B(n,:,:))*Jl'*estl(n,:)';
0205 end
0206
0207
0208
0209
0210 if s == 2
0211 break
0212 end
0213
0214 end
0215
0216 if plot_option == -1
0217 figure(2)
0218 hold on
0219 plot(1:nu,log(conv),'bo-')
0220 end
0221
0222
0223
0224
0225
0226
0227 Jrh = null(estx');
0228 Cxx= Jrh * Cxrxr * Jrh';
0229
0230
0231
0232 if R > 0
0233 sigma_0 = sqrt(Omega/R);
0234 else
0235 sigma_0 = 1;
0236 end
0237
0238
0239 Pce = reshape(estx,3,4);
0240 Pc = sugr_Projection_3D_2D(Pce,Cxx);
0241
0242
0243 P = sugr_uncondition_Projection(Pc,My,MX);
0244