diff --git a/cpp_test/test_math.cpp b/cpp_test/test_math.cpp index b18f8a9..f06c8ce 100644 --- a/cpp_test/test_math.cpp +++ b/cpp_test/test_math.cpp @@ -52,6 +52,48 @@ TEST(get_transformation_matrix_attitude, test_transformation_matrix_zeros) { EXPECT_NEAR(0.0, matrix_norm_diff(expected, transformation_matrix), 0.01); } +TEST(get_transformation_matrix_attitude_quat, + test_transformation_matrix_unit_quat) { + Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); + Eigen::Matrix transformation_matrix{ + get_transformation_matrix_attitude_quat(quat)}; + Eigen::Matrix expected = Eigen::Matrix::Zero(); + expected.bottomRightCorner<3, 3>() = 0.5 * Eigen::Matrix3d::Identity(); + EXPECT_NEAR(0.0, matrix_norm_diff(expected, transformation_matrix), 0.01); +} + +TEST(eigen_vector3d_to_quaternion, zero_vector_returns_identity) { + const Eigen::Vector3d v = Eigen::Vector3d::Zero(); + + const Eigen::Quaterniond q = eigen_vector3d_to_quaternion(v); + + EXPECT_NEAR(q.w(), 1.0, 1e-15); + EXPECT_NEAR(q.x(), 0.0, 1e-15); + EXPECT_NEAR(q.y(), 0.0, 1e-15); + EXPECT_NEAR(q.z(), 0.0, 1e-15); + EXPECT_NEAR(q.norm(), 1.0, 1e-15); +} + +TEST(eigen_vector3d_to_quaternion, general_vector_matches_axis_angle_formula) { + const Eigen::Vector3d v(0.3, -0.4, 0.5); + const double angle = v.norm(); + const Eigen::Vector3d axis = v / angle; + + const double half = 0.5 * angle; + const double c = cos(half); + const double s = sin(half); + const Eigen::Quaterniond expected(c, axis.x() * s, axis.y() * s, + axis.z() * s); + + const Eigen::Quaterniond q = eigen_vector3d_to_quaternion(v); + + EXPECT_NEAR(q.w(), expected.w(), 1e-12); + EXPECT_NEAR(q.x(), expected.x(), 1e-12); + EXPECT_NEAR(q.y(), expected.y(), 1e-12); + EXPECT_NEAR(q.z(), expected.z(), 1e-12); + EXPECT_NEAR(q.norm(), 1.0, 1e-12); +} + // Test that the identity quaternion correctly maps to 0, 0, 0 TEST(quat_to_euler, test_quat_to_euler_1) { Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0); @@ -168,4 +210,69 @@ TEST(euler_to_quat, test_euler_to_quat_5) { } } +// Test that zero euler angles construct the correct quaternion +TEST(euler_to_quat_vec3, zero_angles_identity_quaternion) { + const Eigen::Vector3d euler(0.0, 0.0, 0.0); // roll, pitch, yaw + const Eigen::Quaterniond q = euler_to_quat(euler); + + const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; + const Eigen::Vector4d expected(0.0, 0.0, 0.0, 1.0); + + for (int i = 0; i < 4; ++i) { + EXPECT_NEAR(expected[i], result[i], 0.01); + } +} + +// Test that non-zero roll constructs the correct quaternion +TEST(euler_to_quat_vec3, roll_only) { + const Eigen::Vector3d euler(1.0, 0.0, 0.0); // roll, pitch, yaw + const Eigen::Quaterniond q = euler_to_quat(euler); + + const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; + const Eigen::Vector4d expected(0.479, 0.0, 0.0, 0.877); + + for (int i = 0; i < 4; ++i) { + EXPECT_NEAR(expected[i], result[i], 0.01); + } +} + +// Test that non-zero pitch constructs the correct quaternion +TEST(euler_to_quat_vec3, pitch_only) { + const Eigen::Vector3d euler(0.0, 1.0, 0.0); + const Eigen::Quaterniond q = euler_to_quat(euler); + + const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; + const Eigen::Vector4d expected(0.0, 0.479, 0.0, 0.877); + + for (int i = 0; i < 4; ++i) { + EXPECT_NEAR(expected[i], result[i], 0.01); + } +} + +// Test that non-zero yaw constructs the correct quaternion +TEST(euler_to_quat_vec3, yaw_only) { + const Eigen::Vector3d euler(0.0, 0.0, 1.0); + const Eigen::Quaterniond q = euler_to_quat(euler); + + const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; + const Eigen::Vector4d expected(0.0, 0.0, 0.479, 0.877); + + for (int i = 0; i < 4; ++i) { + EXPECT_NEAR(expected[i], result[i], 0.01); + } +} + +// Test that non-zero euler angles construct the correct quaternion +TEST(euler_to_quat_vec3, roll_pitch_yaw_all_nonzero) { + const Eigen::Vector3d euler(1.0, 1.0, 1.0); + const Eigen::Quaterniond q = euler_to_quat(euler); + + const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; + const Eigen::Vector4d expected(0.1675, 0.5709, 0.1675, 0.786); + + for (int i = 0; i < 4; ++i) { + EXPECT_NEAR(expected[i], result[i], 0.01); + } +} + } // namespace vortex::utils::math diff --git a/include/vortex/utils/math.hpp b/include/vortex/utils/math.hpp index 1e06366..8f46257 100644 --- a/include/vortex/utils/math.hpp +++ b/include/vortex/utils/math.hpp @@ -28,6 +28,13 @@ Eigen::Matrix3d get_rotation_matrix(const double roll, Eigen::Matrix3d get_transformation_matrix_attitude(const double roll, const double pitch); +// @brief Fossen, 2021 eq. 2.78 +Eigen::Matrix get_transformation_matrix_attitude_quat( + const Eigen::Quaterniond& quat); + +// @brief Converts an Eigen::Vector3d with euler angles to Eigen::Quaterniond +Eigen::Quaterniond eigen_vector3d_to_quaternion(const Eigen::Vector3d& vector); + // @brief Converts a quaternion to Euler angles. Eigen::Vector3d quat_to_euler(const Eigen::Quaterniond& q); @@ -36,6 +43,9 @@ Eigen::Quaterniond euler_to_quat(const double roll, const double pitch, const double yaw); +// @brief Converts Eigen::Vector3d with euler angles to quaternion +Eigen::Quaterniond euler_to_quat(const Eigen::Vector3d& euler); + } // namespace vortex::utils::math #endif // VORTEX_UTILS_MATH_HPP diff --git a/src/math.cpp b/src/math.cpp index 96b8100..7a6786e 100644 --- a/src/math.cpp +++ b/src/math.cpp @@ -25,6 +25,32 @@ Eigen::Matrix3d get_rotation_matrix(const double roll, return rotation_matrix; } +Eigen::Matrix get_transformation_matrix_attitude_quat( + const Eigen::Quaterniond& quat) { + Eigen::Matrix T_q = Eigen::Matrix::Zero(); + double qw{quat.w()}; + double qx{quat.x()}; + double qy{quat.y()}; + double qz{quat.z()}; + + T_q << -qx, -qy, -qz, qw, -qz, qy, qz, qw, -qx, -qy, qx, qw; + + T_q *= 0.5; + return T_q; +} + +Eigen::Quaterniond eigen_vector3d_to_quaternion(const Eigen::Vector3d& vector) { + double angle{vector.norm()}; + if (angle < 1e-8) { + return Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + } else { + Eigen::Vector3d axis = vector / angle; + Eigen::Quaterniond quat = + Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); + return quat.normalized(); + } +} + Eigen::Matrix3d get_transformation_matrix_attitude(const double roll, const double pitch) { double sin_r = sin(roll); @@ -69,4 +95,12 @@ Eigen::Quaterniond euler_to_quat(const double roll, return q.normalized(); } +Eigen::Quaterniond euler_to_quat(const Eigen::Vector3d& euler) { + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()); + return q.normalized(); +} + } // namespace vortex::utils::math