Support lunar coordinate transformations

geospatial/include/gz/common/geospatial/Dem.hh

@@ -23,6 +23,7 @@
 
 #include <gz/math/Vector3.hh>
 #include <gz/math/Angle.hh>
+#include <gz/math/SphericalCoordinates.hh>
 
 #include <gz/common/geospatial/Export.hh>
 #include <gz/common/geospatial/HeightmapData.hh>
@@ -44,21 +45,18 @@ namespace gz
       /// \brief Destructor.
       public: virtual ~Dem();
 
+      /// \brief Sets the spherical coordinates reference object.
+      /// \param[in] _worldSphericalCoordinates The spherical coordiantes
+      /// object contained in the world. This is used to compute accurate
+      /// sizes of the DEM object.
+      public: void SetSphericalCoordinates(
+                  const math::SphericalCoordinates &_worldSphericalCoordinates);
+
       /// \brief Load a DEM file.
       /// \param[in] _filename the path to the terrain file.
       /// \return 0 when the operation succeeds to open a file.
       public: int Load(const std::string &_filename = "");
 
-      /// \brief Indicate that this is a non Earth DEM.
-      /// \param[in] _isNonEarthDem Should be true if this is a
-      /// non earth DEM, otherwise false.
-      public: void SetNonEarthDEM(bool _isNonEarthDem);
-
-      /// \brief Check if the loaded DEM is not from the Earth.
-      /// \return True if the loaded DEM is from the Earth, otherwise
-      /// returns False.
-      public: bool GetNonEarthDEM();
-
       /// \brief Get the elevation of a terrain's point in meters.
       /// \param[in] _x X coordinate of the terrain.
       /// \param[in] _y Y coordinate of the terrain.
@@ -75,7 +73,7 @@ namespace gz
       public: float MaxElevation() const override;
 
       /// \brief Get the georeferenced coordinates (lat, long) of the terrain's
-      /// origin in WGS84.
+      /// origin.
       /// \param[out] _latitude Georeferenced latitude.
       /// \param[out] _longitude Georeferenced longitude.
       /// \return True if able to retrieve origin coordinates. False otherwise.
@@ -126,7 +124,7 @@ namespace gz
                   std::vector<float> &_heights) const override;
 
       /// \brief Get the georeferenced coordinates (lat, long) of a terrain's
-      /// pixel in WGS84.
+      /// pixel.
       /// \param[in] _x X coordinate of the terrain.
       /// \param[in] _y Y coordinate of the terrain.
       /// \param[out] _latitude Georeferenced latitude.

geospatial/src/Dem.cc

@@ -22,7 +22,6 @@
 
 #include "gz/common/Console.hh"
 #include "gz/common/geospatial/Dem.hh"
-#include "gz/math/SphericalCoordinates.hh"
 
 using namespace gz;
 using namespace common;
@@ -59,10 +58,14 @@ class gz::common::Dem::Implementation
   /// \brief Full filename used to load the dem.
   public: std::string filename;
 
-  /// \brief Whether the DEM will be handled as from non-Earth.
+  /// \brief Whether the DEM will be handled as unknown.
   /// If true, worldWidth & worldHeight = -1
   /// and GeoReference[Origin] can not be used (will return false)
-  public: bool isNonEarthDem = false;
+  public: bool isUnknownDem = false;
+
+  /// \brief Holds the spherical coordinates object from the world.
+  public: math::SphericalCoordinates sphericalCoordinates =
+           math::SphericalCoordinates();
 };
 
 //////////////////////////////////////////////////
@@ -83,15 +86,10 @@ Dem::~Dem()
 }
 
 //////////////////////////////////////////////////
-void Dem::SetNonEarthDEM(bool _isNonEarthDem)
-{
-  this->dataPtr->isNonEarthDem = _isNonEarthDem;
-}
-
-//////////////////////////////////////////////////
-bool Dem::GetNonEarthDEM()
+void Dem::SetSphericalCoordinates(
+    const math::SphericalCoordinates &_worldSphericalCoordinates)
 {
-  return this->dataPtr->isNonEarthDem;
+  this->dataPtr->sphericalCoordinates =_worldSphericalCoordinates;
 }
 
 //////////////////////////////////////////////////
