From e83e29190d69f35ecdb50786e8583511fb774fb3 Mon Sep 17 00:00:00 2001 From: Henry Winkel Date: Sat, 18 Mar 2023 14:05:06 +0100 Subject: [PATCH] ADD: added eulerconversions for the heading pitch and roll; ADD: added a oriatation class --- CMakeLists.txt | 12 +++ include/SimCore/EulerConversion.hpp | 103 ++++++++++++++++++++ include/SimCore/Orientation.hpp | 41 ++++++++ include/SimCore/SimCore.hpp | 7 ++ src/SimCore/EulerConversion.cpp | 145 ++++++++++++++++++++++++++++ src/SimCore/Orientation.cpp | 65 +++++++++++++ tests/test_OrientationClass.cpp | 35 +++++++ 7 files changed, 408 insertions(+) create mode 100644 include/SimCore/EulerConversion.hpp create mode 100644 include/SimCore/Orientation.hpp create mode 100644 src/SimCore/EulerConversion.cpp create mode 100644 src/SimCore/Orientation.cpp create mode 100644 tests/test_OrientationClass.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 95ee93f..197e0ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,6 +59,12 @@ add_library(SimCore STATIC include/SimCore/Position.hpp src/SimCore/Position.cpp + include/SimCore/Orientation.hpp + src/SimCore/Orientation.cpp + + include/SimCore/EulerConversion.hpp + src/SimCore/EulerConversion.cpp + include/SimCore/SafeMap.hpp src/SimCore/SafeMap.cpp @@ -125,6 +131,12 @@ IF (${TEST_SIMCORE_LIBRARY}) target_link_libraries(test_PositionClass Catch2::Catch2 SimCore eigen loguru) catch_discover_tests(test_PositionClass) + add_executable(test_OrientationClass tests/test_OrientationClass.cpp) + target_link_libraries(test_OrientationClass Catch2::Catch2 SimCore eigen loguru) + catch_discover_tests(test_OrientationClass) + + + add_executable(test_GroundTruthTrackClass tests/test_GroundTruthTrackClass.cpp) target_link_libraries(test_GroundTruthTrackClass Catch2::Catch2 SimCore eigen loguru) catch_discover_tests(test_GroundTruthTrackClass) diff --git a/include/SimCore/EulerConversion.hpp b/include/SimCore/EulerConversion.hpp new file mode 100644 index 0000000..fe31ff5 --- /dev/null +++ b/include/SimCore/EulerConversion.hpp @@ -0,0 +1,103 @@ +#pragma once +#include +#include + +namespace SimCore { + + class EulerConversions{ + public: + + /** + * Gets a degree heading for an entity based on euler angles. All angular + * values passed in must be in radians. + * + * @param lat Entity's latitude, IN RADIANS + * @param lon Entity's longitude, IN RADIANS + * @param psi Psi angle, IN RADIANS + * @param theta Theta angle, IN RADIANS + * @return the heading, in degrees, with 0 being north, positive angles + * going clockwise, and negative angles going counterclockwise (i.e., 90 deg + * is east, -90 is west) + */ + static double getOrientationFromEuler(double lat, double lon, double psi, double theta); + + /** + * Gets a degree pitch for an entity based on euler angles. All angular + * values passed in must be in radians. + * + * @param lat Entity's latitude, IN RADIANS + * @param lon Entity's longitude, IN RADIANS + * @param psi Psi angle, IN RADIANS + * @param theta Theta angle, IN RADIANS + * @return the pitch, in degrees, with 0 being level. A negative values is + * when the entity's nose is pointing downward, positive value is when the + * entity's nose is pointing upward. + */ + static double getPitchFromEuler(double lat, double lon, double psi, double theta); + +/** + * Gets the degree roll for an entity based on euler angles. All angular + * values passed in must be in radians. + * + * @param lat Entity's latitude, IN RADIANS + * @param lon Entity's longitude, IN RADIANS + * @param psi Psi angle, IN RADIANS + * @param theta Theta angle, IN RADIANS + * @param phi Phi angle, IN RADIANS + * @return the roll, in degrees, with 0 being level flight, + roll is + * clockwise when looking out the front of the entity. + */ + static double getRollFromEuler(double lat, double lon, double psi, double theta, double phi); + + /** + * Gets the Euler Theta value (in radians) from position and Tait-Brayn yaw + * and pitch angles + * + * @param lat Entity's latitude, IN RADIANS + * @param lon Entity's longitude, IN RADIANS + * @param yaw entity's yaw angle (also know as the entity's bearing or + * heading angle), in degrees + * @param pitch entity's pitch angle, in degrees + * @return the Theta value in radians + */ + static double getThetaFromTaitBryanAngles(double lat, double lon, double yaw, double pitch); + + /** + * Gets the Euler Psi value (in radians) from position and Tait-Brayn yaw + * and pitch angles + * + * @param lat Entity's latitude, IN RADIANS + * @param lon Entity's longitude, IN RADIANS + * @param yaw ettity's yaw angle (also know as the entity's bearing or + * heading angle), in degrees + * @param pitch entity's pitch angle, in degrees + * @return the Psi value in radians + */ + static double getPsiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch); + + /** + * Gets the Euler Phi value (in radians) from position and Tait-Brayn yaw, + * pitch and roll angles + * + * @param lat Entity's latitude, IN RADIANS + * @param lon Entity's longitude, IN RADIANS + * @param yaw yaw angle (also know as the entity's bearing or heading + * angle), in degrees + * @param pitch entity's pitch angle, in degrees + * @param roll entity's roll angle (0 is level flight, + roll is clockwise + * looking out the nose), in degrees + * @return the Phi value in radians + */ + static double getPhiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch, double roll); + + + private: + + static double toDegrees(double radian); + + constexpr static const double _toDegrees = 57.2957795131; + constexpr static double _toRadians = 0.01745329252; + +}; +} + diff --git a/include/SimCore/Orientation.hpp b/include/SimCore/Orientation.hpp new file mode 100644 index 0000000..3a952a9 --- /dev/null +++ b/include/SimCore/Orientation.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include + + +namespace SimCore +{ + class Orientation + { + + public: + Orientation(double heading = 0, double pitch = 0, double roll = 0); + + Orientation(Eigen::Vector3d angles, double lat, double lon); + + void setHeading(double heading); + double getHeading(); + + void setPitch(double pitch); + double getPitch(); + + void setRoll(double roll); + double getRoll(); + + Eigen::Vector3d getEulerAngles(double lat ,double lon); + + + private: + + + Eigen::Vector3d eulerAngels_; + + double heading_; + double pitch_; + double roll_; + + + }; + + +} \ No newline at end of file diff --git a/include/SimCore/SimCore.hpp b/include/SimCore/SimCore.hpp index 0981046..13b519b 100644 --- a/include/SimCore/SimCore.hpp +++ b/include/SimCore/SimCore.hpp @@ -19,6 +19,13 @@ Y, Z }; +enum EulerAngels : std::uint8_t +{ +PSI = 0, +THETA, +PHI +}; + enum ObjectSource : bool{ diff --git a/src/SimCore/EulerConversion.cpp b/src/SimCore/EulerConversion.cpp new file mode 100644 index 0000000..30c9b91 --- /dev/null +++ b/src/SimCore/EulerConversion.cpp @@ -0,0 +1,145 @@ +#include + + + + + +namespace SimCore +{ + double EulerConversions::getOrientationFromEuler(double lat, double lon, double psi, double theta) { + double sinlat = sin(lat); + double sinlon = sin(lon); + double coslon = cos(lon); + double coslat = cos(lat); + double sinsin = sinlat * sinlon; + + double cosTheta = cos(theta); + double cosPsi = cos(psi); + double sinPsi = sin(psi); + double sinTheta = sin(theta); + + double cosThetaCosPsi = cosTheta * cosPsi; + double cosThetaSinPsi = cosTheta * sinPsi; + double sincos = sinlat * coslon; + + double b11 = -sinlon * cosThetaCosPsi + coslon * cosThetaSinPsi; + double b12 = -sincos * cosThetaCosPsi - sinsin * cosThetaSinPsi - coslat * sinTheta; + + return toDegrees(atan2(b11, b12));//range is -pi to pi + }; + + + + double EulerConversions::getPitchFromEuler(double lat, double lon, double psi, double theta) { + double sinlat = sin(lat); + double sinlon = sin(lon); + double coslon = cos(lon); + double coslat = cos(lat); + double cosLatCosLon = coslat * coslon; + double cosLatSinLon = coslat * sinlon; + + double cosTheta = cos(theta); + double cosPsi = cos(psi); + double sinPsi = sin(psi); + double sinTheta = sin(theta); + + return toDegrees(asin(cosLatCosLon * cosTheta * cosPsi + cosLatSinLon * cosTheta * sinPsi - sinlat * sinTheta)); + }; + + + + double EulerConversions::getRollFromEuler(double lat, double lon, double psi, double theta, double phi) { + double sinlat = sin(lat); + double sinlon = sin(lon); + double coslon = cos(lon); + double coslat = cos(lat); + double cosLatCosLon = coslat * coslon; + double cosLatSinLon = coslat * sinlon; + + double cosTheta = cos(theta); + double sinTheta = sin(theta); + double cosPsi = cos(psi); + double sinPsi = sin(psi); + double sinPhi = sin(phi); + double cosPhi = cos(phi); + + double sinPhiSinTheta = sinPhi * sinTheta; + double cosPhiSinTheta = cosPhi * sinTheta; + + double b23 = cosLatCosLon * (-cosPhi * sinPsi + sinPhiSinTheta * cosPsi) + + cosLatSinLon * (cosPhi * cosPsi + sinPhiSinTheta * sinPsi) + + sinlat * (sinPhi * cosTheta); + + double b33 = cosLatCosLon * (sinPhi * sinPsi + cosPhiSinTheta * cosPsi) + + cosLatSinLon * (-sinPhi * cosPsi + cosPhiSinTheta * sinPsi) + + sinlat * (cosPhi * cosTheta); + + return toDegrees(atan2(-b23, -b33)); + }; + + + double EulerConversions::getThetaFromTaitBryanAngles(double lat, double lon, double yaw, double pitch) { + double sinLat = sin(lat); + double cosLat = cos(lat); + + double cosPitch = cos(pitch * _toRadians); + double sinPitch = sin(pitch * _toRadians); + double cosYaw = cos(yaw * _toRadians); + + return asin(-cosLat * cosYaw * cosPitch - sinLat * sinPitch); + }; + + + + double EulerConversions::getPsiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch) { + + double sinLat = sin(lat); + double sinLon = sin(lon); + double cosLon = cos(lon); + double cosLat = cos(lat); + double cosLatCosLon = cosLat * cosLon; + double cosLatSinLon = cosLat * sinLon; + double sinLatCosLon = sinLat * cosLon; + double sinLatSinLon = sinLat * sinLon; + + double cosPitch = cos(pitch * _toRadians); + double sinPitch = sin(pitch * _toRadians); + double sinYaw = sin(yaw * _toRadians); + double cosYaw = cos(yaw * _toRadians); + + double a_11 = -sinLon * sinYaw * cosPitch - sinLatCosLon * cosYaw * cosPitch + cosLatCosLon * sinPitch; + double a_12 = cosLon * sinYaw * cosPitch - sinLatSinLon * cosYaw * cosPitch + cosLatSinLon * sinPitch; + + return atan2(a_12, a_11); + }; + + + + double EulerConversions::getPhiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch, double roll) { + + double sinLat = sin(lat); + double cosLat = cos(lat); + + double cosRoll = cos(roll * _toRadians); + double sinRoll = sin(roll * _toRadians); + double cosPitch = cos(pitch * _toRadians); + double sinPitch = sin(pitch * _toRadians); + double sinYaw = sin(yaw * _toRadians); + double cosYaw = cos(yaw * _toRadians); + + double a_23 = cosLat * (-sinYaw * cosRoll + cosYaw * sinPitch * sinRoll) - sinLat * cosPitch * sinRoll; + double a_33 = cosLat * (sinYaw * sinRoll + cosYaw * sinPitch * cosRoll) - sinLat * cosPitch * cosRoll; + + return atan2(a_23, a_33); + }; + + + + double EulerConversions::toDegrees(double radian) + { + return (radian *(180/M_PI)); + } + + + +} \ No newline at end of file diff --git a/src/SimCore/Orientation.cpp b/src/SimCore/Orientation.cpp new file mode 100644 index 0000000..8123fe3 --- /dev/null +++ b/src/SimCore/Orientation.cpp @@ -0,0 +1,65 @@ +#include "SimCore/EulerConversion.hpp" +#include "SimCore/SimCore.hpp" +#include + + +namespace SimCore +{ + + Orientation::Orientation(double heading, double pitch , double roll ):heading_(heading),pitch_(pitch),roll_(roll) + { + } + + + Orientation::Orientation(Eigen::Vector3d angles, double lat, double lon) + { + heading_ = EulerConversions::getOrientationFromEuler( lat, lon, angles[PSI],angles[THETA] ); + pitch_ = EulerConversions::getPitchFromEuler(lat, lon, angles[PSI],angles[THETA]); + roll_ = EulerConversions::getRollFromEuler( lat, lon, angles[PSI],angles[THETA],angles[PHI] ); + } + + + + void Orientation::setHeading(double heading) + { + heading_ = heading; + } + + double Orientation::getHeading() + { + return heading_; + } + + void Orientation::setPitch(double pitch) + { + pitch_ = pitch; + } + + double Orientation::getPitch() + { + return pitch_; + } + + void Orientation::setRoll(double roll) + { + roll_ = roll; + } + + double Orientation::getRoll() + { + return roll_; + } + + Eigen::Vector3d Orientation::getEulerAngles(double lat ,double lon) + { + eulerAngels_[PHI] = EulerConversions::getPhiFromTaitBryanAngles( lat, lon, heading_, pitch_, roll_); + eulerAngels_[THETA] = EulerConversions::getThetaFromTaitBryanAngles(lat, lon, heading_, pitch_); + eulerAngels_[PSI] = EulerConversions::getPsiFromTaitBryanAngles(lat, lon, heading_, pitch_); + + return eulerAngels_; + + } + + + +} \ No newline at end of file diff --git a/tests/test_OrientationClass.cpp b/tests/test_OrientationClass.cpp new file mode 100644 index 0000000..ebb61d3 --- /dev/null +++ b/tests/test_OrientationClass.cpp @@ -0,0 +1,35 @@ +#include +#define CATCH_CONFIG_MAIN +#include +#include +#include + + + +SCENARIO("Testing the SimCorePositionClass") +{ + GIVEN("different position in different forms") + { + double lon = 55; + double lat = 10; + + + + + + WHEN("constructing Position Object with data") + { + SimCore::Orientation Ori1(0,0,0); + + LOG_S(INFO)<< Ori1.getEulerAngles(lat,lon); + + THEN("positions attributes are correct") + { + REQUIRE(Ori1.getHeading() == 0); + + + + } //THEN + } // WHEN + } // GIVEN +} //SCENARIO \ No newline at end of file