refactor

mapillary_tools/geotag/gpmf_parser.py

@@ -3,6 +3,8 @@
 import itertools
 import pathlib
 import typing as T
+import datetime
+
 
 import construct as C
 
@@ -137,6 +139,14 @@ class TelemetryData:
     magn: T.List[imu.MagnetometerData]
 
 
+def _gps5_timestamp_to_epoch_time(dtstr: str):
+    # yymmddhhmmss.sss
+    dt = datetime.datetime.strptime(dtstr, "%y%m%d%H%M%S.%f").replace(
+        tzinfo=datetime.timezone.utc
+    )
+    return dt.timestamp()
+
+
 # A GPS5 stream example:
 #     key = b'STRM' type = b'\x00' structure_size =  1 repeat = 400
 #     data = ListContainer:
@@ -197,6 +207,12 @@ def gps5_from_stream(
     else:
         gpsf_value = None
 
+    gpsu = indexed.get(b"GPSU")
+    if gpsu is not None:
+        epoch_time = _gps5_timestamp_to_epoch_time(gpsu[0][0].decode("utf-8"))
+    else:
+        epoch_time = None
+
     gpsp = indexed.get(b"GPSP")
     if gpsp is not None:
         gpsp_value = gpsp[0][0]
@@ -213,14 +229,27 @@ def gps5_from_stream(
             lat=lat,
             lon=lon,
             alt=alt,
-            epoch_time=None,
+            epoch_time=epoch_time,
             fix=gpsf_value,
             precision=gpsp_value,
             ground_speed=ground_speed,
             angle=None,
         )
 
 
+_EPOCH_TIME_IN_2000 = datetime.datetime(
+    2000, 1, 1, tzinfo=datetime.timezone.utc
+).timestamp()
+
+
+def _gps9_timestamp_to_epoch_time(
+    days_since_2000: int, secs_since_midnight: float
+) -> float:
+    epoch_time = _EPOCH_TIME_IN_2000 + days_since_2000 * 24 * 60 * 60
+    epoch_time += secs_since_midnight
+    return epoch_time
+
+
 def gps9_from_stream(
     stream: T.Sequence[KLVDict],
 ) -> T.Generator[GPSPoint, None, None]:
@@ -260,19 +289,21 @@ def gps9_from_stream(
             alt,
             speed_2d,
             _speed_3d,
-            _days_since_2000,
-            _secs_since_midnight,
+            days_since_2000,
+            secs_since_midnight,
             dop,
             gps_fix,
         ) = [v / s for v, s in zip(sample_data, scal_values)]
 
+        epoch_time = _gps9_timestamp_to_epoch_time(days_since_2000, secs_since_midnight)
+
         yield GPSPoint(
             # will figure out the actual timestamp later
             time=0,
             lat=lat,
             lon=lon,
             alt=alt,
-            epoch_time=None,
+            epoch_time=epoch_time,
             fix=GPSFix(gps_fix),
             precision=dop * 100,
             ground_speed=speed_2d,

tests/cli/gpmf_parser.py

@@ -43,19 +43,23 @@ def _convert_points_to_gpx_track_segment(
             {
                 "distance_between": distance,
                 "speed_between": speed,
-                "ground_speed": point.gps_ground_speed,
+                "ground_speed": point.ground_speed,
             }
         )
+        if point.epoch_time is not None:
+            epoch_time = point.epoch_time
+        else:
+            epoch_time = point.time
         gpxp = gpxpy.gpx.GPXTrackPoint(
             point.lat,
             point.lon,
             elevation=point.alt,
-            time=datetime.datetime.utcfromtimestamp(point.time),
-            position_dilution=point.gps_precision,
+            time=datetime.datetime.utcfromtimestamp(epoch_time),
+            position_dilution=point.precision,
             comment=comment,
         )
-        if point.gps_fix is not None:
-            gpxp.type_of_gpx_fix = gps_fix_map.get(point.gps_fix)
+        if point.fix is not None:
+            gpxp.type_of_gpx_fix = gps_fix_map.get(point.fix)
         gpx_segment.points.append(gpxp)
 
     return gpx_segment
@@ -86,10 +90,10 @@ def _convert_geojson(path: pathlib.Path):
         geomtry = {"type": "Point", "coordinates": [p.lon, p.lat]}
         properties = {
             "alt": p.alt,
-            "fix": p.gps_fix.value if p.gps_fix is not None else None,
+            "fix": p.fix.value if p.fix is not None else None,
             "index": idx,
             "name": path.name,
-            "precision": p.gps_precision,
+            "precision": p.precision,
             "time": p.time,
         }
         features.append(