@@ -157,21 +155,20 @@ int Dem::Load(const std::string &_filename)
   {
     // If successful, set the world width and height
     this->dataPtr->worldWidth =
-       math::SphericalCoordinates::DistanceWGS84(upLeftLat, upLeftLong,
-                                              upRightLat, upRightLong);
+       this->dataPtr->sphericalCoordinates.DistanceBetweenPoints(
+           upLeftLat, upLeftLong, upRightLat, upRightLong);
     this->dataPtr->worldHeight =
-       math::SphericalCoordinates::DistanceWGS84(upLeftLat, upLeftLong,
-                                              lowLeftLat, lowLeftLong);
+       this->dataPtr->sphericalCoordinates.DistanceBetweenPoints(
+           upLeftLat, upLeftLong, lowLeftLat, lowLeftLong);
   }
-  // Assume non-Earth DEM (e.g., moon)
+  // Assume unknown DEM.
   else
   {
     gzwarn << "Failed to automatically compute DEM size. "
-            << "Assuming non-Earth DEM. "
             << std::endl;
 
     this->dataPtr->worldWidth = this->dataPtr->worldHeight = -1;
-    this->dataPtr->isNonEarthDem = true;
+    this->dataPtr->isUnknownDem = true;
   }
 
   // Set the terrain's side (the terrain will be squared after the padding)
