% Determines residuals etc. for projection from point pairs [lr,cg,Crrt,atr,btr] = sugr_ghm_cg_Projection_3D_2D_from_point_pairs_e(l,le,xe,Crro) with euclidean representation * l = 6 x 1 vector, observed point pair * le = 6 x 1 vector, approximated fitted point pair * xe = 3 x 4 matrix, approximated projection * Crro = 5 x 5 matrix, reduced covariance matrix of point pair * lr = 5 x 1 vector, reduced observation * cg = 2 x 1 vector, residual of constraint * Crrt = 5 x 5 matrix, reduced covariance matrix of transformed observations * atr = 2 x 11 matrix, transposed Jacobian for cg -> xe * btr = 2 x 5 matrix, transposed jacobian for cg -> le 0 != y - c(P X) = y - c( (X' kron I3) vec(P)) ; c(x) = x(1:2)/x(3) Wolfgang Förstner 02/2013 wfoerstn@uni-bonn.de See also sugr_estimation_algebraic_Projection_3D_2D_from_point_pairs sugr_estimation_ml_Projection_3D_2D_from_point_pairs sugr_ghm_cg_Projection_3D_2D_from_point_pairs
0001 %% Determines residuals etc. for projection from point pairs 0002 % 0003 % [lr,cg,Crrt,atr,btr] = sugr_ghm_cg_Projection_3D_2D_from_point_pairs_e(l,le,xe,Crro) 0004 % 0005 % with euclidean representation 0006 % 0007 % * l = 6 x 1 vector, observed point pair 0008 % * le = 6 x 1 vector, approximated fitted point pair 0009 % * xe = 3 x 4 matrix, approximated projection 0010 % * Crro = 5 x 5 matrix, reduced covariance matrix of point pair 0011 % 0012 % * lr = 5 x 1 vector, reduced observation 0013 % * cg = 2 x 1 vector, residual of constraint 0014 % * Crrt = 5 x 5 matrix, reduced covariance matrix of transformed observations 0015 % * atr = 2 x 11 matrix, transposed Jacobian for cg -> xe 0016 % * btr = 2 x 5 matrix, transposed jacobian for cg -> le 0017 % 0018 % 0 != y - c(P X) = y - c( (X' kron I3) vec(P)) ; c(x) = x(1:2)/x(3) 0019 % 0020 % Wolfgang Förstner 02/2013 0021 % wfoerstn@uni-bonn.de 0022 % 0023 % See also sugr_estimation_algebraic_Projection_3D_2D_from_point_pairs 0024 % sugr_estimation_ml_Projection_3D_2D_from_point_pairs 0025 % sugr_ghm_cg_Projection_3D_2D_from_point_pairs 0026 0027 function [lr,Cr,cg,atr,btr] = sugr_ghm_cg_Projection_3D_2D_from_point_pairs(l,le,xe,C) 0028 0029 %% preparation 0030 % current estimate for projection matrix 0031 Pe = reshape(xe,3,4); 0032 0033 % homogeneous coordinates of projection 0034 yh = Pe * le(1:4); 0035 0036 0037 % 2x3 Jacobian of c(.) from homogenoues to Euclidean (12.129) 0038 Jc = [yh(3)*eye(2), -yh(1:2)]/yh(3)^2; 0039 0040 % Jacobian for observation -> reduced observation: 6 x 5 matrix 0041 Jle = [null(le(1:4)') zeros(4,2);... 0042 zeros(2,3) eye(2)] ; 0043 0044 % reduced observation_ 5 x 1 vector 0045 lr = [null(le(1:4)')' * l(1:4); l(5:6)-le(5:6)]; 0046 0047 0048 %% adaptation of Covariance matrix 0049 % Rotation from l to le: 6 x 6 matrix 0050 R = [calc_Rot_ab(le(1:4),l(1:4)) zeros(4,2);... 0051 zeros(2,4) eye(2)]; 0052 % Jacobian for Cr 0053 JR = Jle' * R' * Jle; %[null(le(1:4)') zeros(4,2);... 0054 % zeros(2,3) eye(2)] ; 0055 % transferred covariance matrix 0056 Cr = JR * C * JR'; 0057 0058 %% Jacobians 0059 % Jacobian for cg -> x 0060 atr = - Jc * kron(le(1:4)', eye(3)) * null(xe'); % 2 x 11 Matrix 0061 0062 % Jacobain for cg -> l 0063 btr = [- Jc * Pe * null(le(1:4)') , eye(2)]; % 2 x 5 matrix 0064 0065 %% residual of constraint (lr = -vr) 0066 0067 % constraint = -(l(5:6)-yh(1:2)/yh(3)); 0068 cg = - (le(5:6)- yh(1:2)/yh(3)) - btr * lr; 0069 %disp(strcat('-cg(le,xe) = ',num2str(le(5:6)'- (yh(1:2)/yh(3))'),', -btr_lr =',num2str((btr * lr)'))); 0070