/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include #include using namespace mrpt; using namespace mrpt::math; using namespace mrpt::topography; using namespace std; void do_test_geodetic_geocentric(const TGeodeticCoords c1) { TGeocentricCoords geo1; mrpt::topography::geodeticToGeocentric(c1, geo1, TEllipsoid::Ellipsoid_WGS84()); TGeodeticCoords c2; mrpt::topography::geocentricToGeodetic(geo1, c2, TEllipsoid::Ellipsoid_WGS84()); EXPECT_NEAR(c1.lat, c2.lat, 1e-8); EXPECT_NEAR(c1.lon, c2.lon, 1e-8); EXPECT_NEAR(c1.height, c2.height, 1e-4); } TEST(TopographyConversion, GeodeticToGeocentricToGeodetic) { do_test_geodetic_geocentric(TGeodeticCoords(TCoords(36, 1, 30), TCoords(3, 2, 40), 20)); do_test_geodetic_geocentric(TGeodeticCoords(TCoords(-36, 1, 30), TCoords(3, 2, 40), 20)); do_test_geodetic_geocentric(TGeodeticCoords(TCoords(36, 1, 30), TCoords(-3, 2, 40), 20)); do_test_geodetic_geocentric(TGeodeticCoords(TCoords(-36, 1, 30), TCoords(-3, 2, 40), 20)); } TEST(TopographyConversion, geodeticToENU_WGS84) { // A test of GPS WGS84 -> ENU Cartessian coordinates with known solution mrpt::topography::TGeodeticCoords gps_point; gps_point.lon = -4.475828390; gps_point.lat = 36.716411055; gps_point.height = 48.243; mrpt::topography::TGeodeticCoords gps_ref; gps_ref.lon = -4.4789588283333330; gps_ref.lat = 36.714459075; gps_ref.height = 38.8887; TPoint3D P; mrpt::topography::geodeticToENU_WGS84(gps_point, P, gps_ref); TPoint3D P_true(279.679067794, 216.621954, 9.34448); EXPECT_NEAR(P.x, P_true.x, 3e-3); // Precision should be *much* better than // 1mm, but don't be so hard... EXPECT_NEAR(P.y, P_true.y, 3e-3); EXPECT_NEAR(P.z, P_true.z, 3e-3); // 2nd test: just modify the height and verify that ENU coordinates only // change in Z: mrpt::topography::TGeodeticCoords gps_point2 = gps_ref; const double A_height = 15.0; gps_point2.height += A_height; mrpt::topography::geodeticToENU_WGS84(gps_point2, P, gps_ref); EXPECT_NEAR(P.x, 0, 0.1e-3); // Precision: 0.1mm EXPECT_NEAR(P.y, 0, 0.1e-3); EXPECT_NEAR(P.z, A_height, 0.1e-3); } TEST(TopographyConversion, geodeticToUTM_WGS84) { mrpt::topography::TGeodeticCoords gps_point; gps_point.lon = -2.404508; gps_point.lat = 36.829293; gps_point.height = 0; mrpt::topography::TUTMCoords utm; int utm_zone = 0; char lat_band = 0; mrpt::topography::geodeticToUTM(gps_point, utm, utm_zone, lat_band); EXPECT_EQ(utm_zone, 30); EXPECT_EQ(lat_band, 'S'); EXPECT_NEAR(utm.x, 553103.02, 0.05); EXPECT_NEAR(utm.y, 4076100.98, 0.05); } TEST(TopographyConversion, UTMToGeodetic_WGS84) { const mrpt::topography::TUTMCoords utm = {553103.020, 4076100.969, 0.0}; mrpt::topography::TGeodeticCoords gc; mrpt::topography::UTMToGeodetic(utm, 30, 'N', gc); EXPECT_NEAR(gc.lat, 36.829293, 1e-6); EXPECT_NEAR(gc.lon, -2.404508, 1e-6); }