Converting GPS co-ordinates to relative trajectories

Question

I wish to convert GPS co-ordinates of an object (say, a car) to a relative trajectory.

So suppose I'm travelling from A to B, at A I need to know the precise x-y-z co-ordinate from my current point (which is A) to, say the next GPS co-ordinate (a little bit away from A) which I can then execute, repeating the process above to reach my destination.

So really, it's finding the relative trajectory of where I should go, using the GPS data given. Here's a linked example:-

Source: https://blog.comma.ai/end-to-end-lateral-planning/

I want to perform this operation on the BDD100K dataset.

Answer 1

So I understand that you have a set of trajectories and you want to convert these trajectories into relative trajectories by making the first point as origin. Let's take an example of following trajectory

trajectory = [[72.90106773376465,19.11997972844786,203],
      [72.90165781974792,19.119878358645423,203],
      [72.90237665176392,19.119685755849503,203],
      [72.90259122848511,19.11964520786387,203],
      [72.902752161026,19.119564111862754,203],
      [72.90287017822266,19.11954383785625,203]]

It appears like this when plotted

I would first convert this trajectory into ECEF or UTM coordinates, since these are meter based coordinates, so it would be easy to normalize and translate them to new origin. Once done I would subtract the coordinates of first point of the trajectory from all the other of the trajectory.

i.e if coordinates of first point is x1,y1,z1 we will do X-x1, Y-y1, Z-z1, where X, Y, Z represent each point coordinate in the trajectory. Thus the first point is set to (0,0,0) and rest ot the points are relative to this now.

Here is the code to do this. I use pyproj for conversions.

from pyproj import Transformer

coords = [[72.90106773376465,19.11997972844786,203],
          [72.90165781974792,19.119878358645423,203],
          [72.90237665176392,19.119685755849503,203],
          [72.90259122848511,19.11964520786387,203],
          [72.902752161026,19.119564111862754,203],
          [72.90287017822266,19.11954383785625,203]]

def lla_to_ecef_pyproj(lat, lon, alt):
    transformer = Transformer.from_crs("epsg:4326", "epsg:4978")
    x, y, z = transformer.transform(lon, lat, alt)
    return x, y, z

def relative_by_first_coord(trajectory):
    first_point_x, first_point_y, first_point_z = trajectory[0]
    translated_trajectory = []
    for point in trajectory:
        new_x = point[0]-first_point_x
        new_y = point[1]-first_point_y
        new_z = point[2]-first_point_z
        translated_trajectory.append([new_x,new_y,new_z])
    return translated_trajectory

if __name__ == '__main__':
    ecef_converted_trajectory = []
    for pt in coords:
        ecef_converted_trajectory.append(lla_to_ecef_pyproj(pt[0], pt[1], pt[2]))
    print("#### ECEF COORDS ####")
    print(ecef_converted_trajectory)
    print("#### RELATIVE COORDS ####")
    relative_trajectory = relative_by_first_coord(ecef_converted_trajectory)
    print(relative_trajectory)

The above code prints following output

#### ECEF COORDS ####
[(1772556.6357742352, 5762167.471603105, 2075968.7476994193), (1772498.372090993, 5762189.239816475, 2075958.1453859464), (1772428.1327407553, 5762218.151895516, 2075938.0009548392), (1772406.9850470952, 5762226.1949371165, 2075933.760016046), (1772391.6645347767, 5762233.983678933, 2075925.2781322717), (1772380.0116577018, 5762238.337023567, 2075923.1576600387)]

#### RELATIVE COORDS ####
[[0.0, 0.0, 0.0], [-58.263683242257684, 21.768213369883597, -10.602313472889364], [-128.50303347990848, 50.68029241077602, -30.746744580101222], [-149.65072714001872, 58.72333401162177, -34.987683373270556], [-164.97123945853673, 66.51207582838833, -43.46956714754924], [-176.62411653343588, 70.86542046256363, -45.590039380593225]]

In case you need relative sub-trajectories based on the current location you are on, you can execute this iteratively for sub-trajectories. For eg. if you have a trajectory with 4 points [A,B,C,D], while at A you need B's location relative to A, and while at B you need C's location relative to B like that, you need to run the above code iteratively for sub trajectories or just for the next point with respect to current point

Answer 2

My approach would be more "brute for and pig ignorance" and is simple enough that (1) you don't need to use libraries, and (2) you should understand what the calculation is doing. The starting point is to use flat earth calculations (because vehicle trajectory distances are very short so using spherical geometry is a waste) and to work with a spherical earth (again, at short ranges, using an ellipsoidal earth is unnecessary.)

Basic information:

• 1 minute of Latitude is 1852M (moving South or North of the equator, lines of Latitude stay the same distance apart)
• 1 minute of Longitude is 1852M * Cos(Latitude) (moving South or North of the equator, lines of Longitude get closer together)

Then if you have two points, (Lat1, Long1, Alt1) and (Lat2, Long2, Alt2) we get the following by simple trigonometry:

DeltaAlt = Alt2-Alt1 // Alt in M or whatever units you want, HAE or AMSL doesn't matter as long as both are the same

DeltaLat = (Lat2-Lat1)*1852M // Latitudes must be converted to minutes of Arc

DeltaLong = (Long2-Long1)*Cos(Lat1)*1852M // Longitudes must be converted to minutes of Arc and make sure units for Lat1 match what the "Cos" function expects

HorizontalDistance = Sqrt(DeltaLat * DeltaLat + DeltaLong * DeltaLong)

These calculations are only valid for "short" distances in earth scale - but I've happily used them for points 10s of km apart, the results are "close" to online haversine calculations. (I recommend the Moveable Type site as a reference for distance / bearing calculations.)