Debian Codegraph @

libs/topography/src/conversions_unittest.cpp

raw
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 }