@@ -275,35 +272,62 @@ float Dem::MaxElevation() const
 bool Dem::GeoReference(double _x, double _y,
     gz::math::Angle &_latitude, gz::math::Angle &_longitude) const
 {
-  if (this->dataPtr->isNonEarthDem)
+  if (this->dataPtr->isUnknownDem)
   {
-    gzerr << "Can not retrieve WGS84 coordinates from non-Earth DEM."
+    gzerr << "Can not retrieve coordinates from unknown DEM."
             << std::endl;
     return false;
   }
 
   double geoTransf[6];
   if (this->dataPtr->dataSet->GetGeoTransform(geoTransf) == CE_None)
   {
+    OGRCoordinateTransformation *cT = nullptr;
+    double xGeoDeg, yGeoDeg;
     OGRSpatialReference sourceCs;
     OGRSpatialReference targetCs;
-    OGRCoordinateTransformation *cT;
-    double xGeoDeg, yGeoDeg;
 
-    // Transform the terrain's coordinate system to WGS84
-    const char *importString
-        = strdup(this->dataPtr->dataSet->GetProjectionRef());
-    if (importString == nullptr || importString[0] == '\0')
+    if (this->dataPtr->sphericalCoordinates.Surface() ==
+        math::SphericalCoordinates::EARTH_WGS84)
     {
-      gzdbg << "Projection coordinate system undefined." << std::endl;
-      return false;
+      // Transform the terrain's coordinate system to WGS84
+      const char *importString
+          = strdup(this->dataPtr->dataSet->GetProjectionRef());
+      if (importString == nullptr || importString[0] == '\0')
+      {
+        // LCOV_EXCL_START
+        gzdbg << "Projection coordinate system undefined." << std::endl;
+        return false;
+        // LCOV_EXCL_STOP
+      }
+      sourceCs.importFromWkt(&importString);
+      targetCs.SetWellKnownGeogCS("WGS84");
     }
-    sourceCs.importFromWkt(&importString);
-    targetCs.SetWellKnownGeogCS("WGS84");
+    else if ((this->dataPtr->sphericalCoordinates.Surface() ==
+        math::SphericalCoordinates::CUSTOM_SURFACE) ||
+        (this->dataPtr->sphericalCoordinates.Surface() ==
+         math::SphericalCoordinates::MOON_SCS))
+    {
+      sourceCs = *(this->dataPtr->dataSet->GetSpatialRef());
+      targetCs = OGRSpatialReference();
+
+      double axisEquatorial =
+        this->dataPtr->sphericalCoordinates.SurfaceAxisEquatorial();
+      double axisPolar =
+        this->dataPtr->sphericalCoordinates.SurfaceAxisPolar();
+
+      std::string surfaceLatLongProjStr =
+        "+proj=latlong +a=" + std::to_string(axisEquatorial) +
+        " +b=" + std::to_string(axisPolar);
+
+      targetCs.importFromProj4(surfaceLatLongProjStr.c_str());
+    }
+
     cT = OGRCreateCoordinateTransformation(&sourceCs, &targetCs);
+
     if (nullptr == cT)
     {
-      gzerr << "Unable to transform terrain coordinate system to WGS84 for "
+      gzerr << "Unable to transform terrain coordinate system for "
              << "coordinates (" << _x << "," << _y << ")" << std::endl;
       OCTDestroyCoordinateTransformation(cT);
       return false;
@@ -350,9 +374,9 @@ unsigned int Dem::Width() const
 //////////////////////////////////////////////////
 double Dem::WorldWidth() const
 {
-  if (this->dataPtr->isNonEarthDem)
+  if (this->dataPtr->isUnknownDem)
   {
-    gzwarn << "Unable to determine world width of non-Earth DEM."
+    gzwarn << "Unable to determine world width of unknown DEM."
             << std::endl;
   }
   return this->dataPtr->worldWidth;
@@ -361,9 +385,9 @@ double Dem::WorldWidth() const
 //////////////////////////////////////////////////
 double Dem::WorldHeight() const
 {
-  if (this->dataPtr->isNonEarthDem)
+  if (this->dataPtr->isUnknownDem)
   {
-    gzwarn << "Unable to determine world height of non-Earth DEM."
+    gzwarn << "Unable to determine world height of unknown DEM."
             << std::endl;
   }
   return this->dataPtr->worldHeight;

geospatial/src/Dem_TEST.cc

@@ -127,6 +127,18 @@ TEST_F(DemTest, BasicAPI)
   EXPECT_TRUE(dem.GeoReferenceOrigin(latitude, longitude));
   EXPECT_FLOAT_EQ(38.001667f, latitude.Degree());
   EXPECT_FLOAT_EQ(-122.22278f, longitude.Degree());
+
+  // Emulate Earth as a custom surface.
+  common::Dem demCustomSurface;
+  auto earthSc = math::SphericalCoordinates();
+  auto customSc = math::SphericalCoordinates(
+      math::SphericalCoordinates::CUSTOM_SURFACE,
+      earthSc.SurfaceRadius(),
+      earthSc.SurfaceRadius());
+  demCustomSurface.SetSphericalCoordinates(customSc);
+  EXPECT_EQ(demCustomSurface.Load(path), 0);
+  EXPECT_FLOAT_EQ(3984.4849f, demCustomSurface.WorldHeight());
+  EXPECT_FLOAT_EQ(3139.7456f, demCustomSurface.WorldWidth());
 }
 
 /////////////////////////////////////////////////
@@ -226,7 +238,7 @@ TEST_F(DemTest, NaNNoData)
 }
 
 /////////////////////////////////////////////////
-TEST_F(DemTest, NonEarthDem)
+TEST_F(DemTest, UnknownDem)
 {
   // moon
   common::Dem dem;
@@ -245,12 +257,28 @@ TEST_F(DemTest, NonEarthDem)
   // unable to get coordinates in WGS84
   gz::math::Angle latitude, longitude;
   EXPECT_FALSE(dem.GeoReferenceOrigin(latitude, longitude));
+}
 
-  // The Load() method in Dem.cc should set the
-  // isNonEarthDEM flag.
-  EXPECT_TRUE(dem.GetNonEarthDEM());
-
-  // This flag can be overridden externally.
-  dem.SetNonEarthDEM(false);
-  EXPECT_FALSE(dem.GetNonEarthDEM());
+TEST_F(DemTest, LunarDemLoad)
+{
+  // Load Moon DEM
+  common::Dem dem;
+  auto path = common::testing::TestFile("data", "dem_moon.tif");
+  // Providing spherical coordinates object.
+  auto moonSc = math::SphericalCoordinates(
+      math::SphericalCoordinates::MOON_SCS);
+  dem.SetSphericalCoordinates(moonSc);
+  EXPECT_EQ(dem.Load(path), 0);
+  EXPECT_NEAR(dem.WorldWidth(), 80.0417, 1e-2);
+  EXPECT_NEAR(dem.WorldHeight(), 80.0417, 1e-2);
+
+  // Use custom spherical coordinates object with same axes as the moon.
+  auto customSc = math::SphericalCoordinates(
+      math::SphericalCoordinates::CUSTOM_SURFACE,
+      moonSc.SurfaceAxisEquatorial(),
+      moonSc.SurfaceAxisPolar());
+  dem.SetSphericalCoordinates(customSc);
+  EXPECT_EQ(dem.Load(path), 0);
+  EXPECT_NEAR(dem.WorldWidth(), 80.0417, 1e-2);
+  EXPECT_NEAR(dem.WorldHeight(), 80.0417, 1e-2);
 }