Home > .. > Matlab_Uncertain_Frames_sz > check_basics_motions_s.m

check_basics_motions_s

PURPOSE ^

% check basic relations for motions with exponential repr.

SYNOPSIS ^

function check_basics_motions_s(Mss,Msps,b_output)

DESCRIPTION ^

%  check basic relations for motions with exponential repr.

 twist vector of adjoint motion
 twist vector of inverse motion
 function for inverse motion
 twist vector of concatenated motion
 twist vector of relative motion
 difference motion M12 as concatenated of inv(M1) and M2

 wf 6/2020

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 function check_basics_motions_s(Mss,Msps,b_output)
0002 %%  check basic relations for motions with exponential repr.
0003 %
0004 % twist vector of adjoint motion
0005 % twist vector of inverse motion
0006 % function for inverse motion
0007 % twist vector of concatenated motion
0008 % twist vector of relative motion
0009 % difference motion M12 as concatenated of inv(M1) and M2
0010 %
0011 % wf 6/2020
0012 
0013 % for generating small twist vectors
0014 sigma_n = 10^-8;
0015 % single motion and pair of motions
0016 Ms  = Mss.Ms;
0017 Msp = Msps.Msp;
0018 
0019 % adjoint motion matrix of M
0020 Mad      = [Ms(1:3,1:3) zeros(3); calc_S(Ms(1:3,4))*Ms(1:3,1:3) Ms(1:3,1:3)];
0021 
0022 %% adjoint
0023 % small twist and motion
0024 s      = randn(6,1)*sigma_n;
0025 M_s    = expm(calc_A_from_s(s));
0026 
0027 % adjoint motion
0028 M_s_ad = Ms * M_s * inv(Ms);
0029 
0030 % twist of adjoint motion
0031 s_ad   = calc_s_from_A(M_s_ad);
0032 
0033 % check
0034 check_s_ad = (s_ad - Mad * s)';
0035 if b_output
0036     check_s_ad = (s_ad - Mad * s)'
0037 end
0038 
0039 %% inverse
0040 
0041 % noisy motion and inverse
0042 M_s_r   = M_s * Ms;
0043 M_s_inv = inv(M_s_r);
0044 
0045 % twist of inverse
0046 s_inv   = calc_s_from_A(M_s_inv * Ms);
0047 
0048 % check
0049 check_s_inv = (s_inv -(- inv(Mad) * s))';
0050 if b_output
0051     check_s_inv = (s_inv -(- inv(Mad) * s))'
0052 end
0053 
0054 % check forward and backward M = inv(inv(M))
0055 [Mssi,J] = calc_inverse_M_s(Mss);
0056 MM       = calc_inverse_M_s(Mssi);
0057 
0058 % check mean and CovM
0059 check_Ms = Mss.Ms-MM.Ms;
0060 check_Cs = Mss.Cs-MM.Cs;
0061 check_J = J-(-inv(Mad));
0062 if b_output
0063     check_Ms = Mss.Ms-MM.Ms
0064     check_Cs = Mss.Cs-MM.Cs
0065     check_J = J-(-inv(Mad));
0066 end
0067 
0068 %% concatenation M = M2*M1
0069 
0070 % joint noisy twist vectors
0071 s   = randn(12,1)*sigma_n;
0072 % noisy motions
0073 M1_r = expm(calc_A_from_s(s(1:6 )))*Msp(:,1:4);
0074 M2_r = expm(calc_A_from_s(s(7:12)))*Msp(:,5:8);
0075 % true and noisy concatenation
0076 M    = Msp(:,5:8) * Msp(:,1:4);
0077 M_r  = M2_r * M1_r;
0078 % twist of noise of concatenations
0079 s_e  = calc_s_from_A(M_r*inv(M));
0080 
0081 % adjunct motion matrix of M2
0082 R2     = Msp(1:3,5:7);
0083 Z2     = Msp(1:3,8);
0084 M_2_ad = [R2 zeros(3); calc_S(Z2)*R2 R2];
0085 
0086 % check
0087 check_s_concatenate = (s_e - (M_2_ad*s(1:6) + s(7:12)))';
0088 if b_output
0089     check_s_concatenate = (s_e - (M_2_ad*s(1:6) + s(7:12)))'
0090 end
0091 
0092 
0093 %% relative motion M = inv(M1)*M2
0094 
0095 % joint noisy twist vectors
0096 s    = randn(12,1)*sigma_n;
0097 % noisy motions
0098 M1_r = expm(calc_A_from_s(s(1:6 )))*Msp(:,1:4);
0099 M2_r = expm(calc_A_from_s(s(7:12)))*Msp(:,5:8);
0100 % true and noisy relative motion
0101 M    = inv(Msp(:,1:4)) * Msp(:,5:8) ;
0102 M_r  = inv(M1_r)      * M2_r;
0103 % twist of noise of relative motion
0104 s_e  = calc_s_from_A(M_r*inv(M));
0105 
0106 % adjunct motion matrix of M1
0107 R1     = Msp(1:3,1:3);
0108 Z1     = Msp(1:3,4);
0109 M_1_ad = [R1 zeros(3); calc_S(Z1)*R1 R1];
0110 
0111 % check
0112 check_s_relative = (s_e - (inv(M_1_ad)*(s(7:12) - s(1:6))))';
0113 if b_output
0114     check_s_relative = (s_e - (inv(M_1_ad)*(s(7:12) - s(1:6))))'
0115 end
0116 %% check all three functions, assume independence
0117 % M1 and M2
0118 Ms1.Ms   = Msps.Msp(:,1:4);
0119 Ms2.Ms   = Msps.Msp(:,5:8);
0120 % joint CovM, uncorrelated motions
0121 C        = [Msps.Csp(1:6,1:6) zeros(6); zeros(6) Msps.Csp(7:12,7:12)];
0122 C1       = Msps.Csp(1:6,1:6);
0123 C2       = Msps.Csp(7:12,7:12);
0124 % motion pair
0125 Msps.Csp = C;
0126 % individual motions
0127 Ms1.Cs   = C1;
0128 Ms2.Cs   = C2;
0129 
0130 % relative motion Md = M1' * M2
0131 Msd     = calc_relative_M_s(Msps);
0132 % check with concatenation:
0133 % inverse of M1
0134 Ms1i    = calc_inverse_M_s(Ms1);
0135 % concatenate
0136 Ms12.Msp = [Ms2.Ms,Ms1i.Ms];
0137 Ms12.Csp = [Ms2.Cs zeros(6); zeros(6) Ms1i.Cs];
0138 Msd_ch   = calc_concatenated_M_s(Ms12);
0139 
0140 % check mean and CovM of rel(M1,M2) = con(M2,inv(M1))
0141 check_relative_Ms = Msd_ch.Ms-Msd.Ms;
0142 check_relative_Cs = Msd_ch.Cs-Msd.Cs;
0143 
0144 if b_output
0145     check_relative_Ms = Msd_ch.Ms-Msd.Ms
0146     check_relative_Cs = Msd_ch.Cs-Msd.Cs
0147 end
0148 
0149 %% final evaluation
0150 T = 10^-6;
0151 if norm(check_s_ad)       < T*sigma_n && ...
0152         norm(check_s_inv) < T*sigma_n && ...
0153         norm(check_Ms)     < T*sigma_n && ...
0154         norm(check_Cs)     < T*sigma_n^2 && ...
0155         norm(check_J)      < T*sigma_n && ...
0156         norm(check_s_concatenate) < T*sigma_n && ...
0157         norm(check_s_relative)    < T*sigma_n && ...
0158         norm(check_relative_Ms)    < T*sigma_n && ...
0159         norm(check_relative_Cs)    < T*sigma_n^2
0160     
0161     display('motions s        ok +++++++++++++++++++++++++++++++++++++++')
0162 else
0163     display('notions s        not ok ***********************************')
0164 end
0165 end
0166

Generated on Wed 10-Jun-2020 19:54:31 by m2html © 2005