test_Geocentric.cpp

Go to the documentation of this file.

#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);
}