CommonRoad / commonroad-scenario-designer

Toolbox for Map Conversion and Scenario Creation for Autonomous Vehicles.
https://commonroad.in.tum.de/tools/scenario-designer
GNU General Public License v3.0
26 stars 9 forks source link

Coordinate Conversion Error in OpenDrive -> Lanelet2 #13

Closed ataparlar closed 8 months ago

ataparlar commented 8 months ago

Hi,

I am trying to convert an example OpenDrive format to Lanelet2 format. Here is the file link.

The problem looks like in this entry in the CommonRoad forum. The GeoTransformation does not work correctly right now. I used to use this method in OSM -> Lanelet2 conversion.

What am I doing wrong? Is there any suitable branch in the repository?

This is the code that I am using:

import os
from lxml import etree
from crdesigner.common.config.lanelet2_config import lanelet2_config
from crdesigner.map_conversion.lanelet2.cr2lanelet import CR2LaneletConverter
from commonroad.scenario.scenario import Location, GeoTransformation
from crdesigner.map_conversion.map_conversion_interface import opendrive_to_commonroad
from pathlib import Path

input_path_str = "/home/ataparlar/temp_backup/crdesigner/import/opendrive_test_file_town.xodr"
input_path = Path(input_path_str)
output_lanelet2_path = "/home/ataparlar/projects/commonroad_project2/commonroad-scenario-designer/exports/opendrive_to_lanelet2_test.osm"
scenario = opendrive_to_commonroad(input_path)
georeference_string = "EPSG:3857"
location_geotransformation = GeoTransformation(georeference_string, 658761.0, 4542599.0, None, None)
scenario_location = Location(11, 41.0, 28.0, location_geotransformation, None)
scenario.location = scenario_location
if scenario:
    l2osm = CR2LaneletConverter(lanelet2_config)
    osm = l2osm(scenario)
    with open(output_lanelet2_path, "wb") as file_out:
        file_out.write(
            etree.tostring(
                osm, xml_declaration=True, encoding="UTF-8", pretty_print=True
        )
    )

Thank you for your supports

Ata Parlar

smaierhofer commented 8 months ago

Hi Ata,

I think the lat/lon translation is not correct. I just checked the values for 41.0/28.0 here and the values should be different. It might also be necessary to use scenario.translate_rotate().

Usually, the projection in the OpenDRIVE map already contains an offset (if necessary). In this case, the manual translation is not necessary since it will be applied automatically.

ataparlar commented 8 months ago

Hi Sebastian,

I noticed that, if there is no georeference in the OpenDrive file, it does not matter if you specify a Location of Scenario. No matter what, it makes the georeferencing of lanelet2 map with the georeference string in the OpenDrive file. However scenario.translate_rotate() is working in every situation. The best result I got was on EPSG:3857 transformed coordinates with specified latitudes and longitudes. Here is the working version of the code:

import os
from lxml import etree
from crdesigner.common.config.lanelet2_config import lanelet2_config
from crdesigner.map_conversion.lanelet2.cr2lanelet import CR2LaneletConverter
from commonroad.scenario.scenario import Location, GeoTransformation
from crdesigner.map_conversion.map_conversion_interface import opendrive_to_commonroad
from pathlib import Path
import numpy as np

input_path_str = "/home/ataparlar/data/gis_data/opendrive_test_files/generated/simpleRoadEnvOrigined.xodr"
input_path = Path(input_path_str)
output_lanelet2_path = "/home/ataparlar/data/gis_data/opendrive_test_files/generated/georef_issue/simpleRoadEnvOrigined_georef.osm"

scenario = opendrive_to_commonroad(input_path)

georeference_string = "EPSG:3857"

# lat: 41.019141875, lon: 28.88822681013889
translation = np.array([3215822.6824539998, 5015165.506405752])  # EPSG:3857
scenario.translate_rotate(translation, 0.0)

lanelet2_config.autoware = True

if scenario:
    l2osm = CR2LaneletConverter(lanelet2_config)
    osm = l2osm(scenario)
    with open(output_lanelet2_path, "wb") as file_out:
        file_out.write(
            etree.tostring(
                osm, xml_declaration=True, encoding="UTF-8", pretty_print=True
        )
    )

Thank you for your help

Best regards, Ata Parlar