0001 function check_basics_rotations(Rs,Rps,b_output)
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014 sigma_n = 10^-8;
0015
0016 R = Rs.R;
0017 Rp = Rps.Rp;
0018
0019
0020
0021 r = randn(3,1)*sigma_n;
0022
0023 R_r_ad = R * expm(calc_S(r)) * R';
0024
0025 r_ad = calc_v_from_S(R_r_ad);
0026
0027 check_rad = (r_ad - R * r)';
0028 if b_output
0029 check_rad = (r_ad - R * r)'
0030 end
0031
0032
0033
0034 r = randn(3,1)*sigma_n;
0035
0036 R_r = expm(calc_S(r))*R;
0037
0038 R_inv_r = R_r';
0039
0040 r_inv = calc_v_from_S(R_inv_r * R);
0041
0042
0043 check_r_inv = (r_inv -(- R' * r))';
0044 if b_output
0045 check_r_inv = (r_inv -(- R' * r))'
0046 end
0047
0048
0049 [Rsi,J]=calc_inverse_R(Rs);
0050 RR =calc_inverse_R(Rsi);
0051
0052
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
0063
0064
0065 r = randn(6,1)*sigma_n;
0066
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
0070 R = Rp(:,4:6) * Rp(:,1:3);
0071 R_r = R2_r * R1_r;
0072
0073 r_e = calc_v_from_S(R_r*R');
0074
0075
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
0082
0083
0084 r = randn(6,1)*sigma_n;
0085
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
0089 R = Rp(:,1:3)' * Rp(:,4:6) ;
0090 R_r = R1_r' * R2_r;
0091
0092 r_e = calc_v_from_S(R_r*R');
0093
0094
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
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
0111 Rd = calc_relative_R(Rps);
0112
0113
0114 R1i = calc_inverse_R(R1);
0115
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
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
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