Read the time of Ricoh Theta GPS tracks from the GPS fix in their maker note

mapillary_tools/camm/camm_parser.py

@@ -17,7 +17,7 @@
 import construct as C
 from typing_extensions import TypeIs
 
-from .. import geo, telemetry
+from .. import exif_read, geo, telemetry
 from ..mp4.mp4_sample_parser import MovieBoxParser, Sample, TrackBoxParser
 
 
@@ -48,6 +48,15 @@ class CAMMType(Enum):
 TTelemetry = T.TypeVar("TTelemetry", bound=TelemetryMeasurement)
 
 
+@dataclasses.dataclass(frozen=True)
+class RMKNGPSFix:
+    """The GPS fix in the RMKN box, the EXIF block that Ricoh Theta cameras write into udta"""
+
+    unix_time: float
+    lat: float
+    lon: float
+
+
 @dataclasses.dataclass
 class CAMMInfo:
     # None indicates the data has been extracted,
@@ -59,16 +68,22 @@ class CAMMInfo:
     magn: list[telemetry.MagnetometerData] | None = None
     make: str = ""
     model: str = ""
+    # The GPS fix in the RMKN box of Ricoh Theta videos, or None if there is
+    # none. The type 5 samples those cameras write have no timestamps of their
+    # own, and this fix is the first of them
+    rmkn_gps_fix: RMKNGPSFix | None = None
 
 
 def extract_camm_info(fp: T.BinaryIO, telemetry_only: bool = False) -> CAMMInfo | None:
     moov = MovieBoxParser.parse_stream(fp)
 
     make, model = "", ""
+    rmkn_gps_fix: RMKNGPSFix | None = None
     if not telemetry_only:
         udta_boxdata = moov.extract_udta_boxdata()
         if udta_boxdata is not None:
             make, model = _extract_camera_make_and_model_from_utda_boxdata(udta_boxdata)
