Skip to content

Commit

Permalink
UTM conversions using geographiclib (#626)
Browse files Browse the repository at this point in the history 
* Use GeographicLib for UTMtoLL conversions
  • Loading branch information
@Timple @ayrton04
Timple authored and ayrton04 committed Jun 3, 2021
1 parent 3fb3be5 commit 2f238a6
Show file tree
Hide file tree
Showing 3 changed files with 94 additions and 184 deletions.
6 changes: 5 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ generate_messages(
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
LIBRARIES
ekf
ekf_localization_nodelet
filter_base
Expand Down Expand Up @@ -328,4 +328,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()
206 changes: 23 additions & 183 deletions include/robot_localization/navsat_conversions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,9 @@
#include <stdio.h>
#include <stdlib.h>

#include <GeographicLib/MGRS.hpp>
#include <GeographicLib/UTMUPS.hpp>

namespace RobotLocalization
{
namespace NavsatConversions
Expand Down Expand Up @@ -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)
Expand Down
66 changes: 66 additions & 0 deletions test/test_navsat_conversions.cpp
View file
Original file line number Diff line number Diff line change
@@ -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();
}

0 comments on commit 2f238a6

Please sign in to comment.