% 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);