01_parse_imu_vts.py

"""
01_parse_imu_vts.py — Binary parser for Trekion .imu and .vts sensor files

Reverse-engineered binary formats:
────────────────────────────────────────────────────────────────────────────────
  .imu  (TRIMU001)
    Header (64 bytes):
      [0:8]   magic    = b'TRIMU001'
      [8:12]  version  = uint32 LE  (= 3)
      [12:16] reserved = uint32 LE  (= 563)
      [16:20] reserved = uint32 LE
      [20:28] ts_start = uint64 LE  (first record timestamp, nanoseconds)
      [28:36] reserved = uint64 LE
      [36:64] padding / zeros

    Record (80 bytes each):
      [0:8]   timestamp   uint64 LE  nanoseconds (Unix/boot epoch)
      [8:20]  accel_xyz   3 × float32 LE  m/s²  (+Z = up, gravity ≈ −9.81)
      [20:32] gyro_xyz    3 × float32 LE  rad/s
      [32:44] mag_xyz     3 × float32 LE  µT    (micro-Tesla)
      [44:48] temperature float32 LE      °C
      [48:80] reserved / zeros

  .vts  (TRIVTS01)  — Video Timestamp Sync
    Header (32 bytes):
      [0:8]   magic       = b'TRIVTS01'
      [8:12]  version     = uint32 LE  (= 2)
      [12:16] fps_milli   = uint32 LE  (= 30000 → 30.000 fps)
      [16:32] padding / zeros

    Record (24 bytes each):
      [0:4]   frame_idx   uint32 LE  0-based video frame index
      [4:8]   col1        uint32 LE  (unused / internal)
      [8:12]  col2        uint32 LE  (unused / internal)
      [12:16] imu_idx     uint32 LE  nearest IMU record index at capture time
      [16:20] ts_us       uint32 LE  frame capture timestamp in microseconds
      [20:24] reserved    uint32 LE  (= 0)
────────────────────────────────────────────────────────────────────────────────
"""

import numpy as np
from typing import List
import os

# Import the canonical parsers and data structures from the shared library
from parse_imu_vts_lib import (
    IMURecord, 
    VTSRecord, 
    parse_imu, 
    parse_vts, 
    build_sync_index
)


# ── Validation ─────────────────────────────────────────────────────────────────

def validate(imu_records: List[IMURecord], vts_records: List[VTSRecord]):
    """Print sanity-check statistics."""
    accel_z = np.array([r.accel[2] for r in imu_records])
    gyro_x  = np.array([r.gyro[0]  for r in imu_records])
    temps   = np.array([r.temperature for r in imu_records])
    mags    = np.stack([r.mag for r in imu_records])

    print("\n── IMU Statistics ──────────────────────────────────────")
    print(f"  Accel Z  mean={accel_z.mean():.3f}  std={accel_z.std():.3f}  "
          f"[expected ≈-9.81 m/s² (gravity)]")
    print(f"  Gyro  X  mean={gyro_x.mean():.4f}  std={gyro_x.std():.4f}  "
          f"[expected ≈0 rad/s when stationary]")
    print(f"  Temp     mean={temps.mean():.2f}°C  range=[{temps.min():.2f}, {temps.max():.2f}]")
    print(f"  Mag XYZ  mean={mags.mean(axis=0)}  [µT]")

    sync = build_sync_index(imu_records, vts_records)
    errors = np.array([abs(e) for _, _, e in sync])
    print("\n── Sync Statistics ─────────────────────────────────────")
    print(f"  Frame-IMU sync error  mean={errors.mean():.3f}ms  "
          f"max={errors.max():.3f}ms  median={np.median(errors):.3f}ms")
    ts_start = vts_records[0].timestamp_s
    ts_end   = vts_records[-1].timestamp_s
    print(f"  Video time range: [{ts_start:.3f}s, {ts_end:.3f}s]  "
          f"duration={ts_end-ts_start:.3f}s")

    return sync


# ── Main ───────────────────────────────────────────────────────────────────────

if __name__ == '__main__':
    BASE = os.path.dirname(os.path.abspath(__file__))

    imu_path = os.path.join(BASE, 'data', 'recording2.imu')
    vts_path = os.path.join(BASE, 'data', 'recording2.vts')

    imu_records = parse_imu(imu_path)
    vts_records = parse_vts(vts_path)

    sync = validate(imu_records, vts_records)

    print("\n── First 5 sync entries ─────────────────────────────────")
    for vr, imu_idx, err in sync[:5]:
        ir = imu_records[imu_idx]
        print(f"  Frame {vr.frame_idx:4d}  ts={vr.timestamp_s:.4f}s  "
              f"→ IMU[{imu_idx}] ts={ir.timestamp_s:.4f}s  Δ={err:+.2f}ms  "
              f"accel={ir.accel}  temp={ir.temperature:.1f}°C")

    print("\n[01_parse_imu_vts.py] Done.")