sugr_get_CbRbR_JebR_from_b_R_Cee

% Determine CbRbR from Cee for given Relative Orientaion

 [EbR,CbRbR,JebR] = sugr_get_CbRbR_JebR_from_b_R_Cee(b,R,Cee)

 input
    b      = 3x1 basis
    R      = 3 x 3 rotation matrix
    Cee    = 9 x 9 Cov(e)

 output
    EbR    = 3 x 3 E-matrix
    CbRbR  = 5 x 5 CovM of [b_r;r] 
    JebR   = 9 x 5 Jacobian de/dbR

 Wolfgang Förstner 11/2017
 wfoerstn@uni-bonn.de

 See also sugr_E_Matrix

0001 %% Determine CbRbR from Cee for given Relative Orientaion
0002 %
0003 % [EbR,CbRbR,JebR] = sugr_get_CbRbR_JebR_from_b_R_Cee(b,R,Cee)
0004 %
0005 % input
0006 %    b      = 3x1 basis
0007 %    R      = 3 x 3 rotation matrix
0008 %    Cee    = 9 x 9 Cov(e)
0009 %
0010 % output
0011 %    EbR    = 3 x 3 E-matrix
0012 %    CbRbR  = 5 x 5 CovM of [b_r;r]
0013 %    JebR   = 9 x 5 Jacobian de/dbR
0014 %
0015 % Wolfgang Förstner 11/2017
0016 % wfoerstn@uni-bonn.de
0017 %
0018 % See also sugr_E_Matrix
0019 function [EbR,CbRbR,JebR] = sugr_get_CbRbR_JebR_from_b_R_Cee(b,R,Cee)
0020 
0021 % essential matrix
0022 EbR = calc_S(b)*R';
0023 
0024 % Jacobian de/d[b_r;r]
0025 % E  = S(b)*R' ... % rows ri and ei of R and E
0026 % de = vec(dS(b)*R' + S(b)*dR')  (10.339)
0027 % de = [S'(r1); S'(r2); S'(r3)] db + vec(S(b)*R'*S'(dr))
0028 % de = -[S(r1); S(r2); S(r3)] J_r(b) db_r + vec[e1'*S'(dr);e2'*S'(dr);e3'*S'(dr)])
0029 % de = -[S(r1); S(r2); S(r3)] J_r(b) db_r + vec[dr'*S(e1);dr'*S(e3);dr'*S(e3)]
0030 % de = -[S(r1); S(r2); S(r3)] J_r(b) db_r + vec[dr'*[S(e1);S(e2);S(e3)]]
0031 % using T_mn for permuting the rows of the Jacobian, s. Fackler (2005)
0032 % Notes on matrix calculus -- see Maple jacobian_vecESr_wrt_r.mws.
0033 % de = -[S(r1); S(r2); S(r3)] J_r(b) db_r + T_mn*vec[S'(e1),S'(e2),S'(e3)]*dr
0034 %
0035 % de = JebR * dbR -> dbR = JebR^+ de
0036 
0037 % Jacobian wrt b_r:
0038 Jb = -[calc_S(R(1,:));...
0039        calc_S(R(2,:));...
0040        calc_S(R(3,:))]...
0041      * null(b');          % 9 x 2
0042 
0043  % Jacobian wrt r
0044 z3=zeros(3,1);
0045 Jp = -[ z3,      -EbR(:,3)  EbR(:,2);...
0046         EbR(:,3) z3        -EbR(:,1);...
0047        -EbR(:,2)  EbR(:,1)  z3];        % 9 x 3
0048    
0049 % joint Jacobian wrt [b_r;r]
0050 JebR = [Jb,Jp];                  % 9 x 5
0051 
0052 %%% Check numerically
0053 %[~,~,J]=var_prop_classical(@e_from_bR,zeros(5,1),eye(5),[b,R]);
0054 
0055 JebRp = (JebR'*JebR)\JebR';      % 5 x 9
0056 
0057 % CovM of e=vecE
0058 CbRbR  = JebRp * Cee * JebRp';   % 5 x 5
0059 
0060