pose: enu-ned transforms

.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:

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;
 

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(),
+        };
+    }
 };
 
 /**