Closed Gilaine closed 3 years ago
Hello @Gilaine. It can definitely be done, but if I recall correctly, there isn't a predefined function to do so.
If you go to the OpenDRIVE file of a town (Unreal/CarlaUE47Content/Carla/Maps/OpenDrive), you can see the geoReference (It is like the 3rd or 4th line of the .xodr file).
From there, some geometry is needed, but if you suppose that the map is flat, the geocoordinates correspond to the x and y (obviously, with some scale) and the altitude is just the z value.
Hello @glopezdiest sorry for my late response I'm checking right now, i found the reference for the long and lat
Yes, the scale is constant. What I meant by that is that 1 degree change of latitude will always correspond to the same change in y.
You basically know that you have had a certain change in degrees in a sphere the size of the earth. You then have to calculate what this degree change means in terms of distance at the surface of the earth.
Mathematically speaking, it would be something like this (they aren't the exact result, some terms are missing):
y = - (lat-49)earth_radius (Just emphasize the negative symbol here) x = (lon-8)earth_radius
I'd like to help you more giving you the exact formula but I just can't find where I put it, sorry.
are these the formulas ?
I appreciate your help so much.
Something like that yeah. I'd recommend using carla.DebugHelper, which paints the points in the simulation, so you can visually check if they are correct
I tried many equations but still couldn't find it @glopezdiest
I tried many equations but still couldn't find it @glopezdiest
Hi GIlaine, Carla's locations are defined in the unreal-engine frame-of-reference, which is a left-handed Cartesian frame, aligned with east-south-up (you can easily verify it by observing GeoLocation vs. Location values of a specific position.
You can transform a left-handed east-south-up location into a right-handed east-north-up location by reversing the sign of the y-coordinate. https://github.com/carla-simulator/ros-bridge/blob/master/carla_ros_bridge/src/carla_ros_bridge/transforms.py#L59.
You would then obtain a coordinate in the east-north-up (ENU) frame, with the origin of the ENU frame, (0,0,0), located at the geographic origin of the OpenDrive map. Then you need to transform these ENU coordinates to geographic coordinates.
As far as I know, a rigorous way would be to transform the ENU coordinates to earth-centered-earth-fixed (ECEF) coordinates and then transform the ECEF coordinates to geographic coordinates.
For example, using the rotation matrix from https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates along with a translation Another way would be to use an approximation, such as the one you've posted https://github.com/carla-simulator/carla/issues/2737#issuecomment-620941230.
Good luck, Gabi
Hello,
I wrote something like this, but I think still something is wrong:
The geo-reference in opendrive:
lat_0=4.9000000000000000e+1 lon_0= - 8.0000000000000000e+0
The reading:
latitude = 48.791695 longitude = - 8.413367 altitude = 1.376821
lat = np.deg2rad(latitude) lon = np.deg2rad(longitude) alt = np.deg2rad(altitude)
x = - math.sin(lon_0)lat - math.cos(lon_0)math.sin(lat_0)lon + math.cos(lon_0)math.cos(lat_0)*alt
y = math.cos(lon_0)lat - math.sin(lon_0)math.sin(lat_0)lon +math.sin(lon_0)math.cos(lat_0)*alt
z = math.cos(lat_0)lon + math.sin(lat_0)alt
@GabiShahmayster Hello Gabi,
I wrote something like this, but I think still something is wrong:
The geo-reference in opendrive:
lat_0=4.9000000000000000e+1 lon_0=8.0000000000000000e+0
The reading:
latitude = 48.791695 longitude = 8.413367 altitude = 1.376821
lat = np.deg2rad(latitude) lon = np.deg2rad(longitude) alt = np.deg2rad(altitude)
x = - math.sin(lon_0)lat - math.cos(lon_0)math.sin(lat_0)lon + math.cos(lon_0)math.cos(lat_0)*alt
y = - (math.cos(lon_0)lat - math.sin(lon_0)math.sin(lat_0)lon +math.sin(lon_0)math.cos(lat_0)*alt)
z = math.cos(lat_0)lon + math.sin(lat_0)alt
HI Gilaine,
First of all, I'm sorry, I probably misunderstood your original question and provided details regarding a carla location to geographic coordinates transformation and not the other way around.
Let's start with something simple. The numbers in your example (latitude = 48.791695, longitude = 8.413367) are very far from the origin (about 37[km], according to https://www.geodatasource.com/distance-calculator... Is that a valid carla location, for your problem?
Hey Gabi, The values received from GNSS in carla are around this in Town 3. latitude = 48.998151 longtitude = 8.000191 atlitude = 1.225561
I start in a random point in the MAP and these are the data i receive from the GNSS. when I make the conversion it gives me like it's a delta values, i tried to sum it up but i'm not sure if this is correct, in that case the difference between the ground truth and the measurement is big. am I doing something wrong?
I appreciate your help.
Hey Gabi, The values received from GNSS in carla are around this in Town 3. latitude = 48.998151 longtitude = 8.000191 atlitude = 1.225561
I start in a random point in the MAP and these are the data i receive from the GNSS. when I make the conversion it gives me like it's a delta values, i tried to sum it up but i'm not sure if this is correct, in that case the difference between the ground truth and the measurement is big. am I doing something wrong?
I appreciate your help so much really.
Hi,
A close-enough (depends on the accuracy you are looking for and OpenDrive's geodetic computations) approximation would be to calculate latitude/longitude differences, w.r.t to the map geographic origin, translate them to meters using earth curvature radii and remember that Carla's Location (unreal-engine frame) is x=east, y=south, z=up. For your example, it should be: ` map = api.map #map obtained by world.get_map() degreesToRadians = 1/180.0*np.pi geolocation_of_map_origin: GeoLocation = map.transform_to_geolocation(Location(0,0,0)) print("GeoLocation of map origin: {0:s}\n".format(str(geolocation_of_map_origin)))
latitude_of_test_point: float = 48.998151
longitude_of_test_point: float = 8.000191
altitude_of_test_point = 1.225561
# construct carla location
true_GeoLocation: GeoLocation = GeoLocation(latitude_of_test_point, longitude_of_test_point, altitude_of_test_point)
print("True GeoLocation of test point: {0:s}\n".format(str(true_GeoLocation)))
# convert to location using earth-curvature radii
north_curvature_radius, east_curvature_radius = get_earth_curvature_radii(lat_rad=degreesToRadians*geolocation_of_map_origin.latitude,
alt_m=geolocation_of_map_origin.altitude)
print("North/South earth curvature radius = {0:f} [m] | East/West earth curvature radius = {1:f} [m] \n".format(north_curvature_radius, east_curvature_radius))
delta_north: float = (latitude_of_test_point - geolocation_of_map_origin.latitude) * degreesToRadians * north_curvature_radius
delta_east: float = (longitude_of_test_point - geolocation_of_map_origin.longitude) * degreesToRadians * east_curvature_radius
delta_alt: float = altitude_of_test_point - geolocation_of_map_origin.altitude
print("delta North = {0:f} | delta East = {1:f} | delta Alt = {2:f}\n".format(delta_north, delta_east, delta_alt))
estimated_unreal_engine_location: Location = Location(delta_east,
-delta_north,
delta_alt)
print('Estimated Carla Location (unreal-engine frame) of test point: {0:s}\n'.format(str(estimated_unreal_engine_location)))
estimated_GeoLocation: GeoLocation = map.transform_to_geolocation(estimated_unreal_engine_location)
print("GeoLocation of Estimated Carla Location (for verification) : {0:s}\n".format(str(estimated_GeoLocation)))
`
The output should be:
GeoLocation of map origin: GeoLocation(latitude=49.000000, longitude=8.000000, altitude=0.000000) True GeoLocation of test point: GeoLocation(latitude=48.998151, longitude=8.000191, altitude=1.225561) North/South earth curvature radius = 6371848.628169 [m] | East/West earth curvature radius = 4192434.938469 [m] delta North = -205.626806 | delta East = 13.975813 | delta Alt = 1.225561 Estimated Carla Location (unreal-engine frame) of test point: Location(x=13.975813, y=205.626801, z=1.225561) GeoLocation of Estimated Carla Location (for verification) : GeoLocation(latitude=48.998153, longitude=8.000191, altitude=1.225561)
Thank you Gabi, is it possible to tell me where can I find this function get_earth_curvature_radii because i searched but i couldn't find something similar.
No problem.
You can find it in http://clynchg3c.com/Technote/geodesy/radiigeo.pdf. North/South radius of curvature would be R_M East/West radius of curvature would be R_N * cos(lat)
Good luck!
These are the values that i calculated for the radius from the calculations, it's not the same as the one you calculated.
6377740.99149 4184676.96212
a = 6378000.1370 e = 0.0167
d2r = 1/180.0np.pi s = (math.sin(49.0d2r)) (math.sin(49.0d2r)) e2 = (e)**2
rm = (a(1-e2))/math.pow(1-((e2)(s)),1.5) rn = a/math.sqrt(1-((e2)*(s)))
Rn = rn(math.cos(49d2r))
why you send the altitude to the calculation ? am I missing something ?
Hi,
Your semi-major (equatorial) Earth radius, a, looks a off... WGS84 value is 6378137 meters (see https://en.m.wikipedia.org/wiki/World_Geodetic_System#:~:text=The%20WGS%2084%20datum%20surface,%3D%201%2F298.257223563). .... Check e, as well...
Good luck, Gabi
Thank you I updated and it's the same right now.
Thanks @GabiShahmayster for driving this conversation!
I have a question, the way used here was the approximated one but what if i wanna use the other way Then first i have to convert from lon lat to xyz ECEF then convert from xyz ECEF to xyz ENU.. then what shall i do to get the distance travelled in x and y from east north because it doesn't give me a correct solution ?
@Gilaine Hi,
Hey ,
Does these steps are the same in simulation and in the real world with real gps or shall we do something else ?
@Gilaine Hi. I don't really understand the question... Any difference between "simulation steps" and "real-world steps" can only be due to the simulator's erroneous representation/modeling of the real-world, otherwise they should be the same...
What I mean the Xeast, Ynorth are the same as the delta x and delta y of the vehicle, I wanna make sure that if i moved 10 meter in x direction with the vehicle that would be equal to xeast, is this correct because that doesn't happen?
What is Xeast? Could you please provide more information or a numerical example...
What i mean is the following, we agreed that we transform from lat, lon,alt to xECEF, yECEF, zECEF, then we calculate the delta from the origin, then transform to the enu using the transformation matrix what i get right nw is delta xEast, delta yNorth, delta Zup in ENU coordinates
Let's assume im moving with the vehicle 10 m forward, then i shall see that the delta yNorth=10 and delta xeast =0.0 assuming x moving right y moving forward and z moving up but what i get is not accurate ... that's why i asked shall we do something extra or am i doing something wrong?
@Gilaine Sounds right. Just to make sure, what is the initial orientation of the vehicle (actor.get_transform().Rotation)? For yaw = 0[deg], your vehicle would be heading east Gabi
@Gilaine I'm sorry, it's very hard to understand your question without looking at data
@Gilaine
I'm sorry, this is a little beyond the scope of carla functionality...
good luck, Gabi
Thanks alot
@Gilaine
Thanks a lot for following up on this issue. Can you please share the latest working code for GNSS to XYZ coordinates in Carla? It will be really helpful for our research.
@hitesh11 Hi,
i.e for any geographic location p_geo:=(lat, lon, alt), the transformation to Carla frame is: P_carla = R_enu_to_carla R_ecef_to_enu (P_ecef - O_ecef) where p_ecef is p_geo transformed to ECEF Good luck, Gabi
p.s you can use pyproj to perform geographic<->ecef conversions, for example, a geo -> ecef conversion would look like this: from pyproj import Proj ecef = Proj(proj='geocent', ellps='WGS84', datum='WGS84') lla = Proj(proj='latlong', ellps='WGS84', datum='WGS84') X, Y, Z = transform(p1=lla, p2=ecef, x=geo_longitude_in_degrees, y=geo_latitude_in_degrees, z=geo_altitude_in_meters, radians=False)
Thanks a lot @GabiShahmayster !! Really appreciate it.
p_ecef Hi @GabiShahmayster thank you for explaining it so briefly. I have a question. How can i calculate
R_enu_to_carla
and is the following function valid for calculating ECEF -> ENU rotation matrix?
def rot_matrix_ecef2enu(lam, phi):
"""
Define the rotation matrix to go from ECEF coordinates to ENU.
This doesn't seem to be in the package pyproj, so we'll define it here.
Typically, you won't need to call this function.
Parameters
----------
lam : numeric
The longitude of the center of the ENU system.
phi : numeric
The longitude of the center of the ENU system.
Returns
-------
numpy array
A 3x3 numpy array defining the rotation matrix.
"""
# Make the matrix below a little easier to type by defining a few variables:
sLam = np.sin(lam)
cLam = np.cos(lam)
sPhi = np.sin(phi)
cPhi = np.cos(phi)
rotMatrix = np.array([[-sLam, cLam, 0],
[-sPhi*cLam, -sPhi*sLam, cPhi],
[cPhi*cLam, cPhi*sLam, sPhi]])
return rotMatrix
and one thing more while performing this R_ecef_to_enu * (P_ecef-O_ecef)
by P_ecef_O is a tuple of 3 points righ ? And the order would be x,y,z ? I'm sorry if my questions sound kinda stupid because i haven't dealt with coordinate transformation before
Hi @ImtiazUlHassan,
Don't worry, if you haven't dealt with coordinates transformations, they can definitely be a little tricky.
Basically, we are dealing with 3D vectors and their representations w.r.t various coordinate systems. The order is indeed usually [x,y,z], however please note that these are linear algebra operations, performed on vectors, so these should not be tuples but mathematical objects, such as Numpy 3x1 arrays (column vectors). Same goes for the rotation matrices, such as R_ecef_to_enu, which are 3x3 arrays (matrices).
The rotation matrix R_enu_to_carla can be defined simply by observing that carla's axes (Unreal Engine's axes) are +x_carla=east, +y_carla=south, +z_carla=up, so if you start with ENU axes (+x_enu=east, +y_enu=north, +z_enu=up), you would need the following transformation: +x_carla = +x_enu = [1 0 0] [x_enu y_enu z_enu]' +y_carla = -y_enu = [0 -1 0] [x_enu y_enu z_enu]' +z_carla = +z_enu = [0 0 1] * [x_enu y_enu z_enu]' which can written in matrix form as: R_enu_to_carla = [1 0 0; 0 -1 0; 0 0 1]
Hope that helps, Gabi
Hi @ImtiazUlHassan,
Don't worry, if you haven't dealt with coordinates transformations, they can definitely be a little tricky.
Basically, we are dealing with 3D vectors and their representations w.r.t various coordinate systems. The order is indeed usually [x,y,z], however please note that these are linear algebra operations, performed on vectors, so these should not be tuples but mathematical objects, such as Numpy 3x1 arrays (column vectors). Same goes for the rotation matrices, such as R_ecef_to_enu, which are 3x3 arrays (matrices).
The rotation matrix R_enu_to_carla can be defined simply by observing that carla's axes (Unreal Engine's axes) are +x_carla=east, +y_carla=south, +z_carla=up, so if you start with ENU axes (+x_enu=east, +y_enu=north, +z_enu=up), you would need the following transformation: +x_carla = +x_enu = [1 0 0] [x_enu y_enu z_enu]' +y_carla = -y_enu = [0 -1 0] [x_enu y_enu z_enu]' +z_carla = +z_enu = [0 0 1] * [x_enu y_enu z_enu]' which can written in matrix form as: R_enu_to_carla = [1 0 0; 0 -1 0; 0 0 1]
Hope that helps, Gabi
Hi @GabiShahmayster Yeah that helps alot and thank you so much. My issue got resolved. I'm writing the following for the community.
a = 6378137
b = 6356752.3142
f = (a - b) / a
e_sq = f * (2-f)
def geodetic_to_ecef(lat, lon, h):
# (lat, lon) in WSG-84 degrees
# h in meters
lamb = math.radians(lat)
phi = math.radians(lon)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x = (h + N) * cos_lambda * cos_phi
y = (h + N) * cos_lambda * sin_phi
z = (h + (1 - e_sq) * N) * sin_lambda
return x, y, z
def ecef_to_enu(x, y, z, lat0, lon0, h0):
lamb = math.radians(lat0)
phi = math.radians(lon0)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x0 = (h0 + N) * cos_lambda * cos_phi
y0 = (h0 + N) * cos_lambda * sin_phi
z0 = (h0 + (1 - e_sq) * N) * sin_lambda
xd = x - x0
yd = y - y0
zd = z - z0
xEast = -sin_phi * xd + cos_phi * yd
yNorth = -cos_phi * sin_lambda * xd - sin_lambda * sin_phi * yd + cos_lambda * zd
zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd
return xEast, yNorth, zUp
def geodetic_to_enu(lat, lon, h, lat_ref, lon_ref, h_ref):
x, y, z = geodetic_to_ecef(lat, lon, h)
return ecef_to_enu(x, y, z, lat_ref, lon_ref, h_ref)
point_latitude=48.99950150520519
point_longitude=8.002271254544008
point_altitude=1.9948198795318604
origin_latitude=49.000000
origin_longitude=8.00000
origin_altitude=0.0000
x,y,z=geodetic_to_ecef(point_latitude,point_longitude,point_altitude)
carla_x,carla_y,carla_z=ecef_to_enu(x,y,z,origin_latitude,origin_longitude,origin_altitude)
print ("The converted values from Geodetic to carla are",carla_x,-carla_y,carla_z)
i don't own any credits to this script. the original script can be found Here
import pymap3d as pm carla_x, carla_y, carla_z = pm.geodetic2enu(lat, lon, h, origin_latitude, origin_longitude, origin_altitude, ell=pm.utils.Ellipsoid('wgs84')) print ("The converted values from Geodetic to carla are", carla_x, -carla_y, carla_z)
Hey guys @ImtiazUlHassan @kamiladamczyk Also I have one slight question about this part :
point_latitude=48.99950150520519
point_longitude=8.002271254544008
point_altitude=1.9948198795318604
origin_latitude=49.000000
origin_longitude=8.00000
origin_altitude=0.0000
Let's say I'm using a gnss sensor on my car in order to measure its position.
The lat, lon and h I received from it are the ones I will pass to the function geodetic_to_ecef(lat, lon, h)
, which and then transformed into x,y and z coordinates.
But in ecef_to_enu(x, y, z, lat0, lon0, h0)
why did you choose the following values ?
--> origin_latitude=49.000000
-->origin_longitude=8.00000
-->origin_altitude=0.0000
How do know that, this, is the origin from where the carla ground truth values are computed in the terrestrial frame of reference???
Hey @LittlePower97, the georeferences are part of the xodrs, and up until a couple releases, they were all [49,8,0] (when we changed the references to all zeros). In any case, you can always get the opendrive content with map.to_opendrive()
and the georeference, using the following code as template:
xml_tree = ET.parse(opendrive_str)
for geo_elem in xml_tree.find('header').find('geoReference').text.split(' '):
if geo_elem.startswith('+lat_0'):
lat_ref = float(geo_elem.split('=')[-1])
elif geo_elem.startswith('+lon_0'):
lon_ref = float(geo_elem.split('=')[-1])
Hello @glopezdiest , Thank you for your answer ! I was actually looking into the Town01.xodr file when I saw your message !
So :
--> origin_latitude= +lat_0
(=0)
-->origin_longitude= +lon_0
(=0)
But what about the --> origin_altitude
What does the k represent here, is it the altitude ?
<geoReference><![CDATA[+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs ]]></geoReference>
Thanks in advance :)
To be honest, no idea, I've always worked with origin altitude equal 0 and I've never had any problem. Maybe as the altitude and the z are the same (but shifted by the origin altitude), it doesn't create any problems
Hello all,
I did the conversion in both ways, but I always have a bias in X and Y which is not constant and it's dependent on the position in the map, is there any way to get rid of this bias ?
Hi @GabiShahmayster thanks for the explanatory reply above. Are you aware of how i can obtain the relative position of a target vehicle in CARLA. Iam using openSCENARIO to create scenarios. Thanks in advance.
Hi @werewolfdev,
It sounds like what you are looking for is simply a way of calculating the relative transformation between the ego-vehicle actor and the target vehicle actor. This can be done by retrieving the individual pose of each of the actors, say T_ego and T_targ. Each pose is a 44 homogeneous transformation matrix, which describes the translation and rotation of the actor, w.r.t Unreal world frame. The relative pose of the target w.r.t ego-vehicles is given by T_rel = inv(T_ego) T_targ. The relative position is T_rel[:3, 3] The relative rotation (from target axes to ego-vehicle axes) is T_rel[:3, :3].
Hope that helps, Gabi
The mathematical method described here seems to have some problem that it has quite a large error (I save on some routes > 2 meter) as mentioned by Gilaine. The most accurate (and simplest) method to convert a CARLA GPS to the standard CARLA coordinates is the one from the CARLA leaderboard starter kit where they simply multiply the GPS by some numbers. I don't know how these numbers were calculated but on all the routes that I tested it with it had less than 1 centimeter conversion error.
def convert_gps_to_carla(gps):
"""
Converts GPS signal into the CARLA coordinate frame
:param gps: gps from gnss sensor
:return: gps as numpy array in CARLA coordinates
"""
gps = (gps - np.array([0.0, 0.0])) * np.array([111324.60662786, 111319.490945])
gps = np.array([gps[1], -gps[0]])
return gps
The mathematical method described here seems to have some problem that it has quite a large error (I save on some routes > 2 meter) as mentioned by Gilaine. The most accurate (and simplest) method to convert a CARLA GPS to the standard CARLA coordinates is the one from the CARLA leaderboard starter kit where they simply multiply the GPS by some numbers. I don't know how these numbers were calculated but on all the routes that I tested it with it had less than 1 centimeter conversion error.
def convert_gps_to_carla(gps): """ Converts GPS signal into the CARLA coordinate frame :param gps: gps from gnss sensor :return: gps as numpy array in CARLA coordinates """ gps = (gps - np.array([0.0, 0.0])) * np.array([111324.60662786, 111319.490945]) gps = np.array([gps[1], -gps[0]]) return gps
Hi @Kait0 , I noticed in Transfuser gps conversion code that there is no gps = np.array([gps[1], -gps[0]]). Just wondering if the above code is for CARLA(left handed coordinate), what kind of coordinate system is the Transfuser gps conversion code converted to? As left handed coordinate to right handed coordinate system should be something different.
The code above converts the numbers to the standard CARLA coordinate system (x front, y right, z up). The GPS values originally use a different one where north is left (x left, y front) which is why I convert it in the code snippet above.
The TransFuser codebase coordinate systems are mostly all over the place.
I would like to know how to convert the data received from GNSS to XYZ coordinates in carla ? is there a possible way to do so ?