add Geocentric and Local Cartesian projectors

lanelet2_projection/README.md

@@ -11,5 +11,7 @@ For more details, read [here](doc/Map_Projections_Coordinate_Systems.md).
 
 ## Supported Projections
 
- - `UTM.h`: WGS84 (for storage in .osm files) <-> UTM (internal processing) (wrapper of https://sourceforge.net/projects/geographiclib/). This projection has the advantage of being very precise, but all points in the map must fit into one UTM Tile. If points exceed a 100 km margin, the map can not be loaded.
- - `Mercator.h`: WGS84 (for storage in .osm files) <-> Local Mercator as in [liblanelet](https://github.com/phbender/liblanelet/blob/master/Commons/mercator.hpp). Approximates the earth
+- `UTM.h`: WGS84 (for storage in .osm files) <-> UTM (internal processing) (wrapper of https://sourceforge.net/projects/geographiclib/). This projection has the advantage of being very precise, but all points in the map must fit into one UTM Tile. If points exceed a 100 km margin, the map can not be loaded.
+- `Mercator.h`: WGS84 (for storage in .osm files) <-> Local Mercator as in [liblanelet](https://github.com/phbender/liblanelet/blob/master/Commons/mercator.hpp). Approximates the earth
+- `Geocentric.h`: WGS84 <-> ECEF (wrapper of https://sourceforge.net/projects/geographiclib/).
+- `LocalCartesian.h`: WGS84 <-> LocalCartesian (wrapper of https://sourceforge.net/projects/geographiclib/). This is similar to UTM; however, it properly treats elevation.

lanelet2_projection/include/lanelet2_projection/Geocentric.h

@@ -0,0 +1,14 @@
+#pragma once
+#include <lanelet2_io/Projection.h>
+
+namespace lanelet {
+namespace projection {
+
+class GeocentricProjector : public Projector {
+ public:
+  BasicPoint3d forward(const GPSPoint& gps) const override;
+  GPSPoint reverse(const BasicPoint3d& enu) const override;
+};
+
+}  // namespace projection
+}  // namespace lanelet

lanelet2_projection/include/lanelet2_projection/LocalCartesian.h

@@ -0,0 +1,21 @@
+#pragma once
+#include <lanelet2_io/Projection.h>
+
+#include <GeographicLib/LocalCartesian.hpp>
+
+namespace lanelet {
+namespace projection {
+class LocalCartesianProjector : public Projector {
+ public:
+  explicit LocalCartesianProjector(Origin origin);
+
+  BasicPoint3d forward(const GPSPoint& gps) const override;
+
+  GPSPoint reverse(const BasicPoint3d& enu) const override;
+
+ private:
+  GeographicLib::LocalCartesian localCartesian_;
+};
+
+}  // namespace projection
+}  // namespace lanelet

lanelet2_projection/package.xml

@@ -10,6 +10,7 @@
   <author email="maximilian.naumann@kit.edu">Maximilian Naumann</author>
   <author email="fabian.poggenhans@kit.edu">Fabian Poggenhans</author>
   <author email="jan-hendrik.pauls@kit.edu">Jan-Hendrik Pauls</author>
+  <author email="michal.antkiewicz@uwaterloo.ca">Michał Antkiewicz</author>
   <url type="repository">https://github.com/fzi-forschungszentrum-informatik/lanelet2.git</url>
 
   <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>

lanelet2_projection/src/Geocentric.cpp

@@ -0,0 +1,31 @@
+#include "lanelet2_projection/Geocentric.h"
+
+#include <GeographicLib/Geocentric.hpp>
+
+namespace lanelet {
+namespace projection {
+
+BasicPoint3d GeocentricProjector::forward(const GPSPoint& gps) const {
+  BasicPoint3d ecef{0., 0., 0.};
+  GeographicLib::Geocentric::WGS84().Forward(gps.lat,
+                                           gps.lon,
+                                           gps.ele,
+                                           ecef[0],
+                                           ecef[1],
+                                           ecef[2]);
+  return ecef;
+}
+
+GPSPoint GeocentricProjector::reverse(const BasicPoint3d& ecef) const {
+  GPSPoint gps{0., 0., 0.};
+  GeographicLib::Geocentric::WGS84().Reverse(ecef[0],
+                                           ecef[1],
+                                           ecef[2],
+                                           gps.lat,
+                                           gps.lon,
+                                           gps.ele);
+  return gps;
+}
+
+}  // namespace projection
+}  // namespace lanelet

lanelet2_projection/src/LocalCartesian.cpp

@@ -0,0 +1,36 @@
+#include "lanelet2_projection/LocalCartesian.h"
+
+#include <GeographicLib/Geocentric.hpp>
+
+namespace lanelet {
+namespace projection {
+
+LocalCartesianProjector::LocalCartesianProjector(Origin origin) :
+      Projector(origin),
+      localCartesian_(origin.position.lat, origin.position.lon, origin.position.ele, GeographicLib::Geocentric::WGS84()) {
+}
+
+BasicPoint3d LocalCartesianProjector::forward(const GPSPoint& gps) const {
+  BasicPoint3d local{0., 0., 0.};
+  this->localCartesian_.Forward(gps.lat,
+                               gps.lon,
+                               gps.ele,
+                               local[0],
+                               local[1],
+                               local[2]);
+  return local;
+}
+
+GPSPoint LocalCartesianProjector::reverse(const BasicPoint3d& local) const {
+  GPSPoint gps{0., 0., 0.};
+  this->localCartesian_.Reverse(local[0],
+                               local[1],
+                               local[2],
+                               gps.lat,
+                               gps.lon,
+                               gps.ele);
+  return gps;
+}
+
+}  // namespace projection
+}  // namespace lanelet

lanelet2_projection/test/test_Geocentric.cpp

@@ -0,0 +1,40 @@
+#include "gtest/gtest.h"
+#include "lanelet2_projection/Geocentric.h"
+
+using namespace lanelet;
+using GeocentricProjector = lanelet::projection::GeocentricProjector;
+
+class GeocentricProjectionTest : public ::testing::Test {
+ public:
+  void SetUp() override {
+    geocentricProjector = std::make_shared<GeocentricProjector>();
+  }
+  GeocentricProjector::Ptr geocentricProjector;
+
+  // Lat, Lon, Ele with respect to the WGS84 ellipsoid
+  double originLat{49.01439};
+  double originLon{8.41722};
+  double originEle{123.0};
+  lanelet::GPSPoint originGps{originLat, originLon, originEle};
+  lanelet::Origin origin{originGps};
+
+  // X, Y, Z with respect to the center of the earth
+  double originX = 4146160.550580083;
+  double originY = 613525.0621995202;
+  double originZ = 4791701.343249619;
+  lanelet::BasicPoint3d originECEF{originX, originY, originZ};
+};
+
+TEST_F(GeocentricProjectionTest, TestForward) {  // NOLINT
+  BasicPoint3d ecefPoint = geocentricProjector->forward(originGps);
+  ASSERT_NEAR(ecefPoint.x(), originX, 0.00001);
+  ASSERT_NEAR(ecefPoint.y(), originY, 0.00001);
+  ASSERT_NEAR(ecefPoint.z(), originZ, 0.00001);
+}
+
+TEST_F(GeocentricProjectionTest, TestReverse) {  // NOLINT
+  lanelet::GPSPoint gpsPoint = geocentricProjector->reverse(originECEF);
+  ASSERT_NEAR(gpsPoint.lat, originLat, 0.00001);
+  ASSERT_NEAR(gpsPoint.lon, originLon, 0.00001);
+  ASSERT_NEAR(gpsPoint.ele, originEle, 0.00001);
+}

lanelet2_projection/test/test_LocalCartesian.cpp

@@ -0,0 +1,40 @@
+#include "gtest/gtest.h"
+#include "lanelet2_projection/LocalCartesian.h"
+
+using namespace lanelet;
+using LocalCartesianProjector = lanelet::projection::LocalCartesianProjector;
+
+class LocalCartesianProjectionTest : public ::testing::Test {
+ public:
+  void SetUp() override {
+    localCartesianProjector = std::make_shared<LocalCartesianProjector>(origin);
+  }
+  LocalCartesianProjector::Ptr localCartesianProjector;
+
+  // Lat, Lon, Ele with respect to the WGS84 ellipsoid
+  double originLat{49.01439};
+  double originLon{8.41722};
+  double originEle{123.0};
+  lanelet::GPSPoint originGps{originLat, originLon, originEle};
+  lanelet::Origin origin{originGps};
+
+  // X, Y, Z with respect to the center of the earth
+  double originX = 4146160.550580083;
+  double originY = 613525.0621995202;
+  double originZ = 4791701.343249619;
+  lanelet::BasicPoint3d originECEF{originX, originY, originZ};
+};
+
+TEST_F(LocalCartesianProjectionTest, TestForward) {  // NOLINT
+  BasicPoint3d localCartesianPoint = localCartesianProjector->forward(originGps);
+  ASSERT_NEAR(localCartesianPoint.x(), 0., 0.00001);
+  ASSERT_NEAR(localCartesianPoint.y(), 0., 0.00001);
+  ASSERT_NEAR(localCartesianPoint.z(), 0., 0.00001);
+}
+
+TEST_F(LocalCartesianProjectionTest, TestReverse) {  // NOLINT
+  lanelet::GPSPoint gpsPoint = localCartesianProjector->reverse({0., 0., 0.});
+  ASSERT_NEAR(gpsPoint.lat, originLat, 0.00001);
+  ASSERT_NEAR(gpsPoint.lon, originLon, 0.00001);
+  ASSERT_NEAR(gpsPoint.ele, originEle, 0.00001);
+}

lanelet2_python/python_api/projection.cpp

@@ -1,4 +1,6 @@
 #include <lanelet2_projection/UTM.h>
+#include <lanelet2_projection/Geocentric.h>
+#include <lanelet2_projection/LocalCartesian.h>
 
 #include <boost/python.hpp>
 
@@ -13,7 +15,11 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) {  // NOLINT
       .def("origin", &Projector::origin, "Global origin of the converter", return_internal_reference<>());
   class_<projection::SphericalMercatorProjector, std::shared_ptr<projection::SphericalMercatorProjector>,  // NOLINT
          bases<Projector>>("MercatorProjector", init<Origin>("origin"));
-  class_<projection::UtmProjector, std::shared_ptr<projection::UtmProjector>, bases<Projector>>("UtmProjector",
-                                                                                                init<Origin>("origin"))
+  class_<projection::GeocentricProjector, std::shared_ptr<projection::GeocentricProjector>,  // NOLINT
+         bases<Projector>>("GeocentricProjector");
+  class_<projection::LocalCartesianProjector, std::shared_ptr<projection::LocalCartesianProjector>,  // NOLINT
+         bases<Projector>>("LocalCartesianProjector", init<Origin>("origin"));
+  class_<projection::UtmProjector, std::shared_ptr<projection::UtmProjector>,  // NOLINT
+         bases<Projector>>("UtmProjector", init<Origin>("origin"))
       .def(init<Origin, bool, bool>("UtmProjector(origin, useOffset, throwInPaddingArea)"));
 }

lanelet2_python/test/test_lanelet2_projection.py

@@ -0,0 +1,74 @@
+import unittest
+import lanelet2  # if we fail here, there is something wrong with lanelet2 registration
+from lanelet2.core import (BasicPoint3d, GPSPoint)
+from lanelet2.io import Origin
+from lanelet2.projection import (UtmProjector,
+                                 GeocentricProjector,
+                                 LocalCartesianProjector)
+
+
+class MatchingApiTestCase(unittest.TestCase):
+    origin_lat = 49.01439
+    origin_lon = 8.41722
+    origin_ele = 123.0
+    origin_gps_point = GPSPoint(origin_lat, origin_lon, origin_ele)
+
+    # X, Y, Z with respect to the center of the earth
+    origin_x = 4146160.550580083
+    origin_y = 613525.0621995202
+    origin_z = 4791701.343249619
+    origin_ecef = BasicPoint3d(origin_x, origin_y, origin_z)
+
+    # the larges number of decimal places for assertEqual comparisons
+    decimals = 8
+
+    def test_UtmProjector(self):
+        # NOTE: the projected plane is on the WGS84 ellipsoid
+        origin = Origin(self.origin_lat, self.origin_lon)
+        utm_projector = UtmProjector(origin)
+
+        utm_point = utm_projector.forward(self.origin_gps_point)
+        self.assertEqual(utm_point.x, 0.0)
+        self.assertEqual(utm_point.y, 0.0)
+        self.assertEqual(utm_point.z, self.origin_ele)
+
+        utm_point = BasicPoint3d(0.0, 0.0, self.origin_ele)
+        gps_point = utm_projector.reverse(utm_point)
+        self.assertEqual(round(gps_point.lat, 5), self.origin_lat)
+        self.assertEqual(round(gps_point.lon, 5), self.origin_lon)
+        self.assertEqual(round(gps_point.alt, 5), self.origin_ele)
+
+    def test_LocalCartesianProjector(self):
+        # NOTE: the projected plane is tangential to the WGS84 ellipsoid
+        # but it is at the given elevation above the ellipsoid
+        origin = Origin(self.origin_lat, self.origin_lon, self.origin_ele)
+        local_cartesian_projector = LocalCartesianProjector(origin)
+
+        local_cartesian_point = local_cartesian_projector.forward(
+            self.origin_gps_point)
+        self.assertEqual(local_cartesian_point.x, 0.0)
+        self.assertEqual(local_cartesian_point.y, 0.0)
+        self.assertEqual(local_cartesian_point.z, 0.0)
+
+        local_cartesian_point = BasicPoint3d(0.0, 0.0, 0.0)
+        gps_point = local_cartesian_projector.reverse(local_cartesian_point)
+        self.assertEqual(round(gps_point.lat, self.decimals), self.origin_lat)
+        self.assertEqual(round(gps_point.lon, self.decimals), self.origin_lon)
+        self.assertEqual(round(gps_point.alt, self.decimals), self.origin_ele)
+
+    def test_GeocentricProjector(self):
+        geocentric_projector = GeocentricProjector()
+
+        ecef_point = geocentric_projector.forward(self.origin_gps_point)
+        self.assertEqual(ecef_point.x, self.origin_ecef.x)
+        self.assertEqual(ecef_point.y, self.origin_ecef.y)
+        self.assertEqual(ecef_point.z, self.origin_ecef.z)
+
+        gps_point = geocentric_projector.reverse(self.origin_ecef)
+        self.assertEqual(round(gps_point.lat, self.decimals), self.origin_lat)
+        self.assertEqual(round(gps_point.lon, self.decimals), self.origin_lon)
+        self.assertEqual(round(gps_point.alt, self.decimals), self.origin_ele)
+
+
+if __name__ == '__main__':
+    unittest.main()