From 50110ac7d80ebcd9efd6616df56eba886a0819a2 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 9 Dec 2024 11:56:50 +0100 Subject: [PATCH] Implement world georeferenciation via UTM zone number --- docs/sensors.rst | 4 +++- docs/world_georeference.rst | 23 +++++++++++++++++-- modules/simulator/include/mvsim/World.h | 4 ++++ .../src/WorldElements/ElevationMap.cpp | 4 ++-- modules/simulator/src/World_load_xml.cpp | 21 +++++++++++++++++ 5 files changed, 51 insertions(+), 5 deletions(-) diff --git a/docs/sensors.rst b/docs/sensors.rst index 35f869d1..6c37225a 100644 --- a/docs/sensors.rst +++ b/docs/sensors.rst @@ -348,6 +348,8 @@ Depth (RGBD) camera :language: xml +.. _sensors-gps: + GNSS sensor ("GPS") --------------------- A "GPS sensor" can be attached to a robot with the code shown below. @@ -358,7 +360,7 @@ For it to work, the ``world`` XML needs to have a :ref:`georeference tag ` ("GPS") to work, they need to know the transformation between the simulator world local coordinates and the Earth. This can be specified with a ```` tag as documented below. .. code-block:: xml - :caption: Georeference example (Using geodetic coordinates) + :caption: Georeference example #1: Using geodetic coordinates ... @@ -23,9 +23,28 @@ This can be specified with a ```` tag as documented below. ... +.. code-block:: xml + :caption: Georeference example #2: Using UTM world coordinates + + + ... + + + 30 + + ... + + + +Parameters for worlds in local coordinates and GNSS geodetic reference: - ``yy`` and ``xx``. Geodetic coordinates of the world (0,0,0) origin. - ``zz``: The height over the WGS84 ellipsoid for the world (0,0,0). - ``tt``: An optional rotation (in degrees) if you want North not to be aligned with +Y as it is the default. + + +Parameters for worlds with local coordinates as `UTM coordinates `_: + +- ``nn``: Specify the UTM zone number (longitude), positive/negative for Northern/Southern Hemisphere. diff --git a/modules/simulator/include/mvsim/World.h b/modules/simulator/include/mvsim/World.h index 61186958..e3019ef9 100644 --- a/modules/simulator/include/mvsim/World.h +++ b/modules/simulator/include/mvsim/World.h @@ -604,11 +604,15 @@ class World : public mrpt::system::COutputLogger */ double world_to_enu_rotation = .0; + /** UTM zone, positive/negative for Northern/Southern Hemisphere */ + int utm_zone = 0; + 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}}, }; }; diff --git a/modules/simulator/src/WorldElements/ElevationMap.cpp b/modules/simulator/src/WorldElements/ElevationMap.cpp index 54d59d08..0d5229e1 100644 --- a/modules/simulator/src/WorldElements/ElevationMap.cpp +++ b/modules/simulator/src/WorldElements/ElevationMap.cpp @@ -187,7 +187,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) const auto ny = static_cast(std::ceil((maxy - miny) / resolution_)); parent()->logFmt( - mrpt::system::LVL_INFO, + mrpt::system::LVL_DEBUG, "[ElevationMap] Loaded %u points, min_corner=(%lf,%lf), max_corner=(%lf,%lf), " "cells=(%u,%u)", static_cast(data.rows()), minx, miny, maxx, maxy, nx, ny); @@ -284,7 +284,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) } parent()->logFmt( - mrpt::system::LVL_INFO, "[ElevationMap] Applying filtering convolution filter %ux%u", + mrpt::system::LVL_DEBUG, "[ElevationMap] Applying filtering convolution filter %ux%u", static_cast(kernel.rows()), static_cast(kernel.cols())); elevation_data = applyConvolution(elevation_data, kernel); diff --git a/modules/simulator/src/World_load_xml.cpp b/modules/simulator/src/World_load_xml.cpp index 2389e80a..d6e8147d 100644 --- a/modules/simulator/src/World_load_xml.cpp +++ b/modules/simulator/src/World_load_xml.cpp @@ -11,6 +11,7 @@ #include #include #include // extractFileDirectory() +#include #include #include // count() @@ -232,6 +233,26 @@ void World::parse_tag_lights(const XmlParserContext& ctx) void World::parse_tag_georeference(const XmlParserContext& ctx) { georeferenceOptions_.parse_from(*ctx.node, *this); + + // handle UTM: + auto& g = georeferenceOptions_; + + if (g.utm_zone != 0) + { + ASSERTMSG_( + g.georefCoord.isClear(), "Cannot define both, and geodetics coordinates"); + + // we will use the (lat,lon) of UTM origin as reference for this world: + const mrpt::topography::TUTMCoords utmCoords = {0, 0, 0}; + + mrpt::topography::UTMToGeodetic( + utmCoords, std::abs(g.utm_zone), g.utm_zone < 0 ? 'S' : 'N', g.georefCoord); + + MRPT_LOG_DEBUG_STREAM( + "Using UTM georeference: utm_zone=" // + << g.utm_zone << " geoRef: lat=" << g.georefCoord.lat.getDecimalValue() + << " lon=" << g.georefCoord.lon.getDecimalValue()); + } } void World::parse_tag_walls(const XmlParserContext& ctx) { process_load_walls(*ctx.node); }