feat: support GPS timestamps (#695)

* remove _ecef_from_lla_DEPRECATED

* move PointWithFix to telemetry.GPSPoint

* rename GPS fields

* format with latest ruff

* add epoch time

* add the missing telemetry.py

* format

* fix format again

* refactor

* fix tests

Author: ptpt

mapillary_tools/camm/camm_builder.py

@@ -43,12 +43,12 @@ def _create_edit_list(
             ]
             break
 
-        assert (
-            0 <= points[0].time
-        ), f"expect non-negative point time but got {points[0]}"
-        assert (
-            points[0].time <= points[-1].time
-        ), f"expect points to be sorted but got first point {points[0]} and last point {points[-1]}"
+        assert 0 <= points[0].time, (
+            f"expect non-negative point time but got {points[0]}"
+        )
+        assert points[0].time <= points[-1].time, (
+            f"expect points to be sorted but got first point {points[0]} and last point {points[-1]}"
+        )
 
         if idx == 0:
             if 0 < points[0].time:
@@ -92,9 +92,9 @@ def convert_points_to_raw_samples(
             timedelta = int((points[idx + 1].time - point.time) * timescale)
         else:
             timedelta = 0
-        assert (
-            0 <= timedelta <= builder.UINT32_MAX
-        ), f"expected timedelta {timedelta} between {points[idx]} and {points[idx + 1]} with timescale {timescale} to be <= UINT32_MAX"
+        assert 0 <= timedelta <= builder.UINT32_MAX, (
+            f"expected timedelta {timedelta} between {points[idx]} and {points[idx + 1]} with timescale {timescale} to be <= UINT32_MAX"
+        )
 
         yield sample_parser.RawSample(
             # will update later

mapillary_tools/exiftool_read_video.py

@@ -5,6 +5,7 @@
 import xml.etree.ElementTree as ET
 
 from . import exif_read, exiftool_read, geo
+from .telemetry import GPSFix, GPSPoint
 
 
 MAX_TRACK_ID = 10
@@ -87,7 +88,7 @@ def _aggregate_gps_track(
     alt_tag: T.Optional[str] = None,
     direction_tag: T.Optional[str] = None,
     ground_speed_tag: T.Optional[str] = None,
-) -> T.List[geo.PointWithFix]:
+) -> T.List[GPSPoint]:
     """
     Aggregate all GPS data by the tags.
     It requires lat, lon to be present, and their lengths must match.
@@ -173,15 +174,16 @@ def _aggregate_float_values_same_length(
         if timestamp is None or lon is None or lat is None:
             continue
         track.append(
-            geo.PointWithFix(
+            GPSPoint(
                 time=timestamp,
                 lon=lon,
                 lat=lat,
                 alt=alt,
                 angle=direction,
-                gps_fix=None,
-                gps_precision=None,
-                gps_ground_speed=ground_speed,
+                epoch_time=None,
+                fix=None,
+                precision=None,
+                ground_speed=ground_speed,
             )
         )
 
@@ -230,8 +232,8 @@ def _aggregate_gps_track_by_sample_time(
     ground_speed_tag: T.Optional[str] = None,
     gps_fix_tag: T.Optional[str] = None,
     gps_precision_tag: T.Optional[str] = None,
-) -> T.List[geo.PointWithFix]:
-    track: T.List[geo.PointWithFix] = []
+) -> T.List[GPSPoint]:
+    track: T.List[GPSPoint] = []
 
     expanded_gps_fix_tag = None
     if gps_fix_tag is not None:
@@ -249,7 +251,7 @@ def _aggregate_gps_track_by_sample_time(
             gps_fix_texts = texts_by_tag.get(expanded_gps_fix_tag)
             if gps_fix_texts:
                 try:
-                    gps_fix = geo.GPSFix(int(gps_fix_texts[0]))
+                    gps_fix = GPSFix(int(gps_fix_texts[0]))
                 except ValueError:
                     gps_fix = None
 
@@ -280,7 +282,7 @@ def _aggregate_gps_track_by_sample_time(
             for idx, point in enumerate(points):
                 point.time = sample_time + idx * avg_timedelta
             track.extend(
-                dataclasses.replace(point, gps_fix=gps_fix, gps_precision=gps_precision)
+                dataclasses.replace(point, fix=gps_fix, precision=gps_precision)
                 for point in points
             )
 
@@ -355,7 +357,7 @@ def extract_model(self) -> T.Optional[str]:
         _, model = self._extract_make_and_model()
         return model
 
-    def _extract_gps_track_from_track(self) -> T.List[geo.PointWithFix]:
+    def _extract_gps_track_from_track(self) -> T.List[GPSPoint]:
         for track_id in range(1, MAX_TRACK_ID + 1):
             track_ns = f"Track{track_id}"
             if self._all_tags_exists(
@@ -397,7 +399,7 @@ def _all_tags_exists(self, tags: T.Set[str]) -> bool:
 
     def _extract_gps_track_from_quicktime(
         self, namespace: str = "QuickTime"
-    ) -> T.List[geo.PointWithFix]:
+    ) -> T.List[GPSPoint]:
         if not self._all_tags_exists(
             {
                 expand_tag(f"{namespace}:GPSDateTime"),

mapillary_tools/ffmpeg.py

@@ -202,10 +202,10 @@ def generate_binary_search(self, sorted_frame_indices: T.Sequence[int]) -> str:
             return "0"
 
         if length == 1:
-            return f"eq(n\\,{ sorted_frame_indices[0] })"
+            return f"eq(n\\,{sorted_frame_indices[0]})"
 
         middle = length // 2
-        return f"if(lt(n\\,{ sorted_frame_indices[middle] })\\,{ self.generate_binary_search(sorted_frame_indices[:middle]) }\\,{ self.generate_binary_search(sorted_frame_indices[middle:]) })"
+        return f"if(lt(n\\,{sorted_frame_indices[middle]})\\,{self.generate_binary_search(sorted_frame_indices[:middle])}\\,{self.generate_binary_search(sorted_frame_indices[middle:])})"
 
     def extract_specified_frames(
         self,

mapillary_tools/geo.py

@@ -6,7 +6,6 @@
 import itertools
 import math
 import typing as T
-from enum import Enum, unique
 
 WGS84_a = 6378137.0
 WGS84_a_SQ = WGS84_a**2
@@ -32,45 +31,6 @@ class Point:
     angle: T.Optional[float]
 
 
-@unique
-class GPSFix(Enum):
-    NO_FIX = 0
-    FIX_2D = 2
-    FIX_3D = 3
-
-
-@dataclasses.dataclass
-class PointWithFix(Point):
-    gps_fix: T.Optional[GPSFix]
-    gps_precision: T.Optional[float]
-    gps_ground_speed: T.Optional[float]
-
-
-def _ecef_from_lla_DEPRECATED(
-    lat: float, lon: float, alt: float
-) -> T.Tuple[float, float, float]:
-    """
-    Deprecated because it is slow. Keep here for reference and comparison.
-    Use _ecef_from_lla2 instead.
-
-    Compute ECEF XYZ from latitude, longitude and altitude.
-
-    All using the WGS94 model.
-    Altitude is the distance to the WGS94 ellipsoid.
-    Check results here http://www.oc.nps.edu/oc2902w/coord/llhxyz.htm
-
-    """
-    a2 = WGS84_a**2
-    b2 = WGS84_b**2
-    lat = math.radians(lat)
-    lon = math.radians(lon)
-    L = 1.0 / math.sqrt(a2 * math.cos(lat) ** 2 + b2 * math.sin(lat) ** 2)
-    x = (a2 * L + alt) * math.cos(lat) * math.cos(lon)
-    y = (a2 * L + alt) * math.cos(lat) * math.sin(lon)
-    z = (b2 * L + alt) * math.sin(lat)
-    return x, y, z
-
-
 def _ecef_from_lla2(lat: float, lon: float) -> T.Tuple[float, float, float]:
     """
     Compute ECEF XYZ from latitude, longitude and altitude.
@@ -172,20 +132,6 @@ def pairwise(iterable: T.Iterable[_IT]) -> T.Iterable[T.Tuple[_IT, _IT]]:
     return zip(a, b)
 
 
-def group_every(
-    iterable: T.Iterable[_IT], n: int
-) -> T.Generator[T.Generator[_IT, None, None], None, None]:
-    """
-    Return a generator that divides the iterable into groups by N.
-    """
-
-    if not (0 < n):
-        raise ValueError("expect 0 < n but got {0}".format(n))
-
-    for _, group in itertools.groupby(enumerate(iterable), key=lambda t: t[0] // n):
-        yield (item for _, item in group)
-
-
 def as_unix_time(dt: T.Union[datetime.datetime, int, float]) -> float:
     if isinstance(dt, (int, float)):
         return dt

mapillary_tools/geotag/geotag_videos_from_exiftool_video.py

@@ -8,6 +8,7 @@
 
 from .. import exceptions, exiftool_read, geo, types
 from ..exiftool_read_video import ExifToolReadVideo
+from ..telemetry import GPSPoint
 from . import gpmf_gps_filter, utils as video_utils
 from .geotag_from_generic import GeotagVideosFromGeneric
 
@@ -45,11 +46,11 @@ def geotag_video(element: ET.Element) -> types.VideoMetadataOrError:
             points = geo.extend_deduplicate_points(points)
             assert points, "must have at least one point"
 
-            if all(isinstance(p, geo.PointWithFix) for p in points):
+            if all(isinstance(p, GPSPoint) for p in points):
                 points = T.cast(
                     T.List[geo.Point],
                     gpmf_gps_filter.remove_noisy_points(
-                        T.cast(T.List[geo.PointWithFix], points)
+                        T.cast(T.List[GPSPoint], points)
                     ),
                 )
                 if not points:

mapillary_tools/geotag/geotag_videos_from_video.py

@@ -9,6 +9,7 @@
 from .. import exceptions, geo, types
 from ..camm import camm_parser
 from ..mp4 import simple_mp4_parser as sparser
+from ..telemetry import GPSPoint
 from . import blackvue_parser, gpmf_gps_filter, gpmf_parser, utils as video_utils
 from .geotag_from_generic import GeotagVideosFromGeneric
 
@@ -155,11 +156,11 @@ def geotag_video(
             video_metadata.points = geo.extend_deduplicate_points(video_metadata.points)
             assert video_metadata.points, "must have at least one point"
 
-            if all(isinstance(p, geo.PointWithFix) for p in video_metadata.points):
+            if all(isinstance(p, GPSPoint) for p in video_metadata.points):
                 video_metadata.points = T.cast(
                     T.List[geo.Point],
                     gpmf_gps_filter.remove_noisy_points(
-                        T.cast(T.List[geo.PointWithFix], video_metadata.points)
+                        T.cast(T.List[GPSPoint], video_metadata.points)
                     ),
                 )
                 if not video_metadata.points:

mapillary_tools/geotag/gpmf_gps_filter.py

@@ -2,6 +2,7 @@
 import typing as T
 
 from .. import constants, geo
+from ..telemetry import GPSPoint
 from . import gps_filter
 
 """
@@ -13,8 +14,8 @@
 
 
 def remove_outliers(
-    sequence: T.Sequence[geo.PointWithFix],
-) -> T.Sequence[geo.PointWithFix]:
+    sequence: T.Sequence[GPSPoint],
+) -> T.Sequence[GPSPoint]:
     distances = [
         geo.gps_distance((left.lat, left.lon), (right.lat, right.lon))
         for left, right in geo.pairwise(sequence)
@@ -37,9 +38,7 @@ def remove_outliers(
         "Split to %d sequences with max distance %f", len(sequences), max_distance
     )
 
-    ground_speeds = [
-        p.gps_ground_speed for p in sequence if p.gps_ground_speed is not None
-    ]
+    ground_speeds = [p.ground_speed for p in sequence if p.ground_speed is not None]
     if len(ground_speeds) < 2:
         return sequence
 
@@ -50,20 +49,20 @@ def remove_outliers(
     )
 
     return T.cast(
-        T.List[geo.PointWithFix],
+        T.List[GPSPoint],
         gps_filter.find_majority(merged.values()),
     )
 
 
 def remove_noisy_points(
-    sequence: T.Sequence[geo.PointWithFix],
-) -> T.Sequence[geo.PointWithFix]:
+    sequence: T.Sequence[GPSPoint],
+) -> T.Sequence[GPSPoint]:
     num_points = len(sequence)
     sequence = [
         p
         for p in sequence
         # include points **without** GPS fix
-        if p.gps_fix is None or p.gps_fix.value in constants.GOPRO_GPS_FIXES
+        if p.fix is None or p.fix.value in constants.GOPRO_GPS_FIXES
     ]
     if len(sequence) < num_points:
         LOG.debug(
@@ -77,7 +76,7 @@ def remove_noisy_points(
         p
         for p in sequence
         # include points **without** precision
-        if p.gps_precision is None or p.gps_precision <= constants.GOPRO_MAX_DOP100
+        if p.precision is None or p.precision <= constants.GOPRO_MAX_DOP100
     ]
     if len(sequence) < num_points:
         LOG.debug(