enu-ned quat convertion (#32)

* enu-ned quat convertion

* lambda initialization

Author: jorgenfj

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

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<Eigen::Quaterniond>& 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

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}

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