Home > .. > Matlab_Uncertain_Frames_sz > check_basics_motions_z.m

check_basics_motions_z

PURPOSE ^

% check basic relations for motions with partially exponential repr.

SYNOPSIS ^

function check_basics_motions_z(Mzs,Mzps,b_output)

DESCRIPTION ^

%  check basic relations for motions with partially 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 5/2020

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 function check_basics_motions_z(Mzs,Mzps,b_output)
0002 %%  check basic relations for motions with partially 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 5/2020
0012 
0013 % for generating small twist vectors
0014 sigma_n = 10^-8;
0015 % M1 and M
0016 Mz = Mzs.Mz;
0017 Mzp = Mzps.Mzp;
0018 
0019 %% twist vector
0020 zeta      = randn(6,1)*sigma_n;
0021 % adjoint motion of a small motion
0022 M_zeta    = [expm(calc_S(zeta(1:3))) zeta(4:6); 0 0 0 1];
0023 M_zeta_ad = Mz * M_zeta * inv(Mz);
0024 
0025 %adjoint of motion matrix
0026 M_ad = [Mz(1:3,1:3) zeros(3); calc_S(Mz(1:3,4))*Mz(1:3,1:3) Mz(1:3,1:3)];
0027 
0028 % twist vector of adjoint motion
0029 zeta_ad = calc_s_from_A(M_zeta_ad);
0030 
0031 % check
0032 check_zeta_ad = (zeta_ad - M_ad * zeta)';
0033 if b_output
0034 check_zeta_ad = (zeta_ad - M_ad * zeta)'
0035 end
0036 
0037 %% inverse
0038 
0039 % noisy small motion matrix and its inverse
0040 M_zeta_r    = [expm(calc_S(zeta(1:3)))*Mz(1:3,1:3) Mz(1:3,4)+zeta(4:6); 0 0 0 1];
0041 M_zeta_inv  = inv(M_zeta_r);
0042 
0043 % twist vector of inverse
0044 zeta_inv = calc_z_from_M_M0(M_zeta_inv,inv(Mz));
0045 
0046 % inverse of adjoint of motion matrix
0047 M_ad_zeta_inv        = [Mz(1:3,1:3)' zeros(3); Mz(1:3,1:3)'*calc_S(Mz(1:3,4)) Mz(1:3,1:3)'];
0048 
0049 % check
0050 check_zeta_inv = (zeta_inv - (-M_ad_zeta_inv  * zeta))';
0051 if b_output
0052 check_zeta_inv = (zeta_inv - (-M_ad_zeta_inv  * zeta))'
0053 end
0054 
0055 % check forward and backward: M = (inv(inv(M))
0056 [Mzsi,J] = calc_inverse_M_z(Mzs);
0057 MM       = calc_inverse_M_z(Mzsi);
0058 
0059 % check
0060 check_Mz_inv = Mzs.Mz-MM.Mz;
0061 check_Cz_inv = Mzs.Cz-MM.Cz;
0062 if b_output
0063 check_Mz_inv = Mzs.Mz-MM.Mz
0064 check_Cz_inv = Mzs.Cz-MM.Cz
0065 end
0066 
0067 %% concatenation M = M2*M1
0068 
0069 % twist vectors of noise of motion pair
0070 zeta   = randn(12,1)*sigma_n;
0071 % noisy motion pair
0072 M1_r   = [expm(calc_S(zeta(1:3)))*Mzp(1:3,1:3) Mzp(1:3,4)+zeta(4:6); 0 0 0 1];
0073 M2_r   = [expm(calc_S(zeta(7:9)))*Mzp(1:3,5:7) Mzp(1:3,8)+zeta(10:12); 0 0 0 1];
0074 % true and noisy concatenation
0075 M      = Mzp(:,5:8) * Mzp(:,1:4);
0076 M_r    = M2_r * M1_r;
0077 % twist vector of noise of concatenations
0078 zeta_e = calc_z_from_M_M0(M_r,M);
0079 
0080 % check zeta = [rho;tau]
0081 R2     = Mzp(1:3,5:7);
0082 Z1     = Mzp(1:3,4);
0083 
0084 check_zeta_concatenate_rho  = ...
0085     zeta_e(1:3) - (R2 * zeta(1:3) + zeta(7:9)); 
0086 check_zeta_concatenate_tau  = ...
0087     zeta_e(4:6) - (R2 * zeta(4:6) + zeta(10:12) - calc_S(R2*Z1)*zeta(7:9));
0088 
0089 check_zeta_concatenate = [check_zeta_concatenate_rho;check_zeta_concatenate_tau]';
0090 if b_output
0091 check_zeta_concatenate = [check_zeta_concatenate_rho;check_zeta_concatenate_tau]'
0092 end
0093 
0094 
0095 %% relative motion M = inv(M1)*M2
0096 
0097 % twist vectors of noise of motion pair
0098 zeta    = randn(12,1)*sigma_n;
0099 % noisy motion pair
0100 M1_r   = [expm(calc_S(zeta(1:3)))*Mzp(1:3,1:3) Mzp(1:3,4)+zeta(4:6); 0 0 0 1];
0101 M2_r   = [expm(calc_S(zeta(7:9)))*Mzp(1:3,5:7) Mzp(1:3,8)+zeta(10:12); 0 0 0 1];
0102 % true and noisy relative motion
0103 M    = inv(Mzp(:,1:4)) * Mzp(:,5:8) ;
0104 M_r  = inv(M1_r)       * M2_r;
0105 % twist vector of noise of relative motion
0106 zeta_e = calc_z_from_M_M0(M_r,M);
0107 
0108 % % adjunct motion matrix of M1
0109 % R1     = Mzp(1:3,1:3);
0110 % Z1     = Mzp(1:3,4);
0111 % M_1_ad = [R1 zeros(3); calc_S(Z1)*R1 R1];
0112 
0113 % check of zeta = [rho;tau]
0114 R1     = Mzp(1:3,1:3);
0115 Z1     = Mzp(1:3,4);
0116 Z2     = Mzp(1:3,8);
0117 
0118 check_zeta_concatenate_rho  = ...
0119     zeta_e(1:3) - (R1' * (zeta(7:9) - zeta(1:3)));
0120 check_zeta_concatenate_tau  = ...
0121     zeta_e(4:6) - (R1' * (zeta(10:12) - zeta(4:6)) ...
0122     + R1' * calc_S(Z2-Z1) * zeta(1:3));
0123 
0124 check_zeta_relative = [check_zeta_concatenate_rho;check_zeta_concatenate_tau]';
0125 if b_output
0126 check_zeta_relative = [check_zeta_concatenate_rho;check_zeta_concatenate_tau]'
0127 end
0128 
0129 %% check all three functions, assume independence
0130 % motions M1 and M2
0131 Mz1.Mz   = Mzps.Mzp(:,1:4);
0132 Mz2.Mz   = Mzps.Mzp(:,5:8);
0133 % CovM of M1, M2
0134 C1       = Mzps.Czp(1:6,1:6);
0135 C2       = Mzps.Czp(7:12,7:12);
0136 % CovM of independent motion pair
0137 C        = [C1 zeros(6); zeros(6) C2];
0138 Mz1.Cz   = C1;
0139 Mz2.Cz   = C2;
0140 Mzps.Czp = C;
0141 
0142 % relative motion Rd = R1' * R2
0143 Mzd      = calc_relative_M_z(Mzps);
0144 % check with concatenation:
0145 % inverse of M1
0146 Mz1i     = calc_inverse_M_z(Mz1);
0147 % concatenate
0148 Mz12.Mzp = [Mz2.Mz,Mz1i.Mz];
0149 Mz12.Czp = [Mz2.Cz zeros(6); zeros(6) Mz1i.Cz];
0150 Mzd_ch   = calc_concatenated_M_z(Mz12);
0151 
0152 % check rel(M1,M2) = con(M2,inv(M1))
0153 check_relative_Mz = Mzd_ch.Mz-Mzd.Mz;
0154 check_relative_Cz = Mzd_ch.Cz-Mzd.Cz;
0155 if b_output
0156 check_relative_Mz = Mzd_ch.Mz-Mzd.Mz
0157 check_relative_Cz = Mzd_ch.Cz-Mzd.Cz
0158 end
0159 
0160 % total evaluation
0161 T = 10^-6;
0162 if norm(check_zeta_ad)         < T*sigma_n && ...
0163         norm(check_zeta_inv)   < T*sigma_n && ...
0164         norm(check_Mz_inv)     < T*sigma_n && ...
0165         norm(check_Cz_inv)     < T*sigma_n^2 && ...
0166         norm(check_zeta_concatenate) < T*sigma_n && ...
0167         norm(check_zeta_relative)    < T*sigma_n && ...
0168         norm(check_relative_Mz)      < T*sigma_n && ...
0169         norm(check_relative_Cz)      < T*sigma_n^2
0170     
0171     display('motions zeta      ok +++++++++++++++++++++++++++++++++++++++')
0172 else
0173     display('notions zeta      not ok ***********************************')
0174 end
0175 end
0176

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