sugr_estimation_algebraic_Homography_2D_from_point_pairs

% Algebraic estimation of 2D homography from point pairs, 
 assuming independent observations
 and conditioned points

 H = sugr_estimation_algebraic_Homography_2D_from_point_pairs(P) 

 P contains point pairs
 P.h = [X,Y]
 * X = N x 3 matrix if points 
 * Y = N x 3 matrix if points
 P.Crr = N x 2 x 2 contains reduced covariance matrix
 

 0 != S^s(y) * H * x 
    = Rm(y) * S(y) * H * x 
    = Rm(y) * S'(H x) * y
    = Rm(y) * S(y) * (x' kron eye(3)) h 

 H.H   = estimated homography with uncertainty, 
         using algebraic minimization, thus neglecting the accuracy
 H.Crr = CovM of reduced parameters, derived by variance propagation


 Wolfgang Förstner
 wfoerstn@uni-bonn.de

 wf 2/2010

0001 %% Algebraic estimation of 2D homography from point pairs,
0002 % assuming independent observations
0003 % and conditioned points
0004 %
0005 % H = sugr_estimation_algebraic_Homography_2D_from_point_pairs(P)
0006 %
0007 % P contains point pairs
0008 % P.h = [X,Y]
0009 % * X = N x 3 matrix if points
0010 % * Y = N x 3 matrix if points
0011 % P.Crr = N x 2 x 2 contains reduced covariance matrix
0012 %
0013 %
0014 % 0 != S^s(y) * H * x
0015 %    = Rm(y) * S(y) * H * x
0016 %    = Rm(y) * S'(H x) * y
0017 %    = Rm(y) * S(y) * (x' kron eye(3)) h
0018 %
0019 % H.H   = estimated homography with uncertainty,
0020 %         using algebraic minimization, thus neglecting the accuracy
0021 % H.Crr = CovM of reduced parameters, derived by variance propagation
0022 %
0023 %
0024 % Wolfgang Förstner
0025 % wfoerstn@uni-bonn.de
0026 %
0027 % wf 2/2010
0028 
0029 function H = sugr_estimation_algebraic_Homography_2D_from_point_pairs(P)
0030 
0031 % pair coordinates
0032 Ph   = P.h;
0033 % pair CovM reduced
0034 PCrr = P.Crr;
0035 % homogeneous coordinates
0036 X = Ph(:,1:3);
0037 Y = Ph(:,4:6);
0038 % number of point pairs
0039 N = size(X,1);
0040 
0041 %%
0042 % estimate H algebraically
0043 %
0044 % build coefficient matrix
0045 A = zeros(2*N,9);
0046 for n = 1:N
0047     % select 2x3 skew matrix
0048     [Ssy,~] = calc_S_reduced(Y(n,:)');
0049     % Kronecker 3x9 -matrix
0050     K  = [X(n,1)*eye(3), X(n,2)*eye(3) X(n,3)*eye(3)];
0051     % 2x9 part of A
0052     A(2*n-1:2*n,:)  = Ssy * K; 
0053 end
0054 
0055 % partition
0056 N_rows = size(A,1);
0057 if N_rows == 8
0058     h = null(A);
0059     A_plus = A'/(A*A');
0060 else
0061     [U,D,V] = svd(A,'econ');
0062     % find algebraically best solution
0063     h        = V(:,9);
0064     % determine A+ = pseudo inverse of A with last singular value = 0
0065     Di       = inv(D+eps*eye(9));
0066     Di(9,9)  = 0;
0067     A_plus   = V * Di * U';
0068 end
0069 % choose unique sign (largest element > 0)
0070 [~,imax] = max(abs(h));
0071 h        = h * sign(h(imax));
0072 He       = reshape(h,3,3);
0073 He=He/(abs(det(He))^(1/3));
0074 
0075 
0076 %% determine covariance matrix Chh = A^+ Cgg A^+'
0077 
0078 % determine Cgg of residuals g of constraints from
0079 % S^s(y) H x                        = g
0080 % Ry Sy  H x                        = g
0081 % d(Ry Sy H x)                      = d(g)
0082 % Ry Sy H dx     - Ry S(H x) dy     = dg
0083 % S^sy  H Jr dxr - Ry S(H x) Jr dyr = dg
0084 %
0085 
0086 %Cxyxy = sparse(zeros(2*N,2*N));
0087 Cxyxy = spalloc(2*N,2*N,2*N);
0088 for n=1:N
0089     % VarProp for g
0090     [Ssy,Ry] = calc_S_reduced(Y(n,:));
0091     J_x = + Ssy * He * null(X(n,:));
0092     J_y = - Ry * calc_S(He * X(n,:)') *  null(Y(n,:));
0093     % effect of both
0094     J = [J_x J_y];  % 2 x 4 Jacobian res/pr = [res/xr res/yr]
0095     Cxyxy(2*n-1:2*n,2*n-1:2*n)  = Cxyxy(2*n-1:2*n,2*n-1:2*n) ...
0096         + J * squeeze(PCrr(n,:,:)) * J';                                    %#ok<SPRIX>
0097 end
0098 Chh = A_plus * Cxyxy * A_plus';
0099 
0100 H = sugr_Homography_2D(He,Chh);