Skip to content
Snippets Groups Projects
Commit 8309b1ff authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

rads and deg in latlonalt transformations

parent cde27071
No related branches found
No related tags found
1 merge request!27Devel
Pipeline #14191 passed
......@@ -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);
......
......@@ -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;
}
......
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment