Alternative reference method for UTM world coordinates

docs/world_georeference.rst

@@ -30,7 +30,11 @@ This can be specified with a ``<georeference>`` tag as documented below.
     ...
       <!-- Define georeferenced coordinates to the world so GNSS/GPS sensors can be properly simulated -->
       <georeference>
-        <utm_zone>30</utm_zone>
+        <world_is_utm>true</world_is_utm>
+        <!-- A reference point roughly within the area of the map to linearize coordinates -->
+        <latitude>36.894718</latitude>
+        <longitude>-2.316988</longitude>
+        <height>100.0</height>
       </georeference>
     ...
     </mvsim_world>
@@ -42,9 +46,9 @@ Parameters for worlds in local coordinates and GNSS geodetic reference:
 
 - ``<height>zz</height>``: The height over the WGS84 ellipsoid for the world (0,0,0).
 
-- ``<world_to_enu_rotation_deg>tt</world_to_enu_rotation_deg>``: An optional rotation (in degrees) if you want North not to be aligned with +Y as it is the default.
+- ``<world_to_enu_rotation_deg>tt</world_to_enu_rotation_deg>``: An optional rotation (in degrees) if you want North not to be aligned with +Y as it is the default. Does not apply when using UTM coordinates.
 
 
 Parameters for worlds with local coordinates as `UTM coordinates <https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system>`_:
 
-- ``<utm_zone>nn</utm_zone>``: Specify the UTM zone number (longitude), positive/negative for Northern/Southern Hemisphere.
+- ``<world_is_utm>true</world_is_utm>``: Indicates that world coordinates are UTM coordinates, defined in the zone of the reference (lat,lon) geodetic coordinates.

modules/simulator/include/mvsim/World.h

@@ -604,15 +604,20 @@ class World : public mrpt::system::COutputLogger
 		 */
 		double world_to_enu_rotation = .0;
 
-		/** UTM zone, positive/negative for Northern/Southern Hemisphere */
-		int utm_zone = 0;
+		/** Is world in UTM coordinates? */
+		bool world_is_utm = false;
+
+		/** The UTM coords of georefCoord (calculated on start up) */
+		mrpt::topography::TUTMCoords utmRef;
+		int utm_zone = 0; // auto calculated
+		char utm_band = 'X'; // auto calculated
 
 		const TParameterDefinitions params = {
 			{"latitude", {"%lf", &georefCoord.lat.decimal_value}},
 			{"longitude", {"%lf", &georefCoord.lon.decimal_value}},
 			{"height", {"%lf", &georefCoord.height}},
 			{"world_to_enu_rotation_deg", {"%lf_deg", &world_to_enu_rotation}},
-			{"utm_zone", {"%i", &utm_zone}},
+			{"world_is_utm", {"%bool", &world_is_utm}},
 		};
 	};
 

modules/simulator/src/Sensors/GNSS.cpp

@@ -126,8 +126,19 @@ void GNSS::internal_simulate_gnss(const TSimulContext& context)
 	const auto worldRotation =
 		mrpt::poses::CPose3D::FromYawPitchRoll(georef.world_to_enu_rotation, .0, .0);
 
+	mrpt::poses::CPose3D vehPoseInWorld = vehicle().getCPose3D();
+
+	if (georef.world_is_utm)
+	{
+		auto posLocal = vehPoseInWorld.translation() - georef.utmRef;
+
+		vehPoseInWorld.x(posLocal.x);
+		vehPoseInWorld.y(posLocal.y);
+		vehPoseInWorld.z(posLocal.z);
+	}
+
 	const mrpt::math::TPoint3D sensorPt =
-		(worldRotation + (vehicle().getCPose3D() + outObs->sensorPose)).translation() + noise;
+		(worldRotation + (vehPoseInWorld + outObs->sensorPose)).translation() + noise;
 
 	// convert from ENU (world coordinates) to geodetic:
 	const thread_local auto WGS84 = mrpt::topography::TEllipsoid::Ellipsoid_WGS84();
@@ -143,9 +154,10 @@ void GNSS::internal_simulate_gnss(const TSimulContext& context)
 			once = true;
 			world()->logStr(
 				mrpt::system::LVL_WARN,
-				"It seems world <georeference> parameters are not set, and they are required for "
+				"World <georeference> parameters are not set, and they are required for "
 				"properly define GNSS sensor simulation");
 		}
+		return;
 	}
 
 	mrpt::topography::TGeocentricCoords gcPt;

modules/simulator/src/World_load_xml.cpp

@@ -237,21 +237,20 @@ void World::parse_tag_georeference(const XmlParserContext& ctx)
 	// handle UTM:
 	auto& g = georeferenceOptions_;
 
-	if (g.utm_zone != 0)
+	if (g.world_is_utm)
 	{
 		ASSERTMSG_(
-			g.georefCoord.isClear(), "Cannot define both, <utm_zone> and geodetics coordinates");
+			g.world_to_enu_rotation == 0,
+			"Cannot define both, <world_to_enu_rotation> and <world_is_utm>");
 
-		// we will use the (lat,lon) of UTM origin as reference for this world:
-		const mrpt::topography::TUTMCoords utmCoords = {0, 0, 0};
+		ASSERTMSG_(
+			!g.georefCoord.isClear(),
+			"<world_is_utm> requires defining a valid reference geodetic coordinates too.");
 
-		mrpt::topography::UTMToGeodetic(
-			utmCoords, std::abs(g.utm_zone), g.utm_zone < 0 ? 'S' : 'N', g.georefCoord);
+		mrpt::topography::GeodeticToUTM(g.georefCoord, g.utmRef, g.utm_zone, g.utm_band);
 
-		MRPT_LOG_DEBUG_STREAM(
-			"Using UTM georeference: utm_zone="	 //
-			<< g.utm_zone << " geoRef: lat=" << g.georefCoord.lat.getDecimalValue()
-			<< " lon=" << g.georefCoord.lon.getDecimalValue());
+		MRPT_LOG_INFO_STREAM(
+			"Using UTM georeference: utm_zone=" << g.utm_zone << " geoRef: utmRef=" << g.utmRef);
 	}
 }