sugr_ghm_cg_E_matrix_from_point_pairs

% Constraints and Jacobians for GHM for estimating E-matrix from point pairs
 i.e., determines residuals etc. for E-matrix from point pairs
 with minimal representation
 
 [lr,cg,Crrt,atr,btr] = sugr_ghm_cg_E_matrix_from_point_pairs(l,le,xe,Crro)
                        

 * l   = 6 x 1 vector, observed point pair
 * le  = 6 x 1 vector, approximated fitted point pair
 * xe  = 3 x 4 matrix [b^a, R^a] of approximated values
 * Crro = 4 x 4 matrix, reduced covariance matrix of point pair

 * lr  = 4 x 1 vector, reduced observation
 * cg  = 1 x 1 vector, residual of constraint
 * Crrt = 4 x 4 matrix, reduced covariance matrix of transformed observations
 * atr  = 1 x 5 vector, transposed Jacobian for cg -> xe
 * btr  = 1 x 4 vector, transposed jacobian for cg -> le

 0 != x'^T S(b) R' x'' = x''^T R S^T(b) x' = 

 Wolfgang Förstner 03/2011
 wfoerstn@uni-bonn.de

 See also sugr_E_Matrix

0001 %% Constraints and Jacobians for GHM for estimating E-matrix from point pairs
0002 % i.e., determines residuals etc. for E-matrix from point pairs
0003 % with minimal representation
0004 %
0005 % [lr,cg,Crrt,atr,btr] = sugr_ghm_cg_E_matrix_from_point_pairs(l,le,xe,Crro)
0006 %
0007 %
0008 % * l   = 6 x 1 vector, observed point pair
0009 % * le  = 6 x 1 vector, approximated fitted point pair
0010 % * xe  = 3 x 4 matrix [b^a, R^a] of approximated values
0011 % * Crro = 4 x 4 matrix, reduced covariance matrix of point pair
0012 %
0013 % * lr  = 4 x 1 vector, reduced observation
0014 % * cg  = 1 x 1 vector, residual of constraint
0015 % * Crrt = 4 x 4 matrix, reduced covariance matrix of transformed observations
0016 % * atr  = 1 x 5 vector, transposed Jacobian for cg -> xe
0017 % * btr  = 1 x 4 vector, transposed jacobian for cg -> le
0018 %
0019 % 0 != x'^T S(b) R' x'' = x''^T R S^T(b) x' =
0020 %
0021 % Wolfgang Förstner 03/2011
0022 % wfoerstn@uni-bonn.de
0023 %
0024 % See also sugr_E_Matrix
0025 
0026 function [lr,Cr,cg,atr,btr] = sugr_ghm_cg_E_matrix_from_point_pairs(l,le,xe,C)
0027 
0028 % approximate values
0029 ba = xe(:,1);
0030 Ra = xe(:,2:4);
0031 Ea = calc_S(ba)*Ra';
0032 
0033 % Jacobian for observation -> reduced observation: 6 x 4 matrix
0034 Jle = [null(le(1:3)')      zeros(3,2);... 
0035        zeros(3,2)      null(le(4:6)')] ;
0036 
0037 % reduced observation_ 4 x 1 vector
0038 lr  = Jle' * l;
0039 
0040 % Rotation from l to le: 6 x 6 matrix
0041 R = [calc_Rot_ab(le(1:3),l(1:3)) zeros(3);...
0042     zeros(3) calc_Rot_ab(le(4:6),l(4:6))];
0043 
0044 % Jacobian for Cr
0045 JR = Jle' * R' * [null(le(1:3)')      zeros(3,2);... 
0046                   zeros(3,2)      null(le(4:6)')] ;
0047 
0048 % transferred covariance matrix
0049 Cr = JR * C * JR';
0050 
0051 
0052 % Jacobian for cg -> x
0053 xais    = le(1:3);
0054 xaiss   = le(4:6);
0055 xaiss1  = Ra' * xaiss;
0056 lais    = Ea  * xaiss;
0057 laiss   = Ea' * xais;
0058 atr = [cross(xaiss1 , xais)' * null(ba') , cross(laiss , xaiss)']; % 1 x 5 vector
0059 
0060 % Jacobain for cg -> l
0061 btr = [lais' * null(xais') , laiss' * null(xaiss')] ; % 1 x 4 vector
0062  
0063 % residual of constraint (lr = -vr)
0064 cg = - le(1:3)'*calc_S(ba)*Ra'*le(4:6)  - btr * lr; 
0065