testGeographicLib.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #include <GeographicLib/Config.h>
20 #include <GeographicLib/UTMUPS.hpp>
22 
23 #include <gtsam/base/types.h>
25 
26 #include <string>
27 #include <iostream>
28 
29 using namespace std;
30 //using namespace gtsam;
31 using namespace GeographicLib;
32 
33 // Dekalb-Peachtree Airport runway 2L
34 static const double lat = 33.87071, lon = -84.30482, h = 274;
35 
36 #if GEOGRAPHICLIB_VERSION_MINOR<37
37 static const auto& kWGS84 = Geocentric::WGS84;
38 #else
39 static const auto& kWGS84 = Geocentric::WGS84();
40 #endif
41 
42 //**************************************************************************
44 
45  // From lat-lon to geocentric
46  double X, Y, Z;
47  kWGS84.Forward(lat, lon, h, X, Y, Z);
48  EXPECT_DOUBLES_EQUAL(526, X/1000, 1);
49  EXPECT_DOUBLES_EQUAL(-5275, Y/1000, 1);
50  EXPECT_DOUBLES_EQUAL(3535, Z/1000, 1);
51 
52  // From geocentric to lat-lon
53  double lat_, lon_, h_;
54  kWGS84.Reverse(X, Y, Z, lat_, lon_, h_);
55  EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
56  EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
57  EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
58 }
59 
60 //**************************************************************************
62 
63  // From lat-lon to UTM
64  int zone;
65  bool northp;
66  double x, y;
67  UTMUPS::Forward(lat, lon, zone, northp, x, y);
68 
69  // UTM is 16N 749305.58 3751090.08
70  // Obtained by
71  // http://geographiclib.sourceforge.net/cgi-bin/GeoConvert?input=33.87071+-84.30482000000001&zone=-3&prec=2&option=Submit
72  auto actual = UTMUPS::EncodeZone(zone, northp);
73  // transform to upper case
74  std::transform(actual.begin(), actual.end(), actual.begin(), ::toupper);
75  EXPECT(actual=="16N");
76  EXPECT_DOUBLES_EQUAL(749305.58, x, 1e-2);
77  EXPECT_DOUBLES_EQUAL(3751090.08, y, 1e-2);
78 }
79 
80 //**************************************************************************
82 
83  // ENU Origin is where the plane was in hold next to runway
84  const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
85  LocalCartesian enu(lat0, lon0, h0, kWGS84);
86 
87  // From lat-lon to geocentric
88  double E, N, U;
89  enu.Forward(lat0, lon0, h0, E, N, U);
90  EXPECT_DOUBLES_EQUAL(0, E, 1e-2);
91  EXPECT_DOUBLES_EQUAL(0, N, 1e-2);
92  EXPECT_DOUBLES_EQUAL(0, U, 1e-2);
93 
94  // From lat-lon to geocentric
95  enu.Forward(lat, lon, h, E, N, U);
96  EXPECT_DOUBLES_EQUAL(133.24, E, 1e-2);
97  EXPECT_DOUBLES_EQUAL(80.98, N, 1e-2);
98  EXPECT_DOUBLES_EQUAL(0, U, 1e-2);
99 
100  // From geocentric to lat-lon
101  double lat_, lon_, h_;
102  enu.Reverse(E, N, U, lat_, lon_, h_);
103  EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
104  EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
105  EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
106 }
107 
108 //**************************************************************************
109 int main() {
110  TestResult tr;
111  return TestRegistry::runAllTests(tr);
112 }
113 //**************************************************************************
const char Y
Typedefs for easier changing of types.
Scalar * y
int main()
static int runAllTests(TestResult &result)
Header for GeographicLib::LocalCartesian class.
static const double lat
Header for GeographicLib::UTMUPS class.
const double h0
Definition: BFloat16.h:88
TEST(GeographicLib, Geocentric)
#define N
Definition: gksort.c:12
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
Geocentric coordinates
Definition: Geocentric.hpp:67
#define Z
Definition: icosphere.cpp:21
const double lat0
static const auto & kWGS84
#define EXPECT(condition)
Definition: Test.h:150
Namespace for GeographicLib.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const double lon0
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
Header for GeographicLib::Geocentric class.
Local cartesian coordinates.
void Forward(real lat, real lon, real h, real &x, real &y, real &z) const
DiscreteKey E(5, 2)
void Reverse(real x, real y, real z, real &lat, real &lon, real &h) const
static const double lon
#define X
Definition: icosphere.cpp:20
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
static const double h


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:17