UTM conversions using geographiclib #626

Timple · 2021-02-23T15:42:28Z

For #222 it made sense to use the GeographicLib library that #575 already introduced.

In preparation I rewrote the current UTM functions.

ayrton04 · 2021-03-22T09:59:42Z

Thanks for this, and for the tests. Are you happy to have this reviewed now?

Timple · 2021-03-22T10:03:00Z

Certainly.

ayrton04 · 2021-03-22T10:36:12Z

include/robot_localization/navsat_conversions.h

-
-/**
- * Convert lat/long to UTM coords.  Equations from USGS Bulletin 1532
+ * Convert lat/long to UTM coords.


Can we also get rid of all of this? Or do we still need it?

robot_localization/include/robot_localization/navsat_conversions.h

Lines 65 to 139 in ab6e851

const double RADIANS_PER_DEGREE = M_PI/180.0;

const double DEGREES_PER_RADIAN = 180.0/M_PI;

// Grid granularity for rounding UTM coordinates to generate MapXY.

const double grid_size = 100000.0; // 100 km grid

// WGS84 Parameters

#define WGS84_A 6378137.0 // major axis

#define WGS84_B 6356752.31424518 // minor axis

#define WGS84_F 0.0033528107 // ellipsoid flattening

#define WGS84_E 0.0818191908 // first eccentricity

#define WGS84_EP 0.0820944379 // second eccentricity

// UTM Parameters

#define UTM_K0 0.9996 // scale factor

#define UTM_FE 500000.0 // false easting

#define UTM_FN_N 0.0 // false northing, northern hemisphere

#define UTM_FN_S 10000000.0 // false northing, southern hemisphere

#define UTM_E2 (WGS84_E*WGS84_E) // e^2

#define UTM_E4 (UTM_E2*UTM_E2) // e^4

#define UTM_E6 (UTM_E4*UTM_E2) // e^6

#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2

/**

* Utility function to convert geodetic to UTM position

*

* Units in are floating point degrees (sign for east/west)

*

* Units out are meters

*

* @todo deprecate this interface in favor of LLtoUTM()

*/

static inline void UTM(double lat, double lon, double *x, double *y)

{

// constants

static const double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);

static const double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);

static const double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);

static const double m3 = -(35*UTM_E6/3072);

// compute the central meridian

int cm = ((lon >= 0.0)

? (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 + 3)

: (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 - 3));

// convert degrees into radians

double rlat = lat * RADIANS_PER_DEGREE;

double rlon = lon * RADIANS_PER_DEGREE;

double rlon0 = cm * RADIANS_PER_DEGREE;

// compute trigonometric functions

double slat = sin(rlat);

double clat = cos(rlat);

double tlat = tan(rlat);

// decide the false northing at origin

double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;

double T = tlat * tlat;

double C = UTM_EP2 * clat * clat;

double A = (rlon - rlon0) * clat;

double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)

+ m2*sin(4*rlat) + m3*sin(6*rlat));

double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);

// compute the easting-northing coordinates

*x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A, 3)/6

+ (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A, 5)/120);

*y = fn + UTM_K0 * (M + V * tlat * (A*A/2

+ (5-T+9*C+4*C*C)*pow(A, 4)/24

+ ((61-58*T+T*T+600*C-330*UTM_EP2)

* pow(A, 6)/720)));

return;

}

And if we do get rid of that, can we also get rid of the other comments and update the license?

robot_localization/include/robot_localization/navsat_conversions.h

Lines 65 to 139 in ab6e851

const double RADIANS_PER_DEGREE = M_PI/180.0;

const double DEGREES_PER_RADIAN = 180.0/M_PI;

// Grid granularity for rounding UTM coordinates to generate MapXY.

const double grid_size = 100000.0; // 100 km grid

// WGS84 Parameters

#define WGS84_A 6378137.0 // major axis

#define WGS84_B 6356752.31424518 // minor axis

