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