GeoReferenceParser.cpp

Go to the documentation of this file.

// Copyright (c) 2026 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "carla/opendrive/parser/GeoReferenceParser.h"
 
#include "carla/Logging.h"
#include "carla/StringUtil.h"
#include "carla/geom/GeoLocation.h"
#include "carla/geom/GeoProjection.h"
#include "carla/road/MapBuilder.h"
#include "carla/geom/Math.h"
 
#include <third-party/pugixml/pugixml.hpp>
 
#include <string>
#include <vector>
#include <regex>
#include <utility>
#include <cctype>
#include <stdexcept>
 
namespace carla {
namespace opendrive {
namespace parser {
 
  template <typename S>
  static std::string ToLowerCase(S&& s) {
    std::string r = s;
    for (auto& c : r)
      c = static_cast<char>(std::tolower(static_cast<unsigned char>(c)));
    return r;
  }
 
  static std::unordered_map<std::string, std::string> ParseProjectionParameters(const std::string &s){
 
    // +key[=value]; value can be "quoted" or 'quoted' or an unquoted token (no spaces)
    // This also captures bare flags like +south
    std::unordered_map<std::string, std::string> geo_parameters_map;
 
    static const std::regex re(R"(\+([A-Za-z0-9_]+)(?:=("[^"]*"|'[^']*'|[^ \t\r\n+]+))?)");
    for (std::sregex_iterator it(s.begin(), s.end(), re), end; it != end; ++it) {
        std::string key = (*it)[1].str();
        std::string val = (*it)[2].str();
        geo_parameters_map[key] = val;
    }
 
    return geo_parameters_map;
  }
 
  static std::unordered_map<std::string, double> ParseOffsetParameters(const pugi::xml_node &geo_offset){
 
    std::unordered_map<std::string, double> geo_offset_result;
 
    for (auto attr = geo_offset.first_attribute(); attr; attr = attr.next_attribute()){
      std::string key = attr.name();
      double value = attr.as_double();
      geo_offset_result[key] = value;
    }
    return geo_offset_result;
  }
 
 
  template <typename M, typename S>
  static bool TryGetParameter(std::string& out, M& parameters, S&& name)
  {
    auto i = parameters.find(std::forward<S>(name));
    if (i == parameters.cend())
      return false;
    out = i->second;
    return true;
  }
 
  template <typename M, typename S, typename O>
  static
  std::enable_if_t<std::is_floating_point<O>::value, bool>
  TryGetParameter(O& out, M& parameters, S&& name)
  {
    auto i = parameters.find(std::forward<S>(name));
    if (i == parameters.cend())
      return false;
    out = static_cast<O>(std::stod(i->second));
    return true;
  }
 
  template <typename M, typename S, typename O>
  static
  std::enable_if_t<std::is_integral<O>::value && std::is_unsigned<O>::value, bool>
  TryGetParameter(O& out, M& parameters, S&& name)
  {
    auto i = parameters.find(std::forward<S>(name));
    if (i == parameters.cend())
      return false;
    out = static_cast<O>(std::stoull(i->second));
    return true;
  }
 
  template <typename M, typename S, typename O>
  static
  std::enable_if_t<std::is_integral<O>::value && std::is_signed<O>::value, bool>
  TryGetParameter(O& out, M& parameters, S&& name)
  {
    auto i = parameters.find(std::forward<S>(name));
    if (i == parameters.cend())
      return false;
    out = static_cast<O>(std::stoll(i->second));
    return true;
  }
 
  template <typename S, typename O>
  static bool TryGetParameter(O& out,std::unordered_map<std::string, double>& parameters,S&& name)
  {
    auto i = parameters.find(std::forward<S>(name));
    if (i == parameters.cend()){
      return false;
    }
    out = static_cast<O>(i->second);
    return true;
  }
 
 
  static geom::Ellipsoid CreateEllipsoid(std::unordered_map<std::string, std::string> parameters){
 
    geom::Ellipsoid ellps;
    std::string value;
    bool initialized=false;
    double x;
 
    if (TryGetParameter(value, parameters, "ellps") || TryGetParameter(value, parameters, "datum")) {
      value = ToLowerCase(value);
      auto it = geom::custom_ellipsoids.find(value);
      if (it != geom::custom_ellipsoids.end()) {
        ellps.a = it->second.first;
        ellps.f_inv = it->second.second;
        initialized=true;
      }
    }
 
    initialized |= TryGetParameter(ellps.a, parameters, "a");
    // Specific semi-major axis
    if (TryGetParameter(x, parameters, "b")) {
      initialized = true;
      ellps.fromb(x);
    }
    if (TryGetParameter(x, parameters, "f")) {
      initialized = true;
      ellps.fromf(x);
    }
    initialized |= TryGetParameter(ellps.f_inv, parameters, "rf");
 
    if (!initialized)    {
      auto val = geom::custom_ellipsoids.find("wgs84");
      ellps.a = val->second.first;
      ellps.f_inv = val->second.second;
    }
 
    return ellps;
  }
 
  static boost::optional<geom::OffsetTransform> CreateOffsetTransform(std::unordered_map<std::string, double>& offset_parameters){
 
    if(offset_parameters.empty()){
      return boost::none;
    }
 
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    double hdg = 0.0;
 
    TryGetParameter(x, offset_parameters, "x");
    TryGetParameter(y, offset_parameters, "y");
    TryGetParameter(z, offset_parameters, "z");
    TryGetParameter(hdg, offset_parameters, "hdg");
 
    return geom::OffsetTransform{x, y, z, hdg};
  }
 
  static geom::GeoProjection CreateTransverseMercatorProjection(
    std::unordered_map<std::string, std::string> parameters,
    std::string proj_string,
    geom::Ellipsoid ellipsoid){
 
    geom::TransverseMercatorParams p;
    TryGetParameter(p.lat_0, parameters, "lat_0");
    TryGetParameter(p.lon_0, parameters, "lon_0");
    TryGetParameter(p.k, parameters, "k");
    TryGetParameter(p.x_0, parameters, "x_0");
    TryGetParameter(p.y_0, parameters, "y_0");
    p.ellps = ellipsoid;
    auto projection = geom::GeoProjection::Make(p);
    projection.SetPROJString(proj_string);
    return projection;
  }
 
  static geom::GeoProjection CreateUniversalTransverseMercatorProjection(
    std::unordered_map<std::string, std::string> parameters,
    std::string proj_string,
    geom::Ellipsoid ellipsoid, boost::optional<carla::geom::OffsetTransform> offset){
 
    geom::UniversalTransverseMercatorParams p;
    if (!TryGetParameter(p.zone, parameters, "zone")) {
      log_warning("Missing 'zone' parameter for UTM projection. Using default value " + std::to_string(p.zone));
    }
    p.north = (parameters.count("south") > 0) ? false : true;
    p.ellps = ellipsoid;
    p.offset = offset;
    auto projection = geom::GeoProjection::Make(p);
    projection.SetPROJString(proj_string);
    return projection;
  }
 
  static geom::GeoProjection CreateWebMercatorProjection(
    std::string proj_string,
    geom::Ellipsoid ellipsoid){
 
    // Parameters are fixed.
    geom::WebMercatorParams p;
    p.ellps = ellipsoid;
    auto projection = geom::GeoProjection::Make(p);
    projection.SetPROJString(proj_string);
    return projection;
  }
 
  static geom::GeoProjection CreateLambertConformalConicProjection(
    std::unordered_map<std::string, std::string> parameters,
    std::string proj_string,
    geom::Ellipsoid ellipsoid){
 
    geom::LambertConformalConicParams p;
    TryGetParameter(p.lon_0, parameters, "lon_0");
    TryGetParameter(p.lat_0, parameters, "lat_0");
    if (!TryGetParameter(p.lat_1, parameters, "lat_1"))
      log_warning("Missing 'lat_1' parameter for LCC projection. Using default value " + std::to_string(p.lat_1));
    if (!TryGetParameter(p.lat_2, parameters, "lat_2"))
      log_warning("Missing 'lat_2' parameter for LCC projection. Using default value " + std::to_string(p.lat_2));
    TryGetParameter(p.x_0, parameters, "x_0");
    TryGetParameter(p.y_0, parameters, "y_0");
    p.ellps = ellipsoid;
    auto projection = geom::GeoProjection::Make(p);
    projection.SetPROJString(proj_string);
    return projection;
  }
 
  // TransverseMercator projection with default parameters.
  static geom::GeoProjection CreateDefaultProjection(geom::Ellipsoid ellipsoid){
    geom::TransverseMercatorParams p;
 
    p.ellps = ellipsoid;
    auto projection = geom::GeoProjection::Make(p);
    return projection;
  }
 
  static geom::GeoLocation CreateTransverseMercatorGeoReference(std::unordered_map<std::string, std::string> parameters){
    geom::GeoLocation result{0.0, 0.0, 0.0};
    TryGetParameter(result.latitude, parameters, "lat_0");
    TryGetParameter(result.longitude, parameters, "lon_0");
    return result;
  }
 
  static geom::GeoLocation CreateUniversalTransverseMercatorGeoReference(std::unordered_map<std::string, std::string> parameters){
    geom::GeoLocation result{0.0, 0.0, 0.0};
    double zone = 0.0;
    if (TryGetParameter(zone, parameters, "zone"))
      result.longitude = 6.0 * zone - 183.0;
    return result;
  }
 
  static geom::GeoLocation CreateWebMercatorGeoReference(){
    geom::GeoLocation result{0.0, 0.0, 0.0};
    return result;
  }
 
  static geom::GeoLocation CreateLambertConic2SPGeoReference(std::unordered_map<std::string, std::string> parameters){
    geom::GeoLocation result{0.0, 0.0, 0.0};
    TryGetParameter(result.latitude, parameters, "lat_0");
    TryGetParameter(result.longitude, parameters, "lon_0");
    return result;
  }
 
  static geom::GeoLocation CreateDefaultGeoReference(){
    geom::GeoLocation result{0.0, 0.0, 0.0};
    return result;
  }
 
  static auto ParseGeoProjectionAndReference(const std::string& proj_string, const pugi::xml_node &geo_offset){
    auto parameters = ParseProjectionParameters(proj_string);
    auto offset_parameters = ParseOffsetParameters(geo_offset);
    auto ellipsoid = CreateEllipsoid(parameters);
    auto offset_transform = CreateOffsetTransform(offset_parameters);
 
    // Get the projection type
    std::string proj;
    if (!TryGetParameter(proj, parameters, "proj")) {
      log_warning("cannot find the type of projection, using default transverse mercator");
      return std::make_pair(
        CreateDefaultProjection(ellipsoid),
        CreateDefaultGeoReference());
    }
 
    // Parse the parameters
    if (proj == "tmerc") {
      return std::make_pair(
        CreateTransverseMercatorProjection(parameters, proj_string, ellipsoid),
        CreateTransverseMercatorGeoReference(parameters));
    } else if (proj == "utm") {
      return std::make_pair(
        CreateUniversalTransverseMercatorProjection(parameters, proj_string, ellipsoid, offset_transform),
        CreateUniversalTransverseMercatorGeoReference(parameters));
    } else if (proj == "merc") {
      return std::make_pair(
        CreateWebMercatorProjection(proj_string, ellipsoid),
        CreateWebMercatorGeoReference());
    } else if (proj == "lcc") {
      return std::make_pair(
        CreateLambertConformalConicProjection(parameters, proj_string, ellipsoid),
        CreateLambertConic2SPGeoReference(parameters));
    } else {
      log_debug("projection '" + proj + "' is not supported, using default transverse mercator.");
      return std::make_pair(
        CreateDefaultProjection(ellipsoid),
        CreateDefaultGeoReference());
    }
  }
 
  void GeoReferenceParser::Parse(
      const pugi::xml_document &xml,
      carla::road::MapBuilder &map_builder) {
 
    auto georeference_string =
      xml
      .child("OpenDRIVE")
      .child("header")
      .child_value("geoReference");
 
    auto georeference_offset_node =
      xml
      .child("OpenDRIVE")
      .child("header")
      .child("offset");
 
    auto reference_and_projection = ParseGeoProjectionAndReference(
      georeference_string, georeference_offset_node);
    map_builder.SetGeoProjection(reference_and_projection.first);
    map_builder.SetGeoReference(reference_and_projection.second);
 
 
  }
 
} // parser
} // opendrive
} // carla