#define WGS84_F 0.0033528107 // ellipsoid flattening

#define WGS84_E 0.0818191908 // first eccentricity

#define WGS84_EP 0.0820944379 // second eccentricity

// UTM Parameters

#define UTM_K0 0.9996 // scale factor

#define UTM_FE 500000.0 // false easting

#define UTM_FN_N 0.0 // false northing, northern hemisphere

#define UTM_FN_S 10000000.0 // false northing, southern hemisphere

#define UTM_E2 (WGS84_E*WGS84_E) // e^2

#define UTM_E4 (UTM_E2*UTM_E2) // e^4

#define UTM_E6 (UTM_E4*UTM_E2) // e^6

#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2

/**

* Utility function to convert geodetic to UTM position

*

* Units in are floating point degrees (sign for east/west)

*

* Units out are meters

*

* @todo deprecate this interface in favor of LLtoUTM()

*/

static inline void UTM(double lat, double lon, double *x, double *y)

{

// constants

static const double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);

static const double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);

static const double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);

static const double m3 = -(35*UTM_E6/3072);

// compute the central meridian

int cm = ((lon >= 0.0)

? (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 + 3)

: (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 - 3));

// convert degrees into radians

double rlat = lat * RADIANS_PER_DEGREE;

double rlon = lon * RADIANS_PER_DEGREE;

double rlon0 = cm * RADIANS_PER_DEGREE;

// compute trigonometric functions

double slat = sin(rlat);

double clat = cos(rlat);

double tlat = tan(rlat);

// decide the false northing at origin

double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;

double T = tlat * tlat;

double C = UTM_EP2 * clat * clat;

double A = (rlon - rlon0) * clat;

double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)

+ m2*sin(4*rlat) + m3*sin(6*rlat));

double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);

// compute the easting-northing coordinates

*x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A, 3)/6

+ (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A, 5)/120);

*y = fn + UTM_K0 * (M + V * tlat * (A*A/2

+ (5-T+9*C+4*C*C)*pow(A, 4)/24

+ ((61-58*T+T*T+600*C-330*UTM_EP2)

* pow(A, 6)/720)));

return;

}

robot_localization/include/robot_localization/navsat_conversions.h

Lines 65 to 139 in ab6e851

const double RADIANS_PER_DEGREE = M_PI/180.0;

const double DEGREES_PER_RADIAN = 180.0/M_PI;

// Grid granularity for rounding UTM coordinates to generate MapXY.

const double grid_size = 100000.0; // 100 km grid

// WGS84 Parameters

#define WGS84_A 6378137.0 // major axis

#define WGS84_B 6356752.31424518 // minor axis

#define WGS84_F 0.0033528107 // ellipsoid flattening

#define WGS84_E 0.0818191908 // first eccentricity

#define WGS84_EP 0.0820944379 // second eccentricity

// UTM Parameters

#define UTM_K0 0.9996 // scale factor

#define UTM_FE 500000.0 // false easting

#define UTM_FN_N 0.0 // false northing, northern hemisphere

#define UTM_FN_S 10000000.0 // false northing, southern hemisphere

#define UTM_E2 (WGS84_E*WGS84_E) // e^2

#define UTM_E4 (UTM_E2*UTM_E2) // e^4

#define UTM_E6 (UTM_E4*UTM_E2) // e^6

#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2

/**

* Utility function to convert geodetic to UTM position

*

* Units in are floating point degrees (sign for east/west)

*

* Units out are meters

*

* @todo deprecate this interface in favor of LLtoUTM()

*/

static inline void UTM(double lat, double lon, double *x, double *y)

{

// constants

static const double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);

static const double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);

static const double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);

static const double m3 = -(35*UTM_E6/3072);

// compute the central meridian

int cm = ((lon >= 0.0)

? (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 + 3)

: (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 - 3));

// convert degrees into radians

