fzi-forschungszentrum-informatik / Lanelet2

Map handling framework for automated driving
BSD 3-Clause "New" or "Revised" License
800 stars 327 forks source link

Incorrect UTM zone when extracting cooridinates from Lanelet #181

Closed miropipa closed 3 years ago

miropipa commented 3 years ago

I plotted my lanelets on JOSM in a street with a UTM coordinates of 363456 143662, at the zone 48N. I then wrote a function which publishes the coordinates of the centerline points on a text file. Given as followed:

for(const ConstPoint3d& point : lanelet.centerline()) { outputFile << point.x() << " " << point.y() << "\n"; }

However, the resulting easting UTM coordinates are way too off from where I have ploted the lanelets.(1.1549e+07 144564 .etc), which led me to suspect that the UTM zone referenced might be wrong. As the written easting coordinates are so large in values, they have now become redundant as they all share the same values in the text file.

Is there a way to rectify this problem such that the published coordinates reflects the UTM zone correctly?

poggenhans commented 3 years ago

I don't think this is a problem with the UTM zone. This rather looks like a misunderstanding in how the UTM projector works.

By default, all projectors in Lanelet2 work in local coordinates where the original origin has the coordinates (0,0). This is to keep the metric coordinates small for convience and to maximize floating point precision (which can be quite relevant, as you observed). Therefore, by default, the transformed points from the UTMProjector are not UTM coordinates, since they were shifted to be centered around the origin.

That the UTM projector internally picks the UTM zone that fits the origin best is rather an implementation detail that should not matter as long as you stay within the "Local lanelet2 world" or use the very same projector to get back to WGS84.

If you still want the local coordinates to be identical with UTM coordinates, you have to set the useOffset flag to false when initializing the UTMProjector and pick an origin close to the middle of the UTM tile, to make sure that the correct zone is used.

Also, I would suggest to increase the precision of the ofstream object to at least 10 or 12 to avoid losing digits:

outputFile << std::setprecision(12) << point.x() << " " << point.y();
miropipa commented 3 years ago

Thank you for your insight. I shall do the necessary corrections to my code.

HuaweiAlgolux commented 2 years ago

Basically, you need to enter the right latitude and longitude for the origin. Based on where your map is, you need to enter the corresponding latitude and longitude. For example, GoMentum Station is a testing facility in Concord, California, I googled and found out that the latitude and longitude of Concord city are 37.9780° N, 122.0311° W. The UTM zone of Concord is UTM 10. so when I'm reading the .osm file, I need to add 37.9 and -122.0 as the origin.

example_file = os.path.join(os.path.dirname(os.path.abspath(__file__)), "../../lanelet2_maps/res/gomentum.osm")
projector = UtmProjector(lanelet2.io.Origin(37.9, -122))
map = lanelet2.io.load(example_file, projector)

If you are using mapping_example.osm, which is located in Karlsruhe, Germany. And you find its lat and lon are 49.0069° N, 8.4037° E respectively. The UTM zone of Karlsruhe is UTM 32. so

example_file = os.path.join(os.path.dirname(os.path.abspath(__file__)), "../../lanelet2_maps/res/mapping_example.osm")
projector = UtmProjector(lanelet2.io.Origin(49, 8.4))
map = lanelet2.io.load(example_file, projector)

Note that the longitude of Karlsruhe is 8.4037° E, not 8.4037° W, so it's positive 8.4, not -8.4.

Also keep in mind that the origin (the lat and lon you entered) is treated as x=0, y=0, and z=0 in the local frame, all the other points with coordinates x, y, z are expressed relative to the origin.