sugr_estimation_ml_Homography_2D_from_point_pairs

% ML estimate of 2D homography from point pairs

 [H,sigma_0,R] = sugr_estimate_ml_Homography_2D_from_point_pairs(l,xa,T,maxiter)

 * l    = struct of point paris
 * xa   = struct/3x3-matrix, approximate value
 * T    = threshold for iteration
 * maxiter = maximal iteration

 * H = struct estimated homography
 * sigma0 = estimated sigma0
 * R = redundancy

 Wolfgang Förstner
 wfoerstn@uni-bonn.de

0001 %% ML estimate of 2D homography from point pairs
0002 %
0003 % [H,sigma_0,R] = sugr_estimate_ml_Homography_2D_from_point_pairs(l,xa,T,maxiter)
0004 %
0005 % * l    = struct of point paris
0006 % * xa   = struct/3x3-matrix, approximate value
0007 % * T    = threshold for iteration
0008 % * maxiter = maximal iteration
0009 %
0010 % * H = struct estimated homography
0011 % * sigma0 = estimated sigma0
0012 % * R = redundancy
0013 %
0014 % Wolfgang Förstner
0015 % wfoerstn@uni-bonn.de
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 %% Initialization
0023 U = 8; % number of unknown parameters
0024 Nlr = 4; % number of reduced parameters per observational group
0025 Gc = 2; % number of constraints per observational group
0026 
0027 lh = l.h;
0028 lCrr = l.Crr;
0029 Nc = size(lh, 1); % number of pairs
0030 
0031 G = Nc * Gc; % number of constraints
0032 R = G - U; % redundancy
0033 if R < 0
0034     return
0035 end
0036 
0037 estl = lh; % initialize estimated observations
0038 w_f = ones(Nc, 1); % initial weights for robust estimate
0039 
0040 if isstruct(xa) % initiate estx, estimated unknowns
0041     estx = xa.H;
0042 else
0043     estx = xa;
0044 end
0045 
0046 s = 0; % control variable for iterations
0047 residuals = zeros(Nc, 2); % intial residuals
0048 
0049 %% Start iteration -------------------------------------
0050 for nu = 1:maxiter
0051     if print_option_estimation > 0
0052         sprintf('nu = %2', nu);
0053     end
0054     Cr = zeros(Nc, Nlr, Nlr); % reduced covariance matrices
0055     v_r = zeros(Nc, Nlr); % reduced residuals
0056     A = zeros(Nc, Gc, U); % Jacobians c -> x
0057     B = zeros(Nc, Gc, Nlr); % Jacobians c -> l
0058     W = zeros(Nc, Gc, Gc); % Weights of constraints
0059     cg = zeros(Nc, Gc); % constraint's residuals
0060     
0061     N_matrix = zeros(U); % normal equation matrix
0062     h_vector = zeros(U, 1); % right hand sides
0063     
0064     %% Build design matrices
0065     for n = 1:Nc
0066         estl_n = estl(n, :)';                 % get estl
0067         l_n = lh(n, :)';                   % get l
0068         Crr_n = squeeze(lCrr(n, :, :));
0069         % determine cg and Jacobians (checked)
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         % Store these
0072         A(n, :, :) = atr_n; % Gc x U
0073         B(n, :, :) = btr_n; % Gc x Nlr
0074         Cr(n, :, :) = Cr_n; % Nlr x Nlr
0075         v_r(n, :) = - lr_n';                % 1 x Nlr
0076         cg(n, :) = cg_n';                 % 1 x Gc
0077         
0078         % weight of contraint
0079         bCovb_n = btr_n * Cr_n * btr_n';      % Gc x Gc
0080         W(n, :, :) = inv(bCovb_n) * w_f(n); %#ok<MINV> % Gc x Gc
0081         aW = atr_n' * squeeze(W(n,:,:)); % U x Gc
0082         
0083         N_matrix = N_matrix + aW * atr_n; % U x U
0084         h_vector = h_vector + aW * cg_n; % U x 1
0085         
0086     end
0087     
0088     %     det(N_matrix);
0089     %% Solve
0090     Cxrxr = inv(N_matrix);
0091     estx_r = Cxrxr * h_vector; %#ok<MINV>
0092     
0093     if print_option_estimation > 0
0094         disp(['Result of estimation in iteration: ', num2str(nu)]);
0095         disp(estx_r);
0096     end
0097     
0098     %     max(abs(estx_r)./sqrt(diag(Cxrxr)));
0099     if max(abs(estx_r) ./ sqrt(diag(Cxrxr))) < T
0100         s = 2;
0101     end
0102     
0103     %% Determine Updates
0104     Omega = 0;
0105     check = zeros(Gc, 1);
0106     for n = 1:Nc
0107         % covariance matrix of observations (normalized)
0108         Clrlr = squeeze(Cr(n, :, :));
0109         
0110         % corrections of reduced observations
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         % sum of squared residuals
0115         if w_f(n) > 0
0116             vvp_r = ver_r' * inv(Clrlr) * ver_r;                           %#ok<MINV>
0117             Omega = Omega + vvp_r;
0118             residuals(n) = vvp_r;
0119             
0120             % eliminate obsservation by setting w_f=0
0121             if vvp_r > 10
0122                 w_f(n) = 0;
0123             end
0124         end
0125         % updated estimates of both points
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     % update estimate of x
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     %% Stop iteration
0159     if s == 2
0160         break
0161     end
0162     
0163 end
0164 
0165 %% Evaluation of result ------------------------------
0166 % determine Cxx
0167 
0168 [~, Jrh] = sugr_get_CovM_homogeneous_Homography_2D(sugr_Homography_2D(estx, zeros(9)));
0169 Cxx = Jrh * Cxrxr * Jrh';                                                  %#ok<MINV>
0170 
0171 % determine sigma_0
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 % % choose factor
0182 % f = 1;
0183 % if R > min_redundancy
0184 %     f = sigma_0;
0185 % end
0186 
0187 % set output
0188 H = sugr_Homography_2D(estx, Cxx);
0189 
0190