utm_conversions.cpp
Go to the documentation of this file.
1 /* $Id: 67e70c2a6633d089c804653bac18a8b325f3082b $ */
2 
3 /*********************************************************************
4 * Software License Agreement (BSD License)
5 *
6 * Copyright (C) 2011 Chuck Gantz, Jack O'Quin
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the author nor other contributors may be
20 * used to endorse or promote products derived from this software
21 * without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36 
37 //#include <exception>
38 #include <ros/ros.h>
39 #include <angles/angles.h>
40 #include <geodesy/utm.h>
41 
55 namespace geodesy
56 {
57 
58 // WGS84 Parameters
59 #define WGS84_A 6378137.0 // major axis
60 #define WGS84_B 6356752.31424518 // minor axis
61 #define WGS84_F 0.0033528107 // ellipsoid flattening
62 #define WGS84_E 0.0818191908 // first eccentricity
63 #define WGS84_EP 0.0820944379 // second eccentricity
64 
65 // UTM Parameters
66 #define UTM_K0 0.9996 // scale factor
67 #define UTM_FE 500000.0 // false easting
68 #define UTM_FN_N 0.0 // false northing, northern hemisphere
69 #define UTM_FN_S 10000000.0 // false northing, southern hemisphere
70 #define UTM_E2 (WGS84_E*WGS84_E) // e^2
71 #define UTM_E4 (UTM_E2*UTM_E2) // e^4
72 #define UTM_E6 (UTM_E4*UTM_E2) // e^6
73 #define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2
74 
75 
82 static char UTMBand(double Lat, double Lon)
83 {
84  char LetterDesignator;
85 
86  if ((84 >= Lat) && (Lat >= 72)) LetterDesignator = 'X';
87  else if ((72 > Lat) && (Lat >= 64)) LetterDesignator = 'W';
88  else if ((64 > Lat) && (Lat >= 56)) LetterDesignator = 'V';
89  else if ((56 > Lat) && (Lat >= 48)) LetterDesignator = 'U';
90  else if ((48 > Lat) && (Lat >= 40)) LetterDesignator = 'T';
91  else if ((40 > Lat) && (Lat >= 32)) LetterDesignator = 'S';
92  else if ((32 > Lat) && (Lat >= 24)) LetterDesignator = 'R';
93  else if ((24 > Lat) && (Lat >= 16)) LetterDesignator = 'Q';
94  else if ((16 > Lat) && (Lat >= 8)) LetterDesignator = 'P';
95  else if (( 8 > Lat) && (Lat >= 0)) LetterDesignator = 'N';
96  else if (( 0 > Lat) && (Lat >= -8)) LetterDesignator = 'M';
97  else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
98  else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
99  else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
100  else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
101  else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
102  else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
103  else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
104  else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
105  else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
106  // '_' is an error flag, the Latitude is outside the UTM limits
107  else LetterDesignator = ' ';
108 
109  return LetterDesignator;
110 }
111 
119 geographic_msgs::GeoPoint toMsg(const UTMPoint &from)
120 {
121  //remove 500,000 meter offset for longitude
122  double x = from.easting - 500000.0;
123  double y = from.northing;
124 
125  double k0 = UTM_K0;
126  double a = WGS84_A;
127  double eccSquared = UTM_E2;
128  double eccPrimeSquared;
129  double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
130  double N1, T1, C1, R1, D, M;
131  double LongOrigin;
132  double mu, phi1Rad;
133 
134  if ((from.band - 'N') < 0)
135  {
136  //point is in southern hemisphere
137  //remove 10,000,000 meter offset used for southern hemisphere
138  y -= 10000000.0;
139  }
140 
141  //+3 puts origin in middle of zone
142  LongOrigin = (from.zone - 1)*6 - 180 + 3;
143  eccPrimeSquared = (eccSquared)/(1-eccSquared);
144 
145  M = y / k0;
146  mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64
147  -5*eccSquared*eccSquared*eccSquared/256));
148 
149  phi1Rad = mu + ((3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
150  + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
151  + (151*e1*e1*e1/96)*sin(6*mu));
152 
153  N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
154  T1 = tan(phi1Rad)*tan(phi1Rad);
155  C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
156  R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
157  D = x/(N1*k0);
158 
159  // function result
160  geographic_msgs::GeoPoint to;
161  to.altitude = from.altitude;
162  to.latitude =
163  phi1Rad - ((N1*tan(phi1Rad)/R1)
164  *(D*D/2
165  -(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
166  +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared
167  -3*C1*C1)*D*D*D*D*D*D/720));
168  to.latitude = angles::to_degrees(to.latitude);
169  to.longitude = ((D-(1+2*T1+C1)*D*D*D/6
170  +(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
171  *D*D*D*D*D/120)
172  / cos(phi1Rad));
173  to.longitude = LongOrigin + angles::to_degrees(to.longitude);
174 
175  // Normalize latitude and longitude to valid ranges.
176  normalize(to);
177  return to;
178 }
179 
187 void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to,
188  const bool& force_zone, const char& band, const uint8_t& zone)
189 {
190  double Lat = from.latitude;
191  double Long = from.longitude;
192 
193  double a = WGS84_A;
194  double eccSquared = UTM_E2;
195  double k0 = UTM_K0;
196 
197  double LongOrigin;
198  double eccPrimeSquared;
199  double N, T, C, A, M;
200 
201  // Make sure the longitude is between -180.00 .. 179.9
202  // (JOQ: this is broken for Long < -180, do a real normalize)
203  double LongTemp = (Long+180)-int((Long+180)/360)*360-180;
204  double LatRad = angles::from_degrees(Lat);
205  double LongRad = angles::from_degrees(LongTemp);
206  double LongOriginRad;
207 
208  to.altitude = from.altitude;
209  if (!force_zone)
210  to.zone = int((LongTemp + 180)/6) + 1;
211  else
212  to.zone = zone;
213 
214  if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
215  to.zone = 32;
216 
217  // Special zones for Svalbard
218  if( Lat >= 72.0 && Lat < 84.0 )
219  {
220  if( LongTemp >= 0.0 && LongTemp < 9.0 ) to.zone = 31;
221  else if( LongTemp >= 9.0 && LongTemp < 21.0 ) to.zone = 33;
222  else if( LongTemp >= 21.0 && LongTemp < 33.0 ) to.zone = 35;
223  else if( LongTemp >= 33.0 && LongTemp < 42.0 ) to.zone = 37;
224  }
225  // +3 puts origin in middle of zone
226  LongOrigin = (to.zone - 1)*6 - 180 + 3;
227  LongOriginRad = angles::from_degrees(LongOrigin);
228 
229  // compute the UTM band from the latitude
230  if (!force_zone)
231  to.band = UTMBand(Lat, LongTemp);
232  else
233  to.band = band;
234 
235 
236 
237 #if 0
238  if (to.band == ' ')
239  throw std::range_error;
240 #endif
241 
242  eccPrimeSquared = (eccSquared)/(1-eccSquared);
243 
244  N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
245  T = tan(LatRad)*tan(LatRad);
246  C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
247  A = cos(LatRad)*(LongRad-LongOriginRad);
248 
249  M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64
250  - 5*eccSquared*eccSquared*eccSquared/256) * LatRad
251  - (3*eccSquared/8 + 3*eccSquared*eccSquared/32
252  + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
253  + (15*eccSquared*eccSquared/256
254  + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
255  - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
256 
257  to.easting = (double)
258  (k0*N*(A+(1-T+C)*A*A*A/6
259  + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
260  + 500000.0);
261 
262  to.northing = (double)
263  (k0*(M+N*tan(LatRad)
264  *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
265  + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
266 
267  if(Lat < 0)
268  {
269  //10000000 meter offset for southern hemisphere
270  to.northing += 10000000.0;
271  }
272 }
273 
275 bool isValid(const UTMPoint &pt)
276 {
277  if (pt.zone < 1 || pt.zone > 60)
278  return false;
279 
280  if (!isupper(pt.band) || pt.band == 'I' || pt.band == 'O')
281  return false;
282 
283  // The Universal Polar Stereographic bands are not currently
284  // supported. When they are: A, B, Y and Z will be valid (and the
285  // zone number will be ignored).
286  if (pt.band < 'C' || pt.band > 'X')
287  return false;
288 
289  return true;
290 }
291 
293 UTMPoint::UTMPoint(const geographic_msgs::GeoPoint &pt)
294 {
295  fromMsg(pt, *this);
296 }
297 
303 geographic_msgs::GeoPose toMsg(const UTMPose &from)
304 {
305  geographic_msgs::GeoPose to;
306  to.position = toMsg(from.position);
307  to.orientation = from.orientation;
308  return to;
309 }
310 
318 void fromMsg(const geographic_msgs::GeoPose &from, UTMPose &to,
319  const bool& force_zone, const char& band, const uint8_t& zone)
320 {
321  fromMsg(from.position, to.position, force_zone, band, zone);
322  to.orientation = from.orientation;
323 }
324 
326 bool isValid(const UTMPose &pose)
327 {
328  if (!isValid(pose.position))
329  return false;
330 
331  // check that orientation quaternion is normalized
332  double len2 = (pose.orientation.x * pose.orientation.x
333  + pose.orientation.y * pose.orientation.y
334  + pose.orientation.z * pose.orientation.z
335  + pose.orientation.w * pose.orientation.w);
336  return fabs(len2 - 1.0) <= tf::QUATERNION_TOLERANCE;
337 }
338 
339 } // end namespace geodesy
char band
MGRS latitude band letter.
Definition: utm.h:130
Definition: utm.h:68
Universal Transverse Mercator coordinates.
void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to, const bool &force_zone=false, const char &band='A', const uint8_t &zone=0)
static const double QUATERNION_TOLERANCE
TFSIMD_FORCE_INLINE const tfScalar & y() const
double northing
northing within grid zone [meters]
Definition: utm.h:127
double altitude
altitude above ellipsoid [meters] or NaN
Definition: utm.h:128
geometry_msgs::Quaternion orientation
Definition: utm.h:173
#define WGS84_A
uint8_t zone
UTM longitude zone number.
Definition: utm.h:129
static double from_degrees(double degrees)
#define UTM_K0
static double to_degrees(double radians)
TFSIMD_FORCE_INLINE const tfScalar & x() const
static void normalize(UTMPoint &pt)
Definition: utm.h:206
#define UTM_E2
bool isValid(const UTMPoint &pt)
double easting
easting within grid zone [meters]
Definition: utm.h:126
UTMPoint position
Definition: utm.h:172
geographic_msgs::GeoPoint toMsg(const UTMPoint &from)
static char UTMBand(double Lat, double Lon)


geodesy
Author(s): Jack O'Quin
autogenerated on Sat May 8 2021 02:29:15