double rlat = lat * RADIANS_PER_DEGREE;

double rlon = lon * RADIANS_PER_DEGREE;

double rlon0 = cm * RADIANS_PER_DEGREE;

// compute trigonometric functions

double slat = sin(rlat);

double clat = cos(rlat);

double tlat = tan(rlat);

// decide the false northing at origin

double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;

double T = tlat * tlat;

double C = UTM_EP2 * clat * clat;

double A = (rlon - rlon0) * clat;

double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)

+ m2*sin(4*rlat) + m3*sin(6*rlat));

double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);

// compute the easting-northing coordinates

*x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A, 3)/6

+ (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A, 5)/120);

*y = fn + UTM_K0 * (M + V * tlat * (A*A/2

+ (5-T+9*C+4*C*C)*pow(A, 4)/24

+ ((61-58*T+T*T+600*C-330*UTM_EP2)

* pow(A, 6)/720)));

return;

}

We can.
I left them in because they are exposed to other packages. So I'm not sure who might be using these variables...

That's a good point. I'll bring this forward to the ROS2 branches, and for Galactic, I'll remove them. Thanks.

I think the whole conversions file can be removed in that case. Since there is now a library doing all that.

@ayrton04 do you still plan for removing these (quite empty) functions for ROS2 and galactic?

Seems like they are re-used: #676
Which is a bit of a waste.

test/test_navsat_conversions.cpp

ayrton04 · 2021-03-22T11:03:56Z

@SteveMacenski, anything glaring to you on this or #627?

SteveMacenski · 2021-03-22T18:43:17Z

I haven't used geographic lib / GPS before in a product so I can't say I have an opinion. Though I'd want a PR analog to this for ROS2 to keep them in sync

Timple · 2021-03-25T08:26:53Z

I could do that. Which branch(es) would I need to target? And which of those would allow breaking changes?

SteveMacenski · 2021-03-25T16:59:22Z

The ros2 branch would allow breaking changes

Timple · 2021-04-06T09:00:14Z

Though I'd want a PR analog to this for ROS2 to keep them in sync

#643

I could not target the ROS2 branch because it's too much behind on Foxy.

SteveMacenski · 2021-04-06T16:57:01Z

@Timple what do you mean, I'm looking at the ROS2->Foxy diff right here its only off by 4 commits that are just rclcpp API changes (only covering like ~80 lines). We need any changes to Foxy to be in ros2 as well or else this change will never be reflected into future distributions and we'll be back here every 12 months

Timple · 2021-04-08T08:58:54Z

Well that is strange. Than how come there is no mention of geographiclib in the ros2 package.xml but there is in the foxy-devel branch?

Did a merge go wrong somewhere?

SteveMacenski · 2021-04-08T17:19:36Z

It looks like those were added 2 weeks ago only in the ROS1 feature sync PR 3dbd48a

@ayrton04 is it your intention to merge this into ros2? Any conflicts you'd expect from cherry picking between foxy and ros2?

ayrton04 · 2021-04-12T08:59:23Z

I can't think of anything offhand.

For the record, the model that I originally tried to follow was;

Create a new <release_name>-devel branch prior to the release being finalized
Let all the new featured (and ABI breakages) go into <release_name>-devel
When <release_name>-devel is released, that branch should only accept bug fixes or minor features that don't break ABI
When <release_name>-devel is released, we also create <next_release_name>-devel and start putting both the bug fixes from <release_name>-devel and new features into <next_release_name>-devel.

Obviously, there have been some violations of that paradigm, but that's generally the way I wanted to do things.

The ros2 branch suggests a slightly different model, I think, though I'm not sure what we do for new ROS2 releases.

All new features and bug fixes to any <release_name>-devel branch should find their way to ros2
If a new ROS2 release is cut, do we then branch ros2 into <previous_release>-devel and let all future development happen on the ros2 branch? Or do we just keep using ros2 for all ROS2 releases until ABI breakage or incompatibility with the rest of the stack for a given ROS2 release forces us to branch?

