Utm using geographiclib foxy

CMakeLists.txt

@@ -256,6 +256,10 @@ if(BUILD_TESTING)
     WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   )
 
+  #### NAVSAT CONVERSION TESTS ####
+  ament_add_gtest(test_navsat_conversions test/test_navsat_conversions.cpp)
+  target_link_libraries(test_navsat_conversions ${library_name})
+
   ament_add_gtest_executable(test_ros_robot_localization_listener
                   test/test_ros_robot_localization_listener.cpp)
   target_link_libraries(test_ros_robot_localization_listener ${library_name})
@@ -290,6 +294,7 @@ if(BUILD_TESTING)
     #test_ekf_localization_node_bag2
     #test_ekf_localization_node_bag3
     test_robot_localization_estimator
+    test_navsat_conversions
     test_ros_robot_localization_listener
     test_ros_robot_localization_listener_publisher
     #test_ukf_localization_node_bag1

include/robot_localization/navsat_conversions.hpp

@@ -51,6 +51,9 @@
 #include <stdio.h>
 #include <stdlib.h>
 
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
 #include <cmath>
 #include <string>
 
@@ -140,276 +143,74 @@ static inline void UTM(double lat, double lon, double * x, double * y)
 }
 
 /**
- * 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';
-  } else {
-    // 'Z' is an error flag, the Latitude is outside the UTM limits
-    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]
  */
 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]
  */
 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;
+  double k_unused;
+  GeographicLib::MGRS::Reverse(UTMZone, zone, northp, x_unused, y_unused, prec_unused, true);
+  GeographicLib::UTMUPS::Reverse(zone, northp, UTMEasting, UTMNorthing, Lat, Long, gamma, k_unused);
 }
 
 /**
-* 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,

test/test_navsat_conversions.cpp

@@ -0,0 +1,72 @@
+/*
+ * 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 <gtest/gtest.h>
+
+#include <string>
+
+#include "robot_localization/navsat_conversions.hpp"
+
+void NavsatConversionsTest(
+  const double lat, const double lon,
+  const double UTMNorthing, const double UTMEasting,
+  const std::string UTMZone, const double gamma)
+{
+  double UTMNorthing_new;
+  double UTMEasting_new;
+  std::string UTMZone_new;
+  double gamma_new;
+  robot_localization::navsat_conversions::LLtoUTM(
+    lat, lon, UTMNorthing_new, UTMEasting_new, UTMZone_new, gamma_new);
+  EXPECT_NEAR(UTMNorthing, UTMNorthing_new, 1e-2);
+  EXPECT_NEAR(UTMEasting, UTMEasting_new, 1e-2);
+  EXPECT_EQ(UTMZone, UTMZone_new);
+  EXPECT_NEAR(gamma, gamma_new, 1e-2);
+  double lat_new;
+  double lon_new;
+  robot_localization::navsat_conversions::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", 1.950);
+  NavsatConversionsTest(-43.530955, 172.636645, 5178919.718, 632246.802, "59G", -1.127);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}