UTM conversions using geographiclib

CMakeLists.txt

@@ -81,7 +81,7 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS
     include
-  LIBRARIES 
+  LIBRARIES
     ekf
     ekf_localization_nodelet
     filter_base
@@ -331,4 +331,8 @@ if (CATKIN_ENABLE_TESTING)
                         ${catkin_LIBRARIES}
                         ${rostest_LIBRARIES})
 
+  #### NAVSAT CONVERSION TESTS ####
+  catkin_add_gtest(navsat_conversions-test test/test_navsat_conversions.cpp)
+  target_link_libraries(navsat_conversions-test navsat_transform ${catkin_LIBRARIES})
+
 endif()

include/robot_localization/navsat_conversions.h

@@ -54,6 +54,9 @@
 #include <stdio.h>
 #include <stdlib.h>
 
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
 namespace RobotLocalization
 {
 namespace NavsatConversions
@@ -135,232 +138,69 @@ static inline void UTM(double lat, double lon, double *x, double *y)
   return;
 }
 
-
 /**
- * Determine the correct UTM letter designator for the
- * given latitude
- *
- * @returns 'Z' if latitude is outside the UTM limits of 84N to 80S
- *
- * Written by Chuck Gantz- [email protected]
- */
-static inline char UTMLetterDesignator(double Lat)
-{
-  char LetterDesignator;
-
-  if     ((84 >= Lat) && (Lat >= 72))  LetterDesignator = 'X';
-  else if ((72 > Lat) && (Lat >= 64))  LetterDesignator = 'W';
-  else if ((64 > Lat) && (Lat >= 56))  LetterDesignator = 'V';
-  else if ((56 > Lat) && (Lat >= 48))  LetterDesignator = 'U';
-  else if ((48 > Lat) && (Lat >= 40))  LetterDesignator = 'T';
-  else if ((40 > Lat) && (Lat >= 32))  LetterDesignator = 'S';
-  else if ((32 > Lat) && (Lat >= 24))  LetterDesignator = 'R';
-  else if ((24 > Lat) && (Lat >= 16))  LetterDesignator = 'Q';
-  else if ((16 > Lat) && (Lat >= 8))   LetterDesignator = 'P';
-  else if (( 8 > Lat) && (Lat >= 0))   LetterDesignator = 'N';
-  else if (( 0 > Lat) && (Lat >= -8))  LetterDesignator = 'M';
-  else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
-  else if ((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
-  else if ((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
-  else if ((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
-  else if ((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
-  else if ((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
-  else if ((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
-  else if ((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
-  else if ((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
-        // 'Z' is an error flag, the Latitude is outside the UTM limits
-  else LetterDesignator = 'Z';
-  return LetterDesignator;
-}
-
-/**
- * Convert lat/long to UTM coords.  Equations from USGS Bulletin 1532
+ * Convert lat/long to UTM coords.
  *
  * 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 LLtoUTM(const double Lat, const double Long,
                            double &UTMNorthing, double &UTMEasting,
                            std::string &UTMZone, double &gamma)
 {
-  double a = WGS84_A;
-  double eccSquared = UTM_E2;
-  double k0 = UTM_K0;
-
-  double LongOrigin;
-  double eccPrimeSquared;
-  double N, T, C, A, M;
-
-  // Make sure the longitude is between -180.00 .. 179.9
-  double LongTemp = (Long+180)-static_cast<int>((Long+180)/360)*360-180;
-
-  double LatRad = Lat*RADIANS_PER_DEGREE;
-  double LongRad = LongTemp*RADIANS_PER_DEGREE;
-  double LongOriginRad;
-  int    ZoneNumber;
-
-  ZoneNumber = static_cast<int>((LongTemp + 180)/6) + 1;
-
-  if ( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
-    ZoneNumber = 32;
-
-        // Special zones for Svalbard
-  if ( Lat >= 72.0 && Lat < 84.0 )
-  {
-    if (      LongTemp >= 0.0  && LongTemp <  9.0 ) ZoneNumber = 31;
-    else if ( LongTemp >= 9.0  && LongTemp < 21.0 ) ZoneNumber = 33;
-    else if ( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35;
-    else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37;
-  }
-        // +3 puts origin in middle of zone
-  LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
-  LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
-
-  // Compute the UTM Zone from the latitude and longitude
-  char zone_buf[] = {0, 0, 0, 0};
-        // We &0x3fU to let GCC know the size of ZoneNumber.  In this case, it's under 63 (6bits)
-  snprintf(zone_buf, sizeof(zone_buf), "%d%c", ZoneNumber & 0x3fU, UTMLetterDesignator(Lat));
-  UTMZone = std::string(zone_buf);
-
-  eccPrimeSquared = (eccSquared)/(1-eccSquared);
-
-  N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
-  T = tan(LatRad)*tan(LatRad);
-  C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
-  A = cos(LatRad)*(LongRad-LongOriginRad);
-
-  M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64
-                - 5*eccSquared*eccSquared*eccSquared/256) * LatRad
-               - (3*eccSquared/8 + 3*eccSquared*eccSquared/32
-                  + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
-               + (15*eccSquared*eccSquared/256
-                  + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
-               - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
-
-  UTMEasting = static_cast<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);
-
-  UTMNorthing = static_cast<double>
-          (k0*(M+N*tan(LatRad)
-               *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
-                 + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
-
-  gamma = atan(tan(LongRad-LongOriginRad)*sin(LatRad)) * DEGREES_PER_RADIAN;
-
-  if (Lat < 0)
-          {
-            // 10000000 meter offset for southern hemisphere
-            UTMNorthing += 10000000.0;
-          }
+  int zone;
+  bool northp;
+  double k_unused;
+  GeographicLib::UTMUPS::Forward(Lat, Long, zone, northp, UTMEasting, UTMNorthing, gamma,
+                                 k_unused, GeographicLib::UTMUPS::zonespec::MATCH);
+  GeographicLib::MGRS::Forward(zone, northp, UTMEasting, UTMNorthing, -1, UTMZone);
 }
 
 /**
- * Convert lat/long to UTM coords.  Equations from USGS Bulletin 1532
+ * Convert lat/long to UTM coords.
  *
  * 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 LLtoUTM(const double Lat, const double Long,
                            double &UTMNorthing, double &UTMEasting,
                            std::string &UTMZone)
 {
-  double gamma;
+  double gamma = 0.0;
   LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, UTMZone, gamma);
 }
 
 /**
- * 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)

test/test_navsat_conversions.cpp

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2021, Charles River Analytics, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "robot_localization/navsat_conversions.h"
+
+#include <gtest/gtest.h>
+
+#include <string>
+
+void NavsatConversionsTest(const double lat, const double lon,
+                           const double UTMNorthing, const double UTMEasting, const std::string UTMZone)
+{
+    double UTMNorthing_new;
+    double UTMEasting_new;
+    std::string UTMZone_new;
+    RobotLocalization::NavsatConversions::LLtoUTM(lat, lon, UTMNorthing_new, UTMEasting_new, UTMZone_new);
+    EXPECT_NEAR(UTMNorthing, UTMNorthing_new, 1e-2);
+    EXPECT_NEAR(UTMEasting, UTMEasting_new, 1e-2);
+    EXPECT_EQ(UTMZone, UTMZone_new);
+    double lat_new;
+    double lon_new;
+    RobotLocalization::NavsatConversions::UTMtoLL(UTMNorthing, UTMEasting, UTMZone, lat_new, lon_new);
+    EXPECT_NEAR(lat_new, lat, 1e-5);
+    EXPECT_NEAR(lon_new, lon, 1e-5);
+}
+
+TEST(NavsatConversionsTest, UtmTest)
+{
+    NavsatConversionsTest(51.423964, 5.494271, 5699924.709, 673409.989, "31U");
+    NavsatConversionsTest(-43.530955, 172.636645, 5178919.718, 632246.802, "59G");
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}