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