Home > .. > Matlab_Uncertain_Frames_sz > check_basics_rotations.m

check_basics_rotations

PURPOSE ^

% check basic relations for rotations

SYNOPSIS ^

function check_basics_rotations(Rs,Rps,b_output)

DESCRIPTION ^

%  check basic relations for rotations

 rotation vector of adjoint rotation
 rotation vector of inverse rotation
 function for inverse rotation
 rotation vector of concatenated rotation
 rotation vector of relative rotation
 difference rotation M12 as concatenated of inv(R1) and R2

 wf 5/2020

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 function check_basics_rotations(Rs,Rps,b_output)
0002 %%  check basic relations for rotations
0003 %
0004 % rotation vector of adjoint rotation
0005 % rotation vector of inverse rotation
0006 % function for inverse rotation
0007 % rotation vector of concatenated rotation
0008 % rotation vector of relative rotation
0009 % difference rotation M12 as concatenated of inv(R1) and R2
0010 %
0011 % wf 5/2020
0012 
0013 % for generating small twist vectors
0014 sigma_n = 10^-8;
0015 % basic rotation and rotation pair
0016 R = Rs.R;
0017 Rp = Rps.Rp;
0018 
0019 %% adjoint rotation
0020 % small rotation vector
0021 r = randn(3,1)*sigma_n;
0022 % adjoint rotation
0023 R_r_ad = R * expm(calc_S(r)) * R';
0024 % rotation vector of adjoint rotation
0025 r_ad  = calc_v_from_S(R_r_ad);
0026 %check
0027 check_rad = (r_ad - R * r)';
0028 if b_output
0029     check_rad = (r_ad - R * r)'
0030 end
0031 
0032 %% inverse
0033 % small rotation vector
0034 r   = randn(3,1)*sigma_n;
0035 % noisy rotation
0036 R_r = expm(calc_S(r))*R;
0037 % inverse
0038 R_inv_r = R_r';
0039 % rotation vector of noise of inverse
0040 r_inv  = calc_v_from_S(R_inv_r * R);
0041 
0042 % check
0043     check_r_inv = (r_inv -(- R' * r))';
0044 if b_output
0045     check_r_inv = (r_inv -(- R' * r))'
0046 end
0047 
0048 % check forward and backward of function inverse_rotation: R = inv(inv(R))
0049 [Rsi,J]=calc_inverse_R(Rs);
0050 RR =calc_inverse_R(Rsi);
0051 
0052 %check
0053     check_R = Rs.R-RR.R;
0054     check_C = Rs.C-RR.C;
0055     check_J = J-(-R');
0056 if b_output
0057     check_R = Rs.R-RR.R
0058     check_C = Rs.C-RR.C
0059     check_J = J-(-R')
0060 end
0061 
0062 %% concatenation R = R2*R1
0063 
0064 % rotation vectors of noise of rotations
0065 r    = randn(6,1)*sigma_n;
0066 % noisy rotations
0067 R1_r = expm(calc_S(r(1:3)))*Rp(:,1:3);
0068 R2_r = expm(calc_S(r(4:6)))*Rp(:,4:6);
0069 % true and noisy concatenation
0070 R    = Rp(:,4:6) * Rp(:,1:3);
0071 R_r  = R2_r * R1_r;
0072 % rotation vector of noise of concatenated rotations
0073 r_e  = calc_v_from_S(R_r*R');
0074 
0075 % check
0076     check_r_concatenate = r_e - (Rp(:,4:6)*r(1:3) + r(4:6));
0077 if b_output
0078     check_r_concatenate = r_e - (Rp(:,4:6)*r(1:3) + r(4:6))
0079 end
0080 
0081 %% difference rotation R = inv(R1)*R2
0082 
0083 % rotation vectors of noise of rotations
0084 r    = randn(6,1)*sigma_n;
0085 % noisy rotations
0086 R1_r = expm(calc_S(r(1:3)))*Rp(:,1:3);
0087 R2_r = expm(calc_S(r(4:6)))*Rp(:,4:6);
0088 % true and noisy concatenation
0089 R    = Rp(:,1:3)' * Rp(:,4:6) ;
0090 R_r  = R1_r'      * R2_r;
0091 % rotation vector of noise of relative rotation
0092 r_e  = calc_v_from_S(R_r*R');
0093 
0094 %check
0095     check_r_relative = r_e - (Rp(:,1:3)'*(r(4:6) - r(1:3)));
0096 if b_output
0097     check_r_relative = r_e - (Rp(:,1:3)'*(r(4:6) - r(1:3)))
0098 end
0099 
0100 %% check all three functions, assume independence rel(R1,R2) = con(R2,inv(R1))
0101 R1.R  = Rps.Rp(:,1:3);
0102 R2.R  = Rps.Rp(:,4:6);
0103 C     = [Rps.Cp(1:3,1:3) zeros(3); zeros(3) Rps.Cp(4:6,4:6)];
0104 C1    = Rps.Cp(1:3,1:3);
0105 C2    = Rps.Cp(4:6,4:6);
0106 Rps.Cp = C;
0107 R1.C  = C1;
0108 R2.C  = C2;
0109 
0110 % relative rotation Rd = R1' * R2
0111 Rd     = calc_relative_R(Rps);
0112 % check with concatenation:
0113 % inverse of R1
0114 R1i    = calc_inverse_R(R1);
0115 % concatenate
0116 R12.Rp = [R2.R,R1i.R];
0117 R12.Cp = [R2.C zeros(3); zeros(3) R1i.C];
0118 Rd_ch  = calc_concatenated_R(R12);
0119 
0120 % check
0121     check_relative_R = Rd_ch.R-Rd.R;
0122     check_relative_C = Rd_ch.C-Rd.C;
0123 if b_output
0124     check_relative_R = Rd_ch.R-Rd.R
0125     check_relative_C = Rd_ch.C-Rd.C
0126 end
0127 
0128 
0129 % total evaluation
0130 T = 10^-6;
0131 if norm(check_rad)   < T*sigma_n && ...
0132    norm(check_r_inv) < T*sigma_n && ...
0133    norm(check_R)     < T*sigma_n && ...
0134    norm(check_C)     < T*sigma_n^2 && ...
0135    norm(check_J)     < T*sigma_n && ...
0136    norm(check_r_concatenate) < T*sigma_n && ...
0137    norm(check_r_relative)    < T*sigma_n && ...
0138    norm(check_relative_R)    < T*sigma_n && ...
0139    norm(check_relative_C)    < T*sigma_n^2 
0140    
0141     display('rotations         ok       +++++++++++++++++++++++++++++++++')
0142 else
0143     display('rotations         not ok   *********************************')
0144 end
0145

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