mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
gps_conversions.h
Go to the documentation of this file.
1 /* Taken from utexas-art-ros-pkg:art_vehicle/applanix */
2 
3 /*
4  * Conversions between coordinate systems.
5  *
6  * Includes LatLong<->UTM.
7  */
8 
9 #ifndef _UTM_H
10 #define _UTM_H
11 
19 #include <cmath>
20 #include <cstdio>
21 #include <cstdlib>
22 #include <string>
23 
24 namespace mrs_lib
25 {
26 
27  const double RADIANS_PER_DEGREE = M_PI / 180.0;
28  const double DEGREES_PER_RADIAN = 180.0 / M_PI;
29 
30  // WGS84 Parameters
31  const double WGS84_A = 6378137.0; // major axis
32  const double WGS84_B = 6356752.31424518; // minor axis
33  const double WGS84_F = 0.0033528107; // ellipsoid flattening
34  const double WGS84_E = 0.0818191908; // first eccentricity
35  const double WGS84_EP = 0.0820944379; // second eccentricity
36 
37  // UTM Parameters
38  const double UTM_K0 = 0.9996; // scale factor
39  const double UTM_FE = 500000.0; // false easting
40  const double UTM_FN_N = 0.0; // false northing on north hemisphere
41  const double UTM_FN_S = 10000000.0; // false northing on south hemisphere
42  const double UTM_E2 = (WGS84_E * WGS84_E); // e^2
43  const double UTM_E4 = (UTM_E2 * UTM_E2); // e^4
44  const double UTM_E6 = (UTM_E4 * UTM_E2); // e^6
45  const double UTM_EP2 = (UTM_E2 / (1 - UTM_E2)); // e'^2
46 
54  static inline void UTM(double lat, double lon, double* x, double* y) {
55  // constants
56  const static double m0 = (1 - UTM_E2 / 4 - 3 * UTM_E4 / 64 - 5 * UTM_E6 / 256);
57  const static double m1 = -(3 * UTM_E2 / 8 + 3 * UTM_E4 / 32 + 45 * UTM_E6 / 1024);
58  const static double m2 = (15 * UTM_E4 / 256 + 45 * UTM_E6 / 1024);
59  const static double m3 = -(35 * UTM_E6 / 3072);
60 
61  // compute the central meridian
62  int cm = ((lon >= 0.0) ? ((int)lon - ((int)lon) % 6 + 3) : ((int)lon - ((int)lon) % 6 - 3));
63 
64  // convert degrees into radians
65  double rlat = lat * RADIANS_PER_DEGREE;
66  double rlon = lon * RADIANS_PER_DEGREE;
67  double rlon0 = cm * RADIANS_PER_DEGREE;
68 
69  // compute trigonometric functions
70  double slat = sin(rlat);
71  double clat = cos(rlat);
72  double tlat = tan(rlat);
73 
74  // decide the false northing at origin
75  double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
76 
77  double T = tlat * tlat;
78  double C = UTM_EP2 * clat * clat;
79  double A = (rlon - rlon0) * clat;
80  double M = WGS84_A * (m0 * rlat + m1 * sin(2 * rlat) + m2 * sin(4 * rlat) + m3 * sin(6 * rlat));
81  double V = WGS84_A / sqrt(1 - UTM_E2 * slat * slat);
82 
83  // compute the easting-northing coordinates
84  *x = UTM_FE + UTM_K0 * V * (A + (1 - T + C) * pow(A, 3) / 6 + (5 - 18 * T + T * T + 72 * C - 58 * UTM_EP2) * pow(A, 5) / 120);
85  *y = fn +
86  UTM_K0 *
87  (M + V * tlat * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * pow(A, 4) / 24 + ((61 - 58 * T + T * T + 600 * C - 330 * UTM_EP2) * pow(A, 6) / 720)));
88 
89  return;
90  }
91 
92 
101  static inline char UTMLetterDesignator(double Lat) {
102  char LetterDesignator;
103 
104  if ((84 >= Lat) && (Lat >= 72))
105  LetterDesignator = 'X';
106  else if ((72 > Lat) && (Lat >= 64))
107  LetterDesignator = 'W';
108  else if ((64 > Lat) && (Lat >= 56))
109  LetterDesignator = 'V';
110  else if ((56 > Lat) && (Lat >= 48))
111  LetterDesignator = 'U';
112  else if ((48 > Lat) && (Lat >= 40))
113  LetterDesignator = 'T';
114  else if ((40 > Lat) && (Lat >= 32))
115  LetterDesignator = 'S';
116  else if ((32 > Lat) && (Lat >= 24))
117  LetterDesignator = 'R';
118  else if ((24 > Lat) && (Lat >= 16))
119  LetterDesignator = 'Q';
120  else if ((16 > Lat) && (Lat >= 8))
121  LetterDesignator = 'P';
122  else if ((8 > Lat) && (Lat >= 0))
123  LetterDesignator = 'N';
124  else if ((0 > Lat) && (Lat >= -8))
125  LetterDesignator = 'M';
126  else if ((-8 > Lat) && (Lat >= -16))
127  LetterDesignator = 'L';
128  else if ((-16 > Lat) && (Lat >= -24))
129  LetterDesignator = 'K';
130  else if ((-24 > Lat) && (Lat >= -32))
131  LetterDesignator = 'J';
132  else if ((-32 > Lat) && (Lat >= -40))
133  LetterDesignator = 'H';
134  else if ((-40 > Lat) && (Lat >= -48))
135  LetterDesignator = 'G';
136  else if ((-48 > Lat) && (Lat >= -56))
137  LetterDesignator = 'F';
138  else if ((-56 > Lat) && (Lat >= -64))
139  LetterDesignator = 'E';
140  else if ((-64 > Lat) && (Lat >= -72))
141  LetterDesignator = 'D';
142  else if ((-72 > Lat) && (Lat >= -80))
143  LetterDesignator = 'C';
144  // 'Z' is an error flag, the Latitude is outside the UTM limits
145  else
146  LetterDesignator = 'Z';
147  return LetterDesignator;
148  }
149 
159  static inline void LLtoUTM(const double Lat, const double Long, double& UTMNorthing, double& UTMEasting, char* UTMZone) {
160  double a = WGS84_A;
161  double eccSquared = UTM_E2;
162  double k0 = UTM_K0;
163 
164  double LongOrigin;
165  double eccPrimeSquared;
166  double N, T, C, A, M;
167 
168  // Make sure the longitude is between -180.00 .. 179.9
169  double LongTemp = (Long + 180) - int((Long + 180) / 360) * 360 - 180;
170 
171  double LatRad = Lat * RADIANS_PER_DEGREE;
172  double LongRad = LongTemp * RADIANS_PER_DEGREE;
173  double LongOriginRad;
174  int ZoneNumber;
175 
176  ZoneNumber = int((LongTemp + 180) / 6) + 1;
177  // range clamping to shut up some compiler warnings
178  // (the UTM Zone number should in reality be in the range <1, 60>)
179  if (ZoneNumber > 99)
180  ZoneNumber = 99;
181  if (ZoneNumber < -9)
182  ZoneNumber = -9;
183 
184  if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
185  ZoneNumber = 32;
186 
187  // Special zones for Svalbard
188  if (Lat >= 72.0 && Lat < 84.0) {
189  if (LongTemp >= 0.0 && LongTemp < 9.0)
190  ZoneNumber = 31;
191  else if (LongTemp >= 9.0 && LongTemp < 21.0)
192  ZoneNumber = 33;
193  else if (LongTemp >= 21.0 && LongTemp < 33.0)
194  ZoneNumber = 35;
195  else if (LongTemp >= 33.0 && LongTemp < 42.0)
196  ZoneNumber = 37;
197  }
198  // +3 puts origin in middle of zone
199  LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3;
200  LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
201 
202  // compute the UTM Zone from the latitude and longitude
203  snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
204 
205  eccPrimeSquared = (eccSquared) / (1 - eccSquared);
206 
207  N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
208  T = tan(LatRad) * tan(LatRad);
209  C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
210  A = cos(LatRad) * (LongRad - LongOriginRad);
211 
212  M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad -
213  (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * LatRad) +
214  (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * LatRad) -
215  (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));
216 
217  UTMEasting =
218  (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120) + 500000.0);
219 
220  UTMNorthing = (double)(k0 * (M + N * tan(LatRad) *
221  (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
222  (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
223  if (Lat < 0)
224  UTMNorthing += 10000000.0; // 10000000 meter offset for southern hemisphere
225  }
226 
227  static inline void LLtoUTM(const double Lat, const double Long, double& UTMNorthing, double& UTMEasting, std::string& UTMZone) {
228  char zone_buf[] = {0, 0, 0, 0};
229 
230  LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, zone_buf);
231 
232  UTMZone = zone_buf;
233  }
234 
235 
245  static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, const char* UTMZone, double& Lat, double& Long) {
246  double k0 = UTM_K0;
247  double a = WGS84_A;
248  double eccSquared = UTM_E2;
249  double eccPrimeSquared;
250  double e1 = (1 - sqrt(1 - eccSquared)) / (1 + sqrt(1 - eccSquared));
251  double N1, T1, C1, R1, D, M;
252  double LongOrigin;
253  double mu, phi1Rad;
254  [[maybe_unused]] double phi1;
255  double x, y;
256  int ZoneNumber;
257  char* ZoneLetter;
258  [[maybe_unused]] int NorthernHemisphere; // 1 for northern hemispher, 0 for southern
259 
260  x = UTMEasting - 500000.0; // remove 500,000 meter offset for longitude
261  y = UTMNorthing;
262 
263  ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
264  if ((*ZoneLetter - 'N') >= 0)
265  NorthernHemisphere = 1; // point is in northern hemisphere
266  else {
267  NorthernHemisphere = 0; // point is in southern hemisphere
268  y -= 10000000.0; // remove 10,000,000 meter offset used for southern hemisphere
269  }
270 
271  LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3; //+3 puts origin in middle of zone
272 
273  eccPrimeSquared = (eccSquared) / (1 - eccSquared);
274 
275  M = y / k0;
276  mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256));
277 
278  phi1Rad = mu + (3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * mu) + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * mu) +
279  (151 * e1 * e1 * e1 / 96) * sin(6 * mu);
280  phi1 = phi1Rad * DEGREES_PER_RADIAN;
281 
282  N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad));
283  T1 = tan(phi1Rad) * tan(phi1Rad);
284  C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
285  R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5);
286  D = x / (N1 * k0);
287 
288  Lat = phi1Rad - (N1 * tan(phi1Rad) / R1) * (D * D / 2 - (5 + 3 * T1 + 10 * C1 - 4 * C1 * C1 - 9 * eccPrimeSquared) * D * D * D * D / 24 +
289  (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - 3 * C1 * C1) * D * D * D * D * D * D / 720);
290  Lat = Lat * DEGREES_PER_RADIAN;
291 
292  Long = (D - (1 + 2 * T1 + C1) * D * D * D / 6 + (5 - 2 * C1 + 28 * T1 - 3 * C1 * C1 + 8 * eccPrimeSquared + 24 * T1 * T1) * D * D * D * D * D / 120) /
293  cos(phi1Rad);
294  Long = LongOrigin + Long * DEGREES_PER_RADIAN;
295  }
296 
297  static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, std::string UTMZone, double& Lat, double& Long) {
298  UTMtoLL(UTMNorthing, UTMEasting, UTMZone.c_str(), Lat, Long);
299  }
300 
301 } // namespace mrs_lib
302 
303 #endif // _UTM_H
mrs_lib
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition: attitude_converter.h:29