lanelete2 projection.

src/map/lanelet2/lanelet2_projection/lanelet2_projection.pro

@@ -0,0 +1,43 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2022-11-21T16:40:05
+#
+#-------------------------------------------------
+
+QT       -= core gui
+
+TARGET = lanelet2_projection
+TEMPLATE = lib
+
+DEFINES += LANELET2_PJOJECTION_LIBRARY
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    src/UTM.cpp
+
+HEADERS +=
+
+unix {
+    target.path = /usr/lib
+    INSTALLS += target
+}
+
+CONFIG += plugin
+
+INCLUDEPATH += $$PWD/../include
+
+unix:INCLUDEPATH += /usr/include/eigen3
+win32:INCLUDEPATH += D:\File\soft\eigen
+
+
+#sudo apt-get install libgeographic-dev

src/map/lanelet2/lanelet2_projection/src/UTM.cpp

@@ -0,0 +1,81 @@
+#include "lanelet2_projection/UTM.h"
+
+#include <GeographicLib/UTMUPS.hpp>
+
+namespace lanelet {
+namespace projection {
+
+UtmProjector::UtmProjector(Origin origin, const bool useOffset, const bool throwInPaddingArea)
+    : Projector(origin), useOffset_{useOffset}, throwInPaddingArea_{throwInPaddingArea} {
+  double x = 0;
+  double y = 0;
+  GeographicLib::UTMUPS::Forward(this->origin().position.lat, this->origin().position.lon, zone_,
+                                 isInNorthernHemisphere_, x, y);
+  if (useOffset_) {
+    xOffset_ = x;
+    yOffset_ = y;
+  }
+}
+
+BasicPoint3d UtmProjector::forward(const GPSPoint& gps) const {
+  BasicPoint3d utm{0., 0., gps.ele};
+  int zone{};
+  bool northp{};
+  try {
+    GeographicLib::UTMUPS::Forward(gps.lat, gps.lon, zone, northp, utm.x(), utm.y());
+  } catch (GeographicLib::GeographicErr& e) {
+    throw ForwardProjectionError(e.what());
+  }
+
+  if (zone != zone_ || northp != isInNorthernHemisphere_) {
+    if (throwInPaddingArea_) {
+      throw ForwardProjectionError("You have left the UTM zone or changed the hemisphere!");
+    }
+    // try to transfer to the desired zone
+    double xAfterTransfer = 0;
+    double yAfterTransfer = 0;
+    int zoneAfterTransfer = 0;
+    try {
+      GeographicLib::UTMUPS::Transfer(zone, northp, utm.x(), utm.y(), zone_, isInNorthernHemisphere_, xAfterTransfer,
+                                      yAfterTransfer, zoneAfterTransfer);
+    } catch (GeographicLib::GeographicErr& e) {
+      throw ForwardProjectionError(e.what());
+    }
+
+    if (zoneAfterTransfer != zone_) {
+      throw ForwardProjectionError("You have left the padding area of the UTM zone!");
+    }
+    utm.x() = xAfterTransfer;
+    utm.y() = yAfterTransfer;
+  }
+
+  if (useOffset_) {
+    utm.x() -= xOffset_;
+    utm.y() -= yOffset_;
+  }
+
+  return utm;
+}
+
+GPSPoint UtmProjector::reverse(const BasicPoint3d& utm) const {
+  GPSPoint gps{0., 0., utm.z()};
+  try {
+    GeographicLib::UTMUPS::Reverse(zone_, isInNorthernHemisphere_, useOffset_ ? utm.x() + xOffset_ : utm.x(),
+                                   useOffset_ ? utm.y() + yOffset_ : utm.y(), gps.lat, gps.lon);
+  } catch (GeographicLib::GeographicErr& e) {
+    throw ReverseProjectionError(e.what());
+  }
+
+  if (throwInPaddingArea_) {
+    // for zone compliance testing:
+    try {
+      forward(gps);
+    } catch (ForwardProjectionError& e) {
+      throw ReverseProjectionError(e.what());
+    };
+  }
+  return gps;
+}
+
+}  // namespace projection
+}  // namespace lanelet