Merge branch 'devel' into 'main'

Devel

See merge request !27

.gitmodules

 [submodule "deps/RTKLIB"]
 	path = deps/RTKLIB
-	url = https://gitlab.iri.upc.edu/mobile_robotics/gauss_project/RTKLIB.git
+	url = https://gitlab.iri.upc.edu/labrobotica/algorithms/RTKLIB.git
 	branch = devel

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);
 

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;
 }

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;