Timestamp issue

data_process/scripts/compute_vel_rmse.py

@@ -2,12 +2,13 @@
 
 import csv
 import math
+from bisect import bisect_left
 from pathlib import Path
-from typing import List, Optional, Tuple
+from typing import List, Tuple
 
 
-VelRec = Tuple[Optional[float], float, float, float]  # (t, vx, vy, vz)
-TIME_CANDIDATES = ("t", "time", "timestamp", "stamp")
+VelRec = Tuple[float, float, float, float]  # (t, vx, vy, vz)
+TIME_CANDIDATES = ("t_abs", "t", "time", "timestamp", "stamp")
 
 # Dataset location (relative to repo root)
 DATASET_DIR = "data/anymalD_grandtour"
@@ -32,7 +33,7 @@
 )
 
 
-def parse_float(value: str) -> Optional[float]:
+def parse_float(value: str) -> float | None:
     try:
         return float(value)
     except (TypeError, ValueError):
@@ -53,35 +54,70 @@ def read_velocity_csv(path: Path) -> List[VelRec]:
                 raise ValueError(f"{path} is missing required column '{req}'")
 
         time_col = next((c for c in TIME_CANDIDATES if c in headers), None)
+        if time_col is None:
+            candidates = ", ".join(TIME_CANDIDATES)
+            raise ValueError(
+                f"{path} is missing a timestamp column (expected one of: {candidates})"
+            )
 
         for row in reader:
+            t = parse_float(row.get(time_col, ""))
             vx = parse_float(row.get("vx", ""))
             vy = parse_float(row.get("vy", ""))
             vz = parse_float(row.get("vz", ""))
-            if vx is None or vy is None or vz is None:
+            if t is None or vx is None or vy is None or vz is None:
                 continue
 
-            t = parse_float(row.get(time_col, "")) if time_col else None
             records.append((t, vx, vy, vz))
 
     if not records:
-        raise ValueError(f"No valid velocity rows found in {path}")
+        raise ValueError(f"No valid timestamped velocity rows found in {path}")
     return records
 
 
 def align_records(gt: List[VelRec], est: List[VelRec]) -> Tuple[List[VelRec], List[VelRec], str]:
-    gt_has_time = all(r[0] is not None for r in gt)
-    est_has_time = all(r[0] is not None for r in est)
-
-    if gt_has_time and est_has_time:
-        gt_map = {round(r[0], 9): r for r in gt if r[0] is not None}
-        est_map = {round(r[0], 9): r for r in est if r[0] is not None}
-        common_t = sorted(set(gt_map.keys()) & set(est_map.keys()))
-        if len(common_t) > 1:
-            return [gt_map[t] for t in common_t], [est_map[t] for t in common_t], "timestamp"
-
-    n = min(len(gt), len(est))
-    return gt[:n], est[:n], "index"
+    if not gt or not est:
+        raise ValueError("GT and EST records must both be non-empty")
+
+    gt_sorted_with_duplicates = sorted(gt, key=lambda r: r[0])
+    gt_sorted: List[VelRec] = []
+    for record in gt_sorted_with_duplicates:
+        if not gt_sorted or record[0] != gt_sorted[-1][0]:
+            gt_sorted.append(record)
+
+    est_sorted = sorted(est, key=lambda r: r[0])
+    gt_times = [r[0] for r in gt_sorted]
+
+    gt_aligned: List[VelRec] = []
+    est_aligned: List[VelRec] = []
+
+    for est_record in est_sorted:
+        t = est_record[0]
+        if t < gt_times[0] or t > gt_times[-1]:
+            continue
+
+        right = bisect_left(gt_times, t)
+        if right < len(gt_sorted) and gt_times[right] == t:
+            gt_record = gt_sorted[right]
+        else:
+            left = right - 1
+            t0, vx0, vy0, vz0 = gt_sorted[left]
+            t1, vx1, vy1, vz1 = gt_sorted[right]
+            alpha = (t - t0) / (t1 - t0)
+            gt_record = (
+                t,
+                vx0 + alpha * (vx1 - vx0),
+                vy0 + alpha * (vy1 - vy0),
+                vz0 + alpha * (vz1 - vz0),
+            )
+
+        gt_aligned.append(gt_record)
+        est_aligned.append(est_record)
+
+    if not gt_aligned:
+        raise ValueError("GT and EST timestamp ranges do not overlap")
+
+    return gt_aligned, est_aligned, "timestamp interpolation (GT -> EST)"
 
 
 def mat3_vec3_mul(R, v):
@@ -160,4 +196,4 @@ def main() -> None:
 
 
 if __name__ == "__main__":
-    main()
+    main()