diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index af690f8..7c91057 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -79,7 +79,7 @@ repos: rev: v2.4.1 hooks: - id: codespell - args: ['--write-changes', '--ignore-words-list=theses'] + args: ['--write-changes', '--ignore-words-list=theses,ned'] # For more information on pre-commit.ci and its configuration, visit: # https://pre-commit.ci/ ci: diff --git a/vortex_utils/cpp_test/test_math.cpp b/vortex_utils/cpp_test/test_math.cpp index 6c95fa3..d60f7d8 100644 --- a/vortex_utils/cpp_test/test_math.cpp +++ b/vortex_utils/cpp_test/test_math.cpp @@ -431,4 +431,32 @@ TEST(average_quaternions, test_degeneracy) { EXPECT_THROW(average_quaternions({q0, q180}), std::runtime_error); } +TEST(enu_ned_rotation, explicit_matrix_values) { + Eigen::Quaterniond q_enu = euler_to_quat(0, 0, M_PI / 2); + Eigen::Quaterniond q_ned = enu_ned_rotation(q_enu); + + Eigen::Matrix3d R_frame; + R_frame << 0, 1, 0, 1, 0, 0, 0, 0, -1; + + Eigen::Matrix3d R_expected = R_frame * q_enu.toRotationMatrix(); + Eigen::Matrix3d R_actual = q_ned.toRotationMatrix(); + + EXPECT_TRUE(R_actual.isApprox(R_expected, 1e-12)); +} + +TEST(enu_ned_rotation, test_symmetry) { + Eigen::Quaterniond q_enu = Eigen::Quaterniond( + Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + Eigen::Quaterniond q_ned = enu_ned_rotation(q_enu); + Eigen::Quaterniond q_enu_converted_back = enu_ned_rotation(q_ned); + + EXPECT_TRUE(q_enu.isApprox(q_enu_converted_back, 1e-12) || + q_enu.isApprox(Eigen::Quaterniond(-q_enu_converted_back.w(), + -q_enu_converted_back.x(), + -q_enu_converted_back.y(), + -q_enu_converted_back.z()), + 1e-12)); +} + } // namespace vortex::utils::math diff --git a/vortex_utils/include/vortex/utils/math.hpp b/vortex_utils/include/vortex/utils/math.hpp index f9377de..8ab189b 100644 --- a/vortex_utils/include/vortex/utils/math.hpp +++ b/vortex_utils/include/vortex/utils/math.hpp @@ -158,6 +158,13 @@ Eigen::VectorXd anti_windup(const double dt, Eigen::Quaterniond average_quaternions( const std::vector& quaternions); +/** + * @brief Convert a quaternion between ENU and NED frames. Works both ways. + * @param quat Eigen::Quaterniond + * @return Eigen::Quaterniond + */ +Eigen::Quaterniond enu_ned_rotation(const Eigen::Quaterniond& quat); + } // namespace vortex::utils::math #endif // VORTEX_UTILS_MATH_HPP diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp index e76cedd..bcc5eeb 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/vortex_utils/include/vortex/utils/types.hpp @@ -183,6 +183,28 @@ struct Pose { return quat.normalized(); } + /** + * @brief Set the position from an Eigen::Vector3d. + * @param pos Eigen::Vector3d + */ + void set_pos(const Eigen::Vector3d& pos) { + x = pos.x(); + y = pos.y(); + z = pos.z(); + } + + /** + * @brief Set the orientation from an Eigen::Quaterniond. + * @param ori Eigen::Quaterniond + */ + void set_ori(const Eigen::Quaterniond& ori) { + Eigen::Quaterniond q = ori.normalized(); + qw = q.w(); + qx = q.x(); + qy = q.y(); + qz = q.z(); + } + /** * @brief Convert to Eigen::Vector7d * @return Eigen::Vector7d{x, y, z, qw, qx, qy, qz} diff --git a/vortex_utils/src/math.cpp b/vortex_utils/src/math.cpp index aab8c41..590c8cf 100644 --- a/vortex_utils/src/math.cpp +++ b/vortex_utils/src/math.cpp @@ -174,4 +174,19 @@ Eigen::Quaterniond average_quaternions( return avg_q.normalized(); } +Eigen::Quaterniond enu_ned_rotation(const Eigen::Quaterniond& quat) { + const Eigen::Matrix3d rotation_matrix_enu_to_ned = [] { + Eigen::Matrix3d rotmat; + rotmat.col(0) = Eigen::Vector3d(0, 1, 0); + rotmat.col(1) = Eigen::Vector3d(1, 0, 0); + rotmat.col(2) = Eigen::Vector3d(0, 0, -1); + return rotmat; + }(); + + Eigen::Quaterniond q_out = + Eigen::Quaterniond(rotation_matrix_enu_to_ned) * quat.normalized(); + + return q_out.normalized(); +} + } // namespace vortex::utils::math