fzi-forschungszentrum-informatik / Lanelet2

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

How to load an .osm file with the Geocentric projection to get the data in ECEF #324

Closed kopp closed 11 months ago

kopp commented 11 months ago

I would like to load a .osm file (e.g. the provided example mapping_example.osm) and use the data in ECEF.

I would have expected to be able to do something like

lanelet2.io.load("/tmp/mapping_example.osm", projector=lanelet2.projection.GeocentricProjector())

which sadly fails with

RuntimeError: You must pass an origin when loading a map with georeferenced (lat/lon) data!

By looking at the code I do not really understand this error and how to circumvent it. The GeocentricPorjector does not accept an origin when creating an instance -- and why should it do so; ECEF should be 'globally accurate', not depending on any origin.

During loading, the projector is checked whether it still has the 'default origin' set -- which is true (because we cannot change the origin) and the function fails with above error.

Please advise how to properly load the .osm file to get the data in ECEF.

kopp commented 11 months ago

@mantkiew you seem to have provided the Geocentric code, do you have a good idea?

kopp commented 11 months ago

Or to make the code more consistent: For ECEF we do know the origin: It's the center of the earth. So how about for GeocentricProjector we set the origin to GpsPoint{.lat = 90, .lon = 0, .ele = -6356752.3}, i.e. the center of the earth?

mantkiew commented 11 months ago

Hi, the tutorial shows using loadRobust, which does not take any origin, just the projector. https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/33e797a5f4c3ac4cf4f63316fcd6965c9cf0cf57/lanelet2_examples/scripts/tutorial.py#L192C35-L192C35

I think it's the same with load, not sure why it would complain about not providing the origin.

There's the old API, where you can give just an origin and it creates some bad default projector, but that should not be used. Always create the projector you want and use that consistently.

kopp commented 11 months ago

Hi @mantkiew , thanks for the reply. I tried that but failed, see below.

I think just passing a projector does make sense -- and the projectors which need an origin actually 'carry their own' origin with them (they get constructed from it). The load API allows to pass only a projector, if that has the origin set. If not, we get the error I was reporting. My feeling is, that in the ECEF case we either should provide an origin (e.g. center of the earth) to satisfy this check or disable this check for projectors which do not need an origin.

For full reproducibility, here my console log:

% pip install lanelet2
Collecting lanelet2
...
Successfully installed lanelet2-1.2.1

% wget https://github.com/fzi-forschungszentrum-informatik/Lanelet2/raw/master/lanelet2_maps/res/mapping_example.osm
....
2023-12-05 16:53:22 (1.27 MB/s) - ‘mapping_example.osm’ saved [594826/594826]

% python
Python 3.8.10 (default, Nov 22 2023, 10:22:35) 
[GCC 9.4.0] on linux
Type "help", "copyright", "credits" or "license" for more information.
>>> import lanelet2
>>> ecef_projector = lanelet2.projection.GeocentricProjector()
>>> m = lanelet2.io.loadRobust("mapping_example.osm", ecef_projector)
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
RuntimeError: You must pass an origin when loading a map with georeferenced (lat/lon) data!

So looking at the code I think load and loadRobust do the same thing, while one throws and the other prints.

mantkiew commented 11 months ago

This is the culprit (handleDefaultProjector throws the exception) : https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/33e797a5f4c3ac4cf4f63316fcd6965c9cf0cf57/lanelet2_io/include/lanelet2_io/io_handlers/IoHandler.h#L34-L39

The GeocentricProjector does not use origin at all, because it's hardcoded in the underlying LibGeographic C++ library, and so it only has the default one defined in the class Projector: https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/33e797a5f4c3ac4cf4f63316fcd6965c9cf0cf57/lanelet2_io/include/lanelet2_io/Projection.h#L25

So, to fix this: the GeocentricProjector should overwrite the origin with whatever value that is different from the default projector.

mantkiew commented 11 months ago

@poggenhans @immel-f this PR fixes this error. It also adds usage demonstration to tutorial.py.