0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018 function [H, sigma_0, R] = sugr_estimation_ml_Homography_2D_from_point_pairs(l, xa, T, maxiter)
0019
0020 global print_option_estimation
0021
0022
0023 U = 8;
0024 Nlr = 4;
0025 Gc = 2;
0026
0027 lh = l.h;
0028 lCrr = l.Crr;
0029 Nc = size(lh, 1);
0030
0031 G = Nc * Gc;
0032 R = G - U;
0033 if R < 0
0034 return
0035 end
0036
0037 estl = lh;
0038 w_f = ones(Nc, 1);
0039
0040 if isstruct(xa)
0041 estx = xa.H;
0042 else
0043 estx = xa;
0044 end
0045
0046 s = 0;
0047 residuals = zeros(Nc, 2);
0048
0049
0050 for nu = 1:maxiter
0051 if print_option_estimation > 0
0052 sprintf('nu = %2', nu);
0053 end
0054 Cr = zeros(Nc, Nlr, Nlr);
0055 v_r = zeros(Nc, Nlr);
0056 A = zeros(Nc, Gc, U);
0057 B = zeros(Nc, Gc, Nlr);
0058 W = zeros(Nc, Gc, Gc);
0059 cg = zeros(Nc, Gc);
0060
0061 N_matrix = zeros(U);
0062 h_vector = zeros(U, 1);
0063
0064
0065 for n = 1:Nc
0066 estl_n = estl(n, :)';
0067 l_n = lh(n, :)';
0068 Crr_n = squeeze(lCrr(n, :, :));
0069
0070 [lr_n, Cr_n, cg_n, atr_n, btr_n] = sugr_ghm_cg_2D_homography_from_point_pairs(l_n, estl_n, estx, Crr_n);
0071
0072 A(n, :, :) = atr_n;
0073 B(n, :, :) = btr_n;
0074 Cr(n, :, :) = Cr_n;
0075 v_r(n, :) = - lr_n';
0076 cg(n, :) = cg_n';
0077
0078
0079 bCovb_n = btr_n * Cr_n * btr_n';
0080 W(n, :, :) = inv(bCovb_n) * w_f(n);
0081 aW = atr_n' * squeeze(W(n,:,:));
0082
0083 N_matrix = N_matrix + aW * atr_n;
0084 h_vector = h_vector + aW * cg_n;
0085
0086 end
0087
0088
0089
0090 Cxrxr = inv(N_matrix);
0091 estx_r = Cxrxr * h_vector;
0092
0093 if print_option_estimation > 0
0094 disp(['Result of estimation in iteration: ', num2str(nu)]);
0095 disp(estx_r);
0096 end
0097
0098
0099 if max(abs(estx_r) ./ sqrt(diag(Cxrxr))) < T
0100 s = 2;
0101 end
0102
0103
0104 Omega = 0;
0105 check = zeros(Gc, 1);
0106 for n = 1:Nc
0107
0108 Clrlr = squeeze(Cr(n, :, :));
0109
0110
0111 delta_l_r = Clrlr * squeeze(B(n, :, :))' * squeeze(W(n,:,:)) * (cg(n,:)' - squeeze(A(n, :, :)) * estx_r) - v_r(n, :)';
0112 ver_r = v_r(n, :)' + delta_l_r;
0113
0114
0115 if w_f(n) > 0
0116 vvp_r = ver_r' * inv(Clrlr) * ver_r;
0117 Omega = Omega + vvp_r;
0118 residuals(n) = vvp_r;
0119
0120
0121 if vvp_r > 10
0122 w_f(n) = 0;
0123 end
0124 end
0125
0126 estl(n, 1:3) = sugr_ghm_update_vector(estl(n, 1:3)',delta_l_r(1:2))';
0127 estl(n, 4:6) = sugr_ghm_update_vector(estl(n, 4:6)',delta_l_r(3:4))';
0128
0129 end
0130 sigma_0 = 1;
0131 if R > 0
0132 sigma_0 = sqrt(Omega / R);
0133 end
0134 if print_option_estimation > 0
0135 disp(sigma_0)
0136 end
0137 if print_option_estimation > 1
0138 disp(check')
0139 end
0140
0141
0142 if print_option_estimation > 1
0143 disp(estx)
0144 end
0145
0146 estx = sugr_ghm_update_homography(estx, estx_r);
0147 for n = 1:Nc
0148 Jl = [null(estl(n, 1:3)) zeros(3, 2); zeros(3, 2) null(estl(n, 4:6))];
0149 [~, Jhr] = sugr_minimal_Homography_2D(estx, zeros(9));
0150 check = check ...
0151 + squeeze(A(n, :, :)) * Jhr * estx(:) ...
0152 + squeeze(B(n, :, :)) * Jl'*estl(n,:)';
0153 end
0154 if print_option_estimation > 1
0155 disp(check)
0156 end
0157
0158
0159 if s == 2
0160 break
0161 end
0162
0163 end
0164
0165
0166
0167
0168 [~, Jrh] = sugr_get_CovM_homogeneous_Homography_2D(sugr_Homography_2D(estx, zeros(9)));
0169 Cxx = Jrh * Cxrxr * Jrh';
0170
0171
0172 if R > 0
0173 sigma_0 = sqrt(Omega / R);
0174 else
0175 sigma_0 = 1;
0176 end
0177 if print_option_estimation > 1
0178 disp(residuals');
0179 end
0180
0181
0182
0183
0184
0185
0186
0187
0188 H = sugr_Homography_2D(estx, Cxx);
0189
0190