From 99352332b9e70656d416964f5c2d36664bd53ee0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 4 Mar 2020 12:26:58 +0100
Subject: [PATCH] transformations and gtest
---
src/gnss_utils.cpp | 2 +-
src/test/gtest_transformations.cpp | 193 +++++++++++++++++++++++++++--
2 files changed, 181 insertions(+), 14 deletions(-)
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index e4336e5..455e25a 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -399,7 +399,7 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve
// receiver-sat vector enu (receiver ecef as origin)
Eigen::Vector3d receiver_sat_enu;
ecef2enu(receiver_geo.data(), //geodetic position {lat,lon} (rad)
- receiver_sat_ecef.data(), //vector in ecef coordinate {x,y,z}
+ receiver_sat_ecef.data(), //vector in ecef coordinate {x,y,z}
receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
// elevation
diff --git a/src/test/gtest_transformations.cpp b/src/test/gtest_transformations.cpp
index 193cb06..cae4e24 100644
--- a/src/test/gtest_transformations.cpp
+++ b/src/test/gtest_transformations.cpp
@@ -1,29 +1,196 @@
#include "gtest/utils_gtest.h"
#include "gnss_utils/gnss_utils.h"
-/* functions being tested:
- *
- * Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef);
- * Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _latlon);
- * Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef);
- * Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu);
- * void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
- * void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
- */
+// Geodetic system parameters
+static double kSemimajorAxis = 6378137;
+static double kSemiminorAxis = 6356752.3142;
+static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
+static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
using namespace GNSSUtils;
TEST(TransformationsTest, ecefToLatLonAlt)
{
Eigen::Vector3d ecef, latlonalt;
- ecef << 0, 0, 1e3;
+ // aligned with ECEF X axis
+ ecef << 1e3, 0, 0;
+ latlonalt = ecefToLatLonAlt(ecef);
+
+ 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);
+
+ ASSERT_NEAR(latlonalt(0),0.0,1e-6);
+ ASSERT_NEAR(latlonalt(1),M_PI / 2,1e-6);
+
+ // aligned with ECEF Z axis
+ ecef << 0, 0, 1e3;
latlonalt = ecefToLatLonAlt(ecef);
- std::cout << latlonalt.transpose();
+ ASSERT_NEAR(latlonalt(0),M_PI / 2,1e-6);
+ ASSERT_NEAR(latlonalt(1),0.0,1e-6);
+}
+
+TEST(TransformationsTest, latLonAltToEcef)
+{
+ Eigen::Vector3d ecef, latlonalt;
+
+ // aligned with ECEF X axis
+ latlonalt << 0, 0, 0;
+ ecef = latLonAltToEcef(latlonalt);
+
+ ASSERT_NEAR(ecef(1),0.0,1e-6);
+ ASSERT_NEAR(ecef(2),0.0,1e-6);
+
+ // aligned with ECEF Y axis
+ latlonalt << 0, M_PI / 2, 0;
+ ecef = latLonAltToEcef(latlonalt);
+
+ ASSERT_NEAR(ecef(0),0.0,1e-6);
+ ASSERT_NEAR(ecef(2),0.0,1e-6);
+
+ // aligned with ECEF Z axis
+ latlonalt << M_PI / 2, 0, 0;
+ ecef = latLonAltToEcef(latlonalt);
+
+ ASSERT_NEAR(ecef(0),0.0,1e-6);
+ ASSERT_NEAR(ecef(1),0.0,1e-6);
+}
+
+TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt)
+{
+ Eigen::Vector3d ecef0, ecef1, latlonalt0, latlonalt1;
+
+ // ecef -> latlon -> ecef
+ for (auto i = 0; i<100; i++)
+ {
+ ecef0 = 1e3 * Eigen::Vector3d::Random();
+
+ latlonalt0 = ecefToLatLonAlt(ecef0);
+ ecef1 = latLonAltToEcef(latlonalt0);
+ ASSERT_MATRIX_APPROX(ecef0,ecef1,1e-6);
+ }
+ // latlon -> ecef -> latlon
+ for (auto i = 0; i<100; i++)
+ {
+ latlonalt0 = Eigen::Vector3d::Random();
+ latlonalt0(0) *= M_PI / 2;
+ latlonalt0(1) *= M_PI;
+ latlonalt0(2) *= 1e3;
+
+ ecef0 = latLonAltToEcef(latlonalt0);
+ latlonalt1 = ecefToLatLonAlt(ecef0);
+ ASSERT_MATRIX_APPROX(latlonalt0,latlonalt1,1e-6);
+ }
+}
+
+TEST(TransformationsTest, computeEnuEcef)
+{
+ Eigen::Vector3d t_ECEF_ENU, t_ENU_latlonalt, latlonalt1;
+ Eigen::Vector3d t_ENU_ECEF1,t_ENU_ECEF2,t_ENU_ECEF3;
+ Eigen::Matrix3d R_ENU_ECEF1,R_ENU_ECEF2,R_ENU_ECEF3;
+
+ // random
+ for (auto i = 0; i<100; i++)
+ {
+ t_ENU_latlonalt = Eigen::Vector3d::Random();
+ t_ENU_latlonalt(0) *= M_PI / 2;
+ t_ENU_latlonalt(1) *= M_PI;
+ t_ENU_latlonalt(2) *= 1e3;
+
+ t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
+
+ // 1 from ECEF
+ computeEnuEcefFromEcef(t_ECEF_ENU, R_ENU_ECEF1, t_ENU_ECEF1);
+ // 2 from latlonalt
+ computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF2, t_ENU_ECEF2);
+ // 3 Hardcoded solution
+ /* Convert ECEF coordinates to geodetic coordinates.
+ * J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
+ * to geodetic coordinates," IEEE Transactions on Aerospace and
+ * Electronic Systems, vol. 30, pp. 957-961, 1994.
+ */
+ double r = std::sqrt(t_ECEF_ENU(0) * t_ECEF_ENU(0) + t_ECEF_ENU(1) * t_ECEF_ENU(1));
+ double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+ double F = 54 * kSemiminorAxis * kSemiminorAxis * t_ECEF_ENU(2) * t_ECEF_ENU(2);
+ double G = r * r + (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq;
+ double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
+ double S = cbrt(1 + C + sqrt(C * C + 2 * C));
+ double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
+ double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
+ double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
+ + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
+ - P * (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * r * r);
+ double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2));
+ double Z_0 = kSemiminorAxis * kSemiminorAxis * t_ECEF_ENU(2) / (kSemimajorAxis * V);
+
+ double latitude = atan((t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
+ double longitude = atan2(t_ECEF_ENU(1), t_ECEF_ENU(0));
+
+ double sLat = sin(latitude);
+ double cLat = cos(latitude);
+ double sLon = sin(longitude);
+ double cLon = cos(longitude);
+
+ R_ENU_ECEF3(0,0) = -sLon;
+ R_ENU_ECEF3(0,1) = cLon;
+ R_ENU_ECEF3(0,2) = 0.0;
+
+ R_ENU_ECEF3(1,0) = -sLat*cLon;
+ R_ENU_ECEF3(1,1) = -sLat * sLon;
+ R_ENU_ECEF3(1,2) = cLat;
+
+ R_ENU_ECEF3(2,0) = cLat * cLon;
+ R_ENU_ECEF3(2,1) = cLat * sLon;
+ R_ENU_ECEF3(2,2) = sLat;
+
+ t_ENU_ECEF3 = -R_ENU_ECEF3*t_ECEF_ENU;
+
+ // CHECK
+ ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF2,1e-6);
+ ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF2,1e-6);
+ ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF3,1e-6);
+ ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF3,1e-6);
+ }
+}
+
+TEST(TransformationsTest, computeSatElevation)
+{
+ Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt,rec_sat_ENU, sat_ECEF;
+ Eigen::Matrix3d R_ENU_ECEF;
+
+ // random receiver position
+ for (auto i = 0; i<100; i++)
+ {
+ t_ENU_latlonalt = Eigen::Vector3d::Random();
+ t_ENU_latlonalt(0) *= M_PI / 2;
+ t_ENU_latlonalt(1) *= M_PI;
+ t_ENU_latlonalt(2) *= 1e3;
+
+ t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
+
+ computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
+
+ // random elevation and heading
+ for (auto j = 0; j<100; j++)
+ {
+ Eigen::Vector2d rand2 = Eigen::Vector2d::Random();
+ double heading = rand2(0) * M_PI;
+ double elevation = rand2(0) * M_PI / 2;
+
+ rec_sat_ENU << cos(heading) * 1000, sin(heading) * 1000, tan(elevation) * 1000;
+
+ sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * rec_sat_ENU;
+
+ double elevation2 = computeSatElevation(t_ECEF_ENU, sat_ECEF);
- ASSERT_DOUBLE_EQ(latlonalt(0),0.0);
- ASSERT_DOUBLE_EQ(latlonalt(1),0.0);
+ ASSERT_NEAR(elevation, elevation2,1e-6);
+ }
+ }
}
int main(int argc, char **argv)
--
GitLab