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_types.cpp b/vortex_utils/cpp_test/test_types.cpp index 4bcb555..dc41b46 100644 --- a/vortex_utils/cpp_test/test_types.cpp +++ b/vortex_utils/cpp_test/test_types.cpp @@ -87,6 +87,48 @@ TEST_F(TypesTests, test_pose_quat) { 1e-12)); } +TEST_F(TypesTests, position_transform) { + using vortex::utils::types::Pose; + Pose p{.x = 1.0, + .y = 2.0, + .z = 3.0, + .qw = 1.0, + .qx = 0.0, + .qy = 0.0, + .qz = 0.0}; + + Pose ned = p.enu_to_ned(); + + EXPECT_DOUBLE_EQ(ned.x, 2.0); + EXPECT_DOUBLE_EQ(ned.y, 1.0); + EXPECT_DOUBLE_EQ(ned.z, -3.0); +} + +TEST_F(TypesTests, orientation_round_trip) { + using vortex::utils::types::Pose; + Eigen::AngleAxisd aa(M_PI / 3.0, + Eigen::Vector3d(0.3, 0.5, 0.8).normalized()); + Eigen::Quaterniond q(aa); + + Pose p{.x = 0.0, + .y = 0.0, + .z = 0.0, + .qw = q.w(), + .qx = q.x(), + .qy = q.y(), + .qz = q.z()}; + + Pose p2 = p.enu_to_ned().ned_to_enu(); + + Eigen::Quaterniond q1 = p.ori_quaternion(); + Eigen::Quaterniond q2 = p2.ori_quaternion(); + + EXPECT_TRUE( + q1.isApprox(q2, 1e-12) || + q1.isApprox(Eigen::Quaterniond(-q2.w(), -q2.x(), -q2.y(), -q2.z()), + 1e-12)); +} + TEST_F(TypesTests, test_pose_from_eigen) { using vortex::utils::types::Pose; diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp index e76cedd..ad08335 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/vortex_utils/include/vortex/utils/types.hpp @@ -245,6 +245,50 @@ struct Pose { * @return PoseEuler */ PoseEuler as_pose_euler() const; + + /** + * @brief Convert from ENU to NED coordinate frame. + * @return Pose in NED frame. + */ + Pose enu_to_ned() const { + Eigen::Matrix3d R; + R << 0, 1, 0, 1, 0, 0, 0, 0, -1; + + Eigen::Quaterniond q_enu(qw, qx, qy, qz); + Eigen::Quaterniond q_ned = Eigen::Quaterniond(R) * q_enu.normalized(); + + return Pose{ + .x = y, + .y = x, + .z = -z, + .qw = q_ned.w(), + .qx = q_ned.x(), + .qy = q_ned.y(), + .qz = q_ned.z(), + }; + } + + /** + * @brief Convert from NED to ENU coordinate frame. + * @return Pose in ENU frame. + */ + Pose ned_to_enu() const { + Eigen::Matrix3d R; + R << 0, 1, 0, 1, 0, 0, 0, 0, -1; + + Eigen::Quaterniond q_ned(qw, qx, qy, qz); + Eigen::Quaterniond q_enu = Eigen::Quaterniond(R) * q_ned.normalized(); + + return Pose{ + .x = y, + .y = x, + .z = -z, + .qw = q_enu.w(), + .qx = q_enu.x(), + .qy = q_enu.y(), + .qz = q_enu.z(), + }; + } }; /**