Use GeographicLib for UTMtoLL conversions

include/robot_localization/navsat_conversions.h

@@ -175,86 +175,32 @@ static inline void LLtoUTM(const double Lat, const double Long,
 }
 
 /**
- * Converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
+ * Converts UTM coords to lat/long.
  *
  * East Longitudes are positive, West longitudes are negative.
  * North latitudes are positive, South latitudes are negative
  * Lat and Long are in fractional degrees.
- * Meridian convergence is computed as for Spherical Transverse Mercator,
- * which gives quite good approximation.
- * 
- * @param[out] gamma meridian convergence at point (degrees).
  *
- * Written by Chuck Gantz- [email protected]
+ * @param[out] gamma meridian convergence at point (degrees).
  */
 static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
                            const std::string &UTMZone, double& Lat, double& Long, double &gamma)
 {
-  double k0 = UTM_K0;
-  double a = WGS84_A;
-  double eccSquared = UTM_E2;
-  double eccPrimeSquared;
-  double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
-  double N1, T1, C1, R1, D, M;
-  double LongOrigin;
-  double mu, phi1Rad;
-  double x, y;
-  int ZoneNumber;
-  char* ZoneLetter;
-
-  x = UTMEasting - 500000.0;  // remove 500,000 meter offset for longitude
-  y = UTMNorthing;
-
-  ZoneNumber = strtoul(UTMZone.c_str(), &ZoneLetter, 10);
-  if ((*ZoneLetter - 'N') < 0)
-          {
-            // remove 10,000,000 meter offset used for southern hemisphere
-            y -= 10000000.0;
-          }
-
-  // +3 puts origin in middle of zone
-  LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
-  eccPrimeSquared = (eccSquared)/(1-eccSquared);
-
-  M = y / k0;
-  mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64
-                   -5*eccSquared*eccSquared*eccSquared/256));
-
-  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)
-                        + (151*e1*e1*e1/96)*sin(6*mu));
-
-  N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
-  T1 = tan(phi1Rad)*tan(phi1Rad);
-  C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
-  R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
-  D = x/(N1*k0);
-
-  Lat = phi1Rad - ((N1*tan(phi1Rad)/R1)
-                         *(D*D/2
-                           -(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
-                           +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared
-                             -3*C1*C1)*D*D*D*D*D*D/720));
-
-  Lat = Lat * DEGREES_PER_RADIAN;
-
-  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)
-                / cos(phi1Rad));
-  Long = LongOrigin + Long * DEGREES_PER_RADIAN;
-
-  gamma = atan(tanh(x/(k0*a))*tan(y/(k0*a))) * DEGREES_PER_RADIAN;
+  int zone;
+  bool northp;
+  double x_unused;
+  double y_unused;
+  int prec_unused;
+  GeographicLib::MGRS::Reverse(UTMZone, zone, northp, x_unused, y_unused, prec_unused, true);
+  GeographicLib::UTMUPS::Reverse(zone, northp, UTMEasting, UTMNorthing, Lat, Long);
 }
 
 /**
- * Converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
+ * Converts UTM coords to lat/long.
  *
  * East Longitudes are positive, West longitudes are negative.
  * North latitudes are positive, South latitudes are negative
  * Lat and Long are in fractional degrees.
- *
- * Written by Chuck Gantz- [email protected]
  */
 static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
                            const std::string &UTMZone, double& Lat, double& Long)