diff --git a/include/gnss_utils/utils/transformations.h b/include/gnss_utils/utils/transformations.h index 92533d1ae2c82b38b852fa762cb48b6662f6eb5a..0f224d0f21d92016e9adfccfde06d8bb4a1a702e 100644 --- a/include/gnss_utils/utils/transformations.h +++ b/include/gnss_utils/utils/transformations.h @@ -36,8 +36,8 @@ namespace GnssUtils { -Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef); -Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon); +Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef, bool in_rads = true); +Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon, bool in_rads = true); Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_ecef); Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_enu); diff --git a/src/utils/transformations.cpp b/src/utils/transformations.cpp index d1850e0145212d883d0bf3649a3c61436444ce01..b1e07d5fd27c756317bca5edc07ceed209a881b4 100644 --- a/src/utils/transformations.cpp +++ b/src/utils/transformations.cpp @@ -26,18 +26,32 @@ using namespace GnssUtils; namespace GnssUtils { -Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef) +Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef, bool in_rads) { - Eigen::Vector3d pos; - ecef2pos(_ecef.data(), pos.data()); - - return pos; + Eigen::Vector3d latlonalt; + ecef2pos(_ecef.data(), latlonalt.data()); + + if (not in_rads) + { + latlonalt(0) = RAD2DEG * latlonalt(0); + latlonalt(1) = RAD2DEG * latlonalt(1); + } + return latlonalt; } -Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon) +Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon, bool in_rads) { Eigen::Vector3d ecef; - pos2ecef(_latlon.data(), ecef.data()); + if (not in_rads) + { + Eigen::Vector3d latlon_rads = _latlon; + latlon_rads(0) = DEG2RAD * _latlon(0); + latlon_rads(1) = DEG2RAD * _latlon(1); + latlon_rads(2) = _latlon(2); + pos2ecef(latlon_rads.data(), ecef.data()); + } + else + pos2ecef(_latlon.data(), ecef.data()); return ecef; } diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp index 76162d6c39f1295ce5cbec34a3fa919d50c88389..e2c3d3852d0d2837786dbbe90e4eb9c0a8917fbf 100644 --- a/test/gtest_transformations.cpp +++ b/test/gtest_transformations.cpp @@ -31,11 +31,11 @@ static double kSecondEccentricitySquared = 6.73949674228 * 0.001; using namespace GnssUtils; -Eigen::Vector3d computeRandomReceiverLatLonAlt() +Eigen::Vector3d computeRandomReceiverLatLonAlt(bool in_rads = true) { Eigen::Vector3d receiver_LLA = Eigen::Vector3d::Random(); // in [-1, 1] - receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2] - receiver_LLA(1) *= M_PI; // in [-PI, PI] + receiver_LLA(0) *= (in_rads ? M_PI / 2 : 90.0); // in [-PI/2, PI/2] + receiver_LLA(1) *= (in_rads ? M_PI : 180); // in [-PI, PI] receiver_LLA(2) += 1; // ([0, 2]) receiver_LLA(2) *= 5e2; // in [0, 1e3] @@ -71,7 +71,7 @@ void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt, } -TEST(TransformationsTest, ecefToLatLonAlt) +TEST(TransformationsTest, ecefToLatLonAlt_rad) { Eigen::Vector3d ecef, latlonalt; @@ -97,7 +97,33 @@ TEST(TransformationsTest, ecefToLatLonAlt) ASSERT_NEAR(latlonalt(1),0.0,1e-6); } -TEST(TransformationsTest, latLonAltToEcef) +TEST(TransformationsTest, ecefToLatLonAlt_deg) +{ + Eigen::Vector3d ecef, latlonalt; + + // aligned with ECEF X axis + ecef << 1e3, 0, 0; + latlonalt = ecefToLatLonAlt(ecef,false); + + ASSERT_NEAR(latlonalt(0),0.0,1e-6); + ASSERT_NEAR(latlonalt(1),0.0,1e-6); + + // aligned with ECEF Y axis + ecef << 0, 1e3, 0; + latlonalt = ecefToLatLonAlt(ecef,false); + + ASSERT_NEAR(latlonalt(0),0.0,1e-6); + ASSERT_NEAR(latlonalt(1),90,1e-6); + + // aligned with ECEF Z axis + ecef << 0, 0, 1e3; + latlonalt = ecefToLatLonAlt(ecef,false); + + ASSERT_NEAR(latlonalt(0),90,1e-6); + ASSERT_NEAR(latlonalt(1),0.0,1e-6); +} + +TEST(TransformationsTest, latLonAltToEcef_rad) { Eigen::Vector3d ecef, latlonalt; @@ -123,7 +149,33 @@ TEST(TransformationsTest, latLonAltToEcef) ASSERT_NEAR(ecef(1),0.0,1e-6); } -TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt) +TEST(TransformationsTest, latLonAltToEcef_deg) +{ + Eigen::Vector3d ecef, latlonalt; + + // aligned with ECEF X axis + latlonalt << 0, 0, 0; + ecef = latLonAltToEcef(latlonalt, false); + + ASSERT_NEAR(ecef(1),0.0,1e-6); + ASSERT_NEAR(ecef(2),0.0,1e-6); + + // aligned with ECEF Y axis + latlonalt << 0, 90, 0; + ecef = latLonAltToEcef(latlonalt, false); + + ASSERT_NEAR(ecef(0),0.0,1e-6); + ASSERT_NEAR(ecef(2),0.0,1e-6); + + // aligned with ECEF Z axis + latlonalt << 90, 0, 0; + ecef = latLonAltToEcef(latlonalt, false); + + ASSERT_NEAR(ecef(0),0.0,1e-6); + ASSERT_NEAR(ecef(1),0.0,1e-6); +} + +TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt_rad) { Eigen::Vector3d ecef0, ecef1, latlonalt0, latlonalt1; @@ -147,6 +199,30 @@ TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt) } } +TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt_deg) +{ + Eigen::Vector3d ecef0, ecef1, latlonalt0, latlonalt1; + + // ecef -> latlon -> ecef + for (auto i = 0; i<100; i++) + { + ecef0 = 1e3 * Eigen::Vector3d::Random(); + + latlonalt0 = ecefToLatLonAlt(ecef0, false); + ecef1 = latLonAltToEcef(latlonalt0, false); + ASSERT_MATRIX_APPROX(ecef0,ecef1,1e-6); + } + // latlon -> ecef -> latlon + for (auto i = 0; i<100; i++) + { + latlonalt0 = computeRandomReceiverLatLonAlt(false); + + ecef0 = latLonAltToEcef(latlonalt0, false); + latlonalt1 = ecefToLatLonAlt(ecef0, false); + ASSERT_MATRIX_APPROX(latlonalt0,latlonalt1,1e-6); + } +} + TEST(TransformationsTest, computeEnuEcef) { Eigen::Vector3d t_ECEF_ENU, t_ENU_latlonalt, latlonalt1;