| 1 |
/* +------------------------------------------------------------------------+ |
| 2 |
| Mobile Robot Programming Toolkit (MRPT) | |
| 3 |
| https://www.mrpt.org/ | |
| 4 |
| | |
| 5 |
| Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | |
| 6 |
| See: https://www.mrpt.org/Authors - All rights reserved. | |
| 7 |
| Released under BSD License. See: https://www.mrpt.org/License | |
| 8 |
+------------------------------------------------------------------------+ */ |
| 9 |
|
| 10 |
#include <gtest/gtest.h> |
| 11 |
#include <mrpt/topography.h> |
| 12 |
|
| 13 |
using namespace mrpt; |
| 14 |
using namespace mrpt::math; |
| 15 |
using namespace mrpt::topography; |
| 16 |
using namespace std; |
| 17 |
|
| 18 |
void do_test_geodetic_geocentric(const TGeodeticCoords c1) |
| 19 |
{ |
| 20 |
TGeocentricCoords geo1; |
| 21 |
mrpt::topography::geodeticToGeocentric(c1, geo1, TEllipsoid::Ellipsoid_WGS84()); |
| 22 |
|
| 23 |
TGeodeticCoords c2; |
| 24 |
mrpt::topography::geocentricToGeodetic(geo1, c2, TEllipsoid::Ellipsoid_WGS84()); |
| 25 |
|
| 26 |
EXPECT_NEAR(c1.lat, c2.lat, 1e-8); |
| 27 |
EXPECT_NEAR(c1.lon, c2.lon, 1e-8); |
| 28 |
EXPECT_NEAR(c1.height, c2.height, 1e-4); |
| 29 |
} |
| 30 |
|
| 31 |
TEST(TopographyConversion, GeodeticToGeocentricToGeodetic) |
| 32 |
{ |
| 33 |
do_test_geodetic_geocentric(TGeodeticCoords(TCoords(36, 1, 30), TCoords(3, 2, 40), 20)); |
| 34 |
do_test_geodetic_geocentric(TGeodeticCoords(TCoords(-36, 1, 30), TCoords(3, 2, 40), 20)); |
| 35 |
do_test_geodetic_geocentric(TGeodeticCoords(TCoords(36, 1, 30), TCoords(-3, 2, 40), 20)); |
| 36 |
do_test_geodetic_geocentric(TGeodeticCoords(TCoords(-36, 1, 30), TCoords(-3, 2, 40), 20)); |
| 37 |
} |
| 38 |
|
| 39 |
TEST(TopographyConversion, geodeticToENU_WGS84) |
| 40 |
{ |
| 41 |
// A test of GPS WGS84 -> ENU Cartessian coordinates with known solution |
| 42 |
mrpt::topography::TGeodeticCoords gps_point; |
| 43 |
gps_point.lon = -4.475828390; |
| 44 |
gps_point.lat = 36.716411055; |
| 45 |
gps_point.height = 48.243; |
| 46 |
|
| 47 |
mrpt::topography::TGeodeticCoords gps_ref; |
| 48 |
gps_ref.lon = -4.4789588283333330; |
| 49 |
gps_ref.lat = 36.714459075; |
| 50 |
gps_ref.height = 38.8887; |
| 51 |
|
| 52 |
TPoint3D P; |
| 53 |
mrpt::topography::geodeticToENU_WGS84(gps_point, P, gps_ref); |
| 54 |
|
| 55 |
TPoint3D P_true(279.679067794, 216.621954, 9.34448); |
| 56 |
EXPECT_NEAR(P.x, P_true.x, 3e-3); // Precision should be *much* better than |
| 57 |
// 1mm, but don't be so hard... |
| 58 |
EXPECT_NEAR(P.y, P_true.y, 3e-3); |
| 59 |
EXPECT_NEAR(P.z, P_true.z, 3e-3); |
| 60 |
|
| 61 |
// 2nd test: just modify the height and verify that ENU coordinates only |
| 62 |
// change in Z: |
| 63 |
mrpt::topography::TGeodeticCoords gps_point2 = gps_ref; |
| 64 |
const double A_height = 15.0; |
| 65 |
gps_point2.height += A_height; |
| 66 |
|
| 67 |
mrpt::topography::geodeticToENU_WGS84(gps_point2, P, gps_ref); |
| 68 |
|
| 69 |
EXPECT_NEAR(P.x, 0, 0.1e-3); // Precision: 0.1mm |
| 70 |
EXPECT_NEAR(P.y, 0, 0.1e-3); |
| 71 |
EXPECT_NEAR(P.z, A_height, 0.1e-3); |
| 72 |
} |
| 73 |
|
| 74 |
TEST(TopographyConversion, geodeticToUTM_WGS84) |
| 75 |
{ |
| 76 |
mrpt::topography::TGeodeticCoords gps_point; |
| 77 |
gps_point.lon = -2.404508; |
| 78 |
gps_point.lat = 36.829293; |
| 79 |
gps_point.height = 0; |
| 80 |
|
| 81 |
mrpt::topography::TUTMCoords utm; |
| 82 |
int utm_zone = 0; |
| 83 |
char lat_band = 0; |
| 84 |
|
| 85 |
mrpt::topography::geodeticToUTM(gps_point, utm, utm_zone, lat_band); |
| 86 |
|
| 87 |
EXPECT_EQ(utm_zone, 30); |
| 88 |
EXPECT_EQ(lat_band, 'S'); |
| 89 |
EXPECT_NEAR(utm.x, 553103.02, 0.05); |
| 90 |
EXPECT_NEAR(utm.y, 4076100.98, 0.05); |
| 91 |
} |
| 92 |
|
| 93 |
TEST(TopographyConversion, UTMToGeodetic_WGS84) |
| 94 |
{ |
| 95 |
const mrpt::topography::TUTMCoords utm = {553103.020, 4076100.969, 0.0}; |
| 96 |
|
| 97 |
mrpt::topography::TGeodeticCoords gc; |
| 98 |
mrpt::topography::UTMToGeodetic(utm, 30, 'N', gc); |
| 99 |
|
| 100 |
EXPECT_NEAR(gc.lat, 36.829293, 1e-6); |
| 101 |
EXPECT_NEAR(gc.lon, -2.404508, 1e-6); |
| 102 |
} |