+            rmkn_gps_fix = _extract_rmkn_gps_fix(udta_boxdata)
 
     gps_only_construct = _construct_with_selected_camm_types(
         [CAMMType.MIN_GPS, CAMMType.GPS]
@@ -126,7 +141,13 @@ def extract_camm_info(fp: T.BinaryIO, telemetry_only: bool = False) -> CAMMInfo
 
                 _normalize_gps_epochs(gps, make, moov)
 
-                return CAMMInfo(mini_gps=mini_gps, gps=gps, make=make, model=model)
+                return CAMMInfo(
+                    mini_gps=mini_gps,
+                    gps=gps,
+                    make=make,
+                    model=model,
+                    rmkn_gps_fix=rmkn_gps_fix,
+                )
 
     return None
 
@@ -697,3 +718,38 @@ def _extract_camera_make_and_model_from_utda_boxdata(
         model = model.strip()
 
     return make, model
+
+
+def _extract_rmkn_gps_fix(utda_boxdata: dict) -> RMKNGPSFix | None:
+    """
+    Return the GPS fix in the RMKN box, the EXIF block that Ricoh Theta
+    cameras write into udta, or None if it has none.
+
+    On a Theta X, the fix is the first type 5 sample of the video: it has the
+    same position. A camera without a fix when the recording starts writes no
+    GPS time, and its track begins later in the video.
+    """
+    for box in utda_boxdata:
+        if box.type != b"RMKN":
+            continue
+
+        exif = exif_read.ExifReadFromEXIF(io.BytesIO(box.data))
+
+        # V (void) means that the receiver had no fix, so the time is stale
+        status = exif.tags.get("GPS GPSStatus")
+        if status is not None and str(status.values).strip().upper() == "V":
+            return None
+
+        gps_datetime = exif.extract_gps_datetime()
+        if gps_datetime is None or not exif_read.is_valid_capture_time(gps_datetime):
+            return None
+
+        lonlat = exif.extract_lon_lat()
+        if lonlat is None:
+            return None
+
+        return RMKNGPSFix(
+            unix_time=gps_datetime.timestamp(), lat=lonlat[1], lon=lonlat[0]
+        )
+
+    return None

mapillary_tools/geotag/video_extractors/native.py

@@ -5,6 +5,7 @@
 
 from __future__ import annotations
 
+import logging
 import sys
 import typing as T
 from pathlib import Path
@@ -28,6 +29,13 @@
 from ...mp4 import construct_mp4_parser, simple_mp4_parser
 from .base import BaseVideoExtractor
 
+LOG = logging.getLogger(__name__)
+
+# How far the first type 5 sample can be from the GPS fix in the RMKN box and
+# still be that fix: the EXIF block rounds the position to 0.01 arc seconds,
+# about 0.3 m
+_MAX_RMKN_FIX_DISTANCE_METERS = 1.0
+
 
 class GoProVideoExtractor(BaseVideoExtractor):
     def __init__(self, video_path: Path, filter_noisy_points: bool = True):
@@ -86,12 +94,23 @@ def extract(self) -> types.VideoMetadata:
         if not camm_info.gps and not camm_info.mini_gps:
             raise exceptions.MapillaryGPXEmptyError("Empty GPS data found")
 
+        mini_gps = camm_info.mini_gps or []
+        if not camm_info.gps and camm_info.rmkn_gps_fix is not None:
+            timestamped = self._timestamp_mini_gps(mini_gps, camm_info.rmkn_gps_fix)
+            if timestamped is None:
+                LOG.debug(
+                    "The GPS fix in the RMKN box of %s is not its first GPS sample, so it does not date the track",
+                    self.video_path.name,
+                )
+            else:
+                mini_gps = timestamped
+
         # A track may mix type 6 and type 5 samples, so use both. No camera is
         # known to interleave them, and it would cost the absolute timestamps
         # of sampled frames: interpolating between the two types returns a
         # plain geo.Point (see CAMMGPSPoint.interpolate_with), so sample_video
         # falls back to the container start time.
-        points: list[geo.Point] = [*(camm_info.gps or []), *(camm_info.mini_gps or [])]
+        points: list[geo.Point] = [*(camm_info.gps or []), *mini_gps]
         points.sort(key=lambda p: p.time)
 
         return types.VideoMetadata(
@@ -103,6 +122,58 @@ def extract(self) -> types.VideoMetadata:
             model=camm_info.model,
         )
 
+    @staticmethod
+    def _timestamp_mini_gps(
+        mini_gps: T.Sequence[geo.Point], rmkn_gps_fix: camm_parser.RMKNGPSFix
+    ) -> list[geo.Point] | None:
+        """
+        Stamp type 5 samples, which have no timestamps, by the GPS fix in the
+        RMKN box, which is the first of them: the time of the fix, plus each
+        sample's time in the video after the first sample. None if the first
+        sample is elsewhere than the fix, as then it is not that fix.
+
+        The points are built as the description file decodes timestamped
+        points, so that a video reads the same from either
+
+        >>> fix = camm_parser.RMKNGPSFix(unix_time=1661576963.0, lat=1, lon=2)
+        >>> first = geo.Point(time=0.875, lat=1, lon=2, alt=3, angle=None)
+        >>> second = geo.Point(time=1.875, lat=1.00001, lon=2, alt=None, angle=None)
+        >>> [p.epoch_time for p in CAMMVideoExtractor._timestamp_mini_gps([first, second], fix)]
+        [1661576963.0, 1661576964.0]
+        >>> CAMMVideoExtractor._timestamp_mini_gps([second], fix) is None
+        True
+        """
+        if not mini_gps:
+            return []
+
+        first = min(mini_gps, key=lambda p: p.time)
+        if (
+            geo.gps_distance(
+                (first.lat, first.lon), (rmkn_gps_fix.lat, rmkn_gps_fix.lon)
+            )
+            > _MAX_RMKN_FIX_DISTANCE_METERS
+        ):
+            return None
+
+        return [
+            telemetry.CAMMGPSPoint(
+                time=p.time,
+                lat=p.lat,
+                lon=p.lon,
+                alt=p.alt,
+                angle=p.angle,
+                epoch_time=rmkn_gps_fix.unix_time + (p.time - first.time),
+                gps_fix_type=3 if p.alt is not None else 2,
+                horizontal_accuracy=0.0,
+                vertical_accuracy=0.0,
+                velocity_east=0.0,
+                velocity_north=0.0,
+                velocity_up=0.0,
+                speed_accuracy=0.0,
+            )
+            for p in mini_gps
+        ]
+
 
 class BlackVueVideoExtractor(BaseVideoExtractor):
     @override