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