Timple · 2021-04-12T09:25:58Z

Both suggestions result in the ros2 branch to be some kind of a 'rolling' branch allowing breaking changes.

All new features and bug fixes to any <release_name>-devel branch should find their way to ros2

Certainly this.

So I'll target the foxy-devel for the backwards compatibility I build in. And I'll target the ros2 (once it gets updated) to include the breaking changes proposed.

SteveMacenski · 2021-04-12T18:08:25Z

The new ROS2-y workflow is

Have a master ros2 or similarly named branch that all development is done on, targeting ROS2 rolling / master builds
When you release a new distro, you branch off ros2 at the time of a new distro and that's the new distro release to use
Over time, sync the distro and ros2 branches by cherry picking commits from ros2 to the distro that are ABI compatible for release to those distros

Depending on my laziness, I also sometimes ask people to target their PR to multiple branches (ros2, and foxy, and noetic or something) to avoid the cherry picking later / forgetting.

* Use GeographicLib for UTMtoLL conversions

Timple added 4 commits February 23, 2021 19:59

Add single test for navsat_conversions

6852587

Add a southern point to the navsat_transform test

8f5bead

LLtoUTM using GeographicLib

11e821a

Use GeographicLib for UTMtoLL conversions

ab6e851

Timple force-pushed the utm-using-geographiclib branch from e8c54ca to ab6e851 Compare February 23, 2021 19:00

ayrton04 reviewed Mar 22, 2021

View reviewed changes

2 space indentations

e60bc1b

ayrton04 approved these changes Mar 22, 2021

View reviewed changes

Timple mentioned this pull request Mar 31, 2021

Utm using geographiclib foxy #643

Merged

ayrton04 merged commit 5eec32b into cra-ros-pkg:noetic-devel Apr 12, 2021

ayrton04 pushed a commit that referenced this pull request Jun 3, 2021

UTM conversions using geographiclib (#626)

2f238a6

* Use GeographicLib for UTMtoLL conversions

VAM7686 mentioned this pull request Jul 17, 2021

changed geographiclib to <depend> tag #684

Merged

Timple mentioned this pull request Dec 22, 2023

Feature/set utm service #856

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

UTM conversions using geographiclib #626

UTM conversions using geographiclib #626

Timple commented Feb 23, 2021

ayrton04 commented Mar 22, 2021

Timple commented Mar 22, 2021

ayrton04 Mar 22, 2021

Timple Mar 22, 2021

ayrton04 Mar 22, 2021

Timple Mar 22, 2021

ayrton04 Mar 22, 2021

Timple Jul 1, 2021

ayrton04 commented Mar 22, 2021

SteveMacenski commented Mar 22, 2021

Timple commented Mar 25, 2021

SteveMacenski commented Mar 25, 2021

Timple commented Apr 6, 2021

SteveMacenski commented Apr 6, 2021 •

edited

Loading

Timple commented Apr 8, 2021 •

edited

Loading

SteveMacenski commented Apr 8, 2021

ayrton04 commented Apr 12, 2021

Timple commented Apr 12, 2021

SteveMacenski commented Apr 12, 2021 •

edited

Loading

	const double RADIANS_PER_DEGREE = M_PI/180.0;
	const double DEGREES_PER_RADIAN = 180.0/M_PI;

	// Grid granularity for rounding UTM coordinates to generate MapXY.
	const double grid_size = 100000.0; // 100 km grid

	// WGS84 Parameters
	#define WGS84_A 6378137.0 // major axis
	#define WGS84_B 6356752.31424518 // minor axis
	#define WGS84_F 0.0033528107 // ellipsoid flattening
	#define WGS84_E 0.0818191908 // first eccentricity
	#define WGS84_EP 0.0820944379 // second eccentricity

	// UTM Parameters
	#define UTM_K0 0.9996 // scale factor
	#define UTM_FE 500000.0 // false easting
	#define UTM_FN_N 0.0 // false northing, northern hemisphere
	#define UTM_FN_S 10000000.0 // false northing, southern hemisphere
	#define UTM_E2 (WGS84_E*WGS84_E) // e^2
	#define UTM_E4 (UTM_E2*UTM_E2) // e^4
	#define UTM_E6 (UTM_E4*UTM_E2) // e^6
	#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2

	/**
	* Utility function to convert geodetic to UTM position
	*
	* Units in are floating point degrees (sign for east/west)
	*
	* Units out are meters
	*
	* @todo deprecate this interface in favor of LLtoUTM()
	*/
	static inline void UTM(double lat, double lon, double x, double y)
	{
	// constants
	static const double m0 = (1 - UTM_E2/4 - 3UTM_E4/64 - 5UTM_E6/256);
	static const double m1 = -(3UTM_E2/8 + 3UTM_E4/32 + 45*UTM_E6/1024);
	static const double m2 = (15UTM_E4/256 + 45UTM_E6/1024);
	static const double m3 = -(35*UTM_E6/3072);

	// compute the central meridian
	int cm = ((lon >= 0.0)
	? (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 + 3)
	: (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 - 3));

	// convert degrees into radians
	double rlat = lat * RADIANS_PER_DEGREE;
	double rlon = lon * RADIANS_PER_DEGREE;
	double rlon0 = cm * RADIANS_PER_DEGREE;

	// compute trigonometric functions
	double slat = sin(rlat);
	double clat = cos(rlat);
	double tlat = tan(rlat);

	// decide the false northing at origin
	double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;

	double T = tlat * tlat;
	double C = UTM_EP2 * clat * clat;
	double A = (rlon - rlon0) * clat;
	double M = WGS84_A * (m0rlat + m1sin(2*rlat)
	+ m2sin(4rlat) + m3sin(6rlat));
	double V = WGS84_A / sqrt(1 - UTM_E2slatslat);

	// compute the easting-northing coordinates
	x = UTM_FE + UTM_K0 V * (A + (1-T+C)*pow(A, 3)/6
	+ (5-18T+TT+72C-58UTM_EP2)*pow(A, 5)/120);
	y = fn + UTM_K0 (M + V * tlat * (A*A/2
	+ (5-T+9C+4CC)pow(A, 4)/24
	+ ((61-58T+TT+600C-330UTM_EP2)
	* pow(A, 6)/720)));

	return;
	}

UTM conversions using geographiclib #626

UTM conversions using geographiclib #626

Conversation

Timple commented Feb 23, 2021

ayrton04 commented Mar 22, 2021

Timple commented Mar 22, 2021

ayrton04 Mar 22, 2021

Choose a reason for hiding this comment

Timple Mar 22, 2021

Choose a reason for hiding this comment

ayrton04 Mar 22, 2021

Choose a reason for hiding this comment

Timple Mar 22, 2021

Choose a reason for hiding this comment

ayrton04 Mar 22, 2021

Choose a reason for hiding this comment

Timple Jul 1, 2021

Choose a reason for hiding this comment

ayrton04 commented Mar 22, 2021

SteveMacenski commented Mar 22, 2021

Timple commented Mar 25, 2021

SteveMacenski commented Mar 25, 2021

Timple commented Apr 6, 2021

SteveMacenski commented Apr 6, 2021 • edited Loading

Timple commented Apr 8, 2021 • edited Loading

SteveMacenski commented Apr 8, 2021

ayrton04 commented Apr 12, 2021

Timple commented Apr 12, 2021

SteveMacenski commented Apr 12, 2021 • edited Loading

SteveMacenski commented Apr 6, 2021 •

edited

Loading

Timple commented Apr 8, 2021 •

edited

Loading

SteveMacenski commented Apr 12, 2021 •

edited

Loading