|
| 1 | +#include <gtest/gtest.h> |
| 2 | +#include "vortex/utils/math.hpp" |
| 3 | + |
| 4 | +namespace vortex::utils::math { |
| 5 | + |
| 6 | +// Test that the value does not change when already in the interval [-pi, pi] |
| 7 | +TEST(ssa, test_ssa_0) { |
| 8 | + EXPECT_EQ(0, ssa(0)); |
| 9 | +} |
| 10 | + |
| 11 | +// Test that 2 pi correctly maps to 0 |
| 12 | +TEST(ssa, test_ssa_2pi) { |
| 13 | + EXPECT_EQ(0, ssa(2 * M_PI)); |
| 14 | +} |
| 15 | + |
| 16 | +// Test that values over pi gets mapped to the negative interval |
| 17 | +TEST(ssa, test_ssa_3_5) { |
| 18 | + EXPECT_NEAR(-2.78, ssa(3.5), 0.01); |
| 19 | +} |
| 20 | + |
| 21 | +// Test that values under -pi gets mapped to the positive interval |
| 22 | +TEST(ssa, test_ssa_minus_3_5) { |
| 23 | + EXPECT_NEAR(2.78, ssa(-3.5), 0.01); |
| 24 | +} |
| 25 | + |
| 26 | +// Test that the skew-symmetric matrix is correctly calculated |
| 27 | +TEST(get_skew_symmetric_matrix, test_skew_symmetric) { |
| 28 | + Eigen::Vector3d vector(1, 2, 3); |
| 29 | + Eigen::Matrix3d expected; |
| 30 | + expected << 0, -3, 2, 3, 0, -1, -2, 1, 0; |
| 31 | + |
| 32 | + Eigen::Matrix3d result = get_skew_symmetric_matrix(vector); |
| 33 | + EXPECT_EQ(expected, result); |
| 34 | +} |
| 35 | + |
| 36 | +// Test that rotation matrix is correctly constructed |
| 37 | +TEST(get_rotation_matrix, test_rotation_matrix) { |
| 38 | + double roll{1.0}; |
| 39 | + double pitch{2.0}; |
| 40 | + double yaw{3.0}; |
| 41 | + Eigen::Matrix3d expected; |
| 42 | + expected << 0.41198225, -0.83373765, -0.36763046, -0.05872664, -0.42691762, |
| 43 | + 0.90238159, -0.90929743, -0.35017549, -0.2248451; |
| 44 | + Eigen::Matrix3d result = get_rotation_matrix(roll, pitch, yaw); |
| 45 | + EXPECT_NEAR(0.0, matrix_norm_diff(expected, result), 0.01); |
| 46 | +} |
| 47 | + |
| 48 | +TEST(get_transformation_matrix_attitude, test_transformation_matrix_zeros) { |
| 49 | + Eigen::Matrix3d transformation_matrix{ |
| 50 | + get_transformation_matrix_attitude(0.0, 0.0)}; |
| 51 | + Eigen::Matrix3d expected{Eigen::Matrix3d::Identity()}; |
| 52 | + EXPECT_NEAR(0.0, matrix_norm_diff(expected, transformation_matrix), 0.01); |
| 53 | +} |
| 54 | + |
| 55 | +// Test that the identity quaternion correctly maps to 0, 0, 0 |
| 56 | +TEST(quat_to_euler, test_quat_to_euler_1) { |
| 57 | + Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0); |
| 58 | + Eigen::Vector3d expected(0.0, 0.0, 0.0); |
| 59 | + Eigen::Vector3d result = quat_to_euler(q); |
| 60 | + for (int i = 0; i < 3; ++i) { |
| 61 | + EXPECT_NEAR(expected[i], result[i], 0.01); |
| 62 | + } |
| 63 | +} |
| 64 | + |
| 65 | +// Test that only changing w and x in the quat only affects roll |
| 66 | +TEST(quat_to_euler, test_quat_to_euler_2) { |
| 67 | + Eigen::Quaterniond q2(0.707, 0.707, 0.0, 0.0); |
| 68 | + Eigen::Vector3d expected2(1.57, 0.0, 0.0); |
| 69 | + Eigen::Vector3d result2 = quat_to_euler(q2); |
| 70 | + for (int i = 0; i < 3; ++i) { |
| 71 | + EXPECT_NEAR(expected2[i], result2[i], 0.01); |
| 72 | + } |
| 73 | +} |
| 74 | + |
| 75 | +// Test that only changing w and z in the quat only affects yaw |
| 76 | +TEST(quat_to_euler, test_quat_to_euler_3) { |
| 77 | + Eigen::Quaterniond q4(0.707, 0.0, 0.0, 0.707); |
| 78 | + Eigen::Vector3d expected4(0.0, 0.0, 1.57); |
| 79 | + Eigen::Vector3d result4 = quat_to_euler(q4); |
| 80 | + for (int i = 0; i < 3; ++i) { |
| 81 | + EXPECT_NEAR(expected4[i], result4[i], 0.01); |
| 82 | + } |
| 83 | +} |
| 84 | + |
| 85 | +// Test that a quaternion is correctly converted to euler angles |
| 86 | +TEST(quat_to_euler, test_quat_to_euler_4) { |
| 87 | + Eigen::Quaterniond q5(0.770, 0.4207, -0.4207, -0.229); |
| 88 | + Eigen::Vector3d expected5(1.237, -0.4729, -0.9179); |
| 89 | + Eigen::Vector3d result5 = quat_to_euler(q5); |
| 90 | + for (int i = 0; i < 3; ++i) { |
| 91 | + EXPECT_NEAR(expected5[i], result5[i], 0.01); |
| 92 | + } |
| 93 | +} |
| 94 | + |
| 95 | +// Test that a quaternion with flipped signs is correctly converted to euler |
| 96 | +// angles |
| 97 | +TEST(quat_to_euler, test_quat_to_euler_5) { |
| 98 | + Eigen::Quaterniond q5(0.770, 0.4207, 0.4207, 0.229); |
| 99 | + Eigen::Vector3d expected5(1.237, 0.4729, 0.9179); |
| 100 | + Eigen::Vector3d result5 = quat_to_euler(q5); |
| 101 | + for (int i = 0; i < 3; ++i) { |
| 102 | + EXPECT_NEAR(expected5[i], result5[i], 0.01); |
| 103 | + } |
| 104 | +} |
| 105 | + |
| 106 | +// Test that zero euler angles construct the correct quaternion |
| 107 | +TEST(euler_to_quat, test_euler_to_quat_1) { |
| 108 | + double roll{}; |
| 109 | + double pitch{}; |
| 110 | + double yaw{}; |
| 111 | + Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; |
| 112 | + Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; |
| 113 | + Eigen::Vector4d expected{0.0, 0.0, 0.0, 1.0}; |
| 114 | + for (int i = 0; i < 4; ++i) { |
| 115 | + EXPECT_NEAR(expected[i], result[i], 0.01); |
| 116 | + } |
| 117 | +} |
| 118 | + |
| 119 | +// Test that non-zero roll constructs the correct quaternion |
| 120 | +TEST(euler_to_quat, test_euler_to_quat_2) { |
| 121 | + double roll{1.0}; |
| 122 | + double pitch{}; |
| 123 | + double yaw{}; |
| 124 | + Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; |
| 125 | + Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; |
| 126 | + Eigen::Vector4d expected{0.479, 0.0, 0.0, 0.877}; |
| 127 | + for (int i = 0; i < 4; ++i) { |
| 128 | + EXPECT_NEAR(expected[i], result[i], 0.01); |
| 129 | + } |
| 130 | +} |
| 131 | + |
| 132 | +// Test that non-zero pitch constructs the correct quaternion |
| 133 | +TEST(euler_to_quat, test_euler_to_quat_3) { |
| 134 | + double roll{}; |
| 135 | + double pitch{1.0}; |
| 136 | + double yaw{}; |
| 137 | + Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; |
| 138 | + Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; |
| 139 | + Eigen::Vector4d expected{0.0, 0.479, 0.0, 0.877}; |
| 140 | + for (int i = 0; i < 4; ++i) { |
| 141 | + EXPECT_NEAR(expected[i], result[i], 0.01); |
| 142 | + } |
| 143 | +} |
| 144 | + |
| 145 | +// Test that non-zero yaw constructs the correct quaternion |
| 146 | +TEST(euler_to_quat, test_euler_to_quat_4) { |
| 147 | + double roll{}; |
| 148 | + double pitch{}; |
| 149 | + double yaw{1.0}; |
| 150 | + Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; |
| 151 | + Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; |
| 152 | + Eigen::Vector4d expected{0.0, 0.0, 0.479, 0.877}; |
| 153 | + for (int i = 0; i < 4; ++i) { |
| 154 | + EXPECT_NEAR(expected[i], result[i], 0.01); |
| 155 | + } |
| 156 | +} |
| 157 | + |
| 158 | +// Test that non-zero euler angles constructs the correct quaternion |
| 159 | +TEST(euler_to_quat, test_euler_to_quat_5) { |
| 160 | + double roll{1.0}; |
| 161 | + double pitch{1.0}; |
| 162 | + double yaw{1.0}; |
| 163 | + Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; |
| 164 | + Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; |
| 165 | + Eigen::Vector4d expected{0.1675, 0.5709, 0.1675, 0.786}; |
| 166 | + for (int i = 0; i < 4; ++i) { |
| 167 | + EXPECT_NEAR(expected[i], result[i], 0.01); |
| 168 | + } |
| 169 | +} |
| 170 | + |
| 171 | +} // namespace vortex::utils::math |
0 commit comments