% Algebraic estimate of Projection from corresponding 3D-2D point pairs
assuming independent observations
P = sugr_estimation_algebraic_Projection_3D_2D_from_points(X,y)
X.e = N x 3 matrix if points
y.e = N x 2 matrix if points
X.Cee = N x 3 x 3 matrix of covariances for 3D points
y.Cee = N x 2 x 2 matrix of covariances for 2D points
model with constraints assuming points not at infinity
0 != Rm * S(y) * P * X Rm = [eye(2), zeros(2,1)]
= Rm * S'(P X) * y
= Rm * S(y) * (X' kron eye(3)) p
P.P = estimated projection matrix with uncertainty,
using algebraic minimization, thus neglecting the accuracy
P.Crr = CovM of reduced parameters, derived by variance propagation
points may not be conditioned.
Wolfgang Förstner 2/2013
wfoerstn@uni-bonn.de
See also sugr_estimation_ml_Projection_3D_2D_from_point_pairs0001 %% Algebraic estimate of Projection from corresponding 3D-2D point pairs 0002 % 0003 % assuming independent observations 0004 % 0005 % P = sugr_estimation_algebraic_Projection_3D_2D_from_points(X,y) 0006 % 0007 % X.e = N x 3 matrix if points 0008 % y.e = N x 2 matrix if points 0009 % X.Cee = N x 3 x 3 matrix of covariances for 3D points 0010 % y.Cee = N x 2 x 2 matrix of covariances for 2D points 0011 % 0012 % model with constraints assuming points not at infinity 0013 % 0 != Rm * S(y) * P * X Rm = [eye(2), zeros(2,1)] 0014 % = Rm * S'(P X) * y 0015 % = Rm * S(y) * (X' kron eye(3)) p 0016 % 0017 % P.P = estimated projection matrix with uncertainty, 0018 % using algebraic minimization, thus neglecting the accuracy 0019 % P.Crr = CovM of reduced parameters, derived by variance propagation 0020 % 0021 % points may not be conditioned. 0022 % 0023 % Wolfgang Förstner 2/2013 0024 % wfoerstn@uni-bonn.de 0025 % 0026 % See also sugr_estimation_ml_Projection_3D_2D_from_point_pairs 0027 0028 function P = sugr_estimation_algebraic_Projection_3D_2D_from_point_pairs(X,y) 0029 0030 % number of point pairs 0031 N = size(X.e,1); 0032 U = 12; 0033 0034 % condition 0035 [Xs,MX] = sugr_condition_Points(X); 0036 [ye,My] = sugr_condition_Points(y); 0037 0038 0039 % homogeneous coordinates 0040 X = Xs.h; 0041 % nonhomogeneous coordinates 0042 y = ye.e; 0043 0044 0045 %% estimate P algebraically 0046 % 0047 % build coefficient matrix 0048 A = zeros(2*N,U); 0049 for n = 1:N 0050 % % use first two coordinates 0051 % S * Kronecker 3xU -matrix 0052 SK = calc_S([y(n,:),1]') * ... 0053 [X(n,1)*eye(3), X(n,2)*eye(3) X(n,3)*eye(3) X(n,4)*eye(3)]; 0054 % 2xU part of A 0055 A(2*n-1:2*n,:) = SK(1:2,:); 0056 end 0057 0058 % partition 0059 [Um,Dm,Vm] = svd(A,'econ'); 0060 log(abs(diag(Dm))); 0061 % find algebraically best solution 0062 h = Vm(:,U); 0063 % determine A+ = pseudo inverse of A with last singular value = 0 0064 Di = inv(Dm+eps*eye(U)); 0065 Di(U,U) = 0; 0066 A_plus = Vm * Di * Um'; 0067 0068 % rehape and choose unique sign 0069 Pe = reshape(h,3,4); 0070 Pe = Pe * sign(det(Pe(1:3,1:3))); 0071 0072 %% 0073 % determine covariance matrix Chh = A^+ Cgg A^+' 0074 0075 % determine Cgg of residuals of constraints from 0076 % Rm S(yh) P X = g yh = [y;1] 0077 % d(Rm S(yh) P X) = d(g) 0078 % Rm S(yh) P dX - Rm S(P X) dyh = dg 0079 % Rm S(yh) P Jr dXr - Rm S(P X) Rm' dy = dg 0080 % 0081 CXyXy = sparse(zeros(2*N,2*N)); 0082 for n=1:N 0083 % VarProp for g 0084 % [Ssy,Ry] = calc_S_reduced([y(n,:),1]'); 0085 % S = calc_S([y(n,:),1]'); 0086 Rm = [eye(2) zeros(2,1)]; 0087 J_X = + Rm * calc_S([y(n,:),1]') * Pe * null(X(n,:)); 0088 J_y = - Rm * calc_S(Pe * X(n,:)') * Rm' ; 0089 % effect of both 0090 CXyXy(2*n-1:2*n,2*n-1:2*n) = CXyXy(2*n-1:2*n,2*n-1:2*n) ... 0091 + J_y * squeeze(ye.Cee(n,:,:)) * J_y' ... 0092 + J_X * squeeze(Xs.Crr(n,:,:)) * J_X'; %#ok<SPRIX> 0093 end 0094 Chh = A_plus * CXyXy * A_plus'; 0095 0096 Pc = sugr_Projection_3D_2D(Pe,Chh); 0097 0098 % uncondition projection matrix 0099 P = sugr_uncondition_Projection(Pc,My,MX